Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

EKF allow range aid without horizontal aiding

Browse Source 
- skip range aid velocity check when horizontal aiding isn't active
This commit is contained in:
Daniel Agar
2021-02-22 11:55:56 -05:00
committed by GitHub
parent 29943a2c09
commit 098d5ce5c0
1 changed files with 14 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+14 -13
View File
@@ -1168,24 +1168,25 @@ void Ekf::checkRangeAidSuitability()
{
if (_control_status.flags.in_air
&& _range_sensor.isHealthy()
&& isTerrainEstimateValid()
&& isHorizontalAidingActive()) {
&& isTerrainEstimateValid()) {
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
const bool is_in_range = _is_range_aid_suitable
? (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid)
: (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid * 0.7f);
const float range_hagl = _terrain_vpos - _state.pos(2);
const float range_hagl_max = _is_range_aid_suitable ? _params.max_hagl_for_range_aid : (_params.max_hagl_for_range_aid * 0.7f);
const bool is_in_range = range_hagl < range_hagl_max;
const float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));
const bool is_below_max_speed = _is_range_aid_suitable
? ground_vel < _params.max_vel_for_range_aid
: ground_vel < _params.max_vel_for_range_aid * 0.7f;
const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var));
const bool is_hagl_stable = _is_range_aid_suitable ? (hagl_test_ratio < 1.f) : (hagl_test_ratio < 0.01f);
const bool is_hagl_stable = _is_range_aid_suitable
? ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f)
: ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
if (isHorizontalAidingActive()) {
const float max_vel = _is_range_aid_suitable ? _params.max_vel_for_range_aid : (_params.max_vel_for_range_aid * 0.7f);
const bool is_below_max_speed = !_state.vel.xy().longerThan(max_vel);
_is_range_aid_suitable = is_in_range && is_below_max_speed && is_hagl_stable;
_is_range_aid_suitable = is_in_range && is_hagl_stable && is_below_max_speed;
} else {
_is_range_aid_suitable = is_in_range && is_hagl_stable;
}
} else {
_is_range_aid_suitable = false;