fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 14:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

rng aid: use hysteresis with lower hagl check

Browse Source
This commit is contained in:
ChristophTobler
2017-07-07 17:17:36 +02:00
parent 9da9aa632d
commit 517fe8a71f
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+9 -1
View File
@@ -902,7 +902,15 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
use_range_finder = false;
}
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
// use hysteresis to check for hagl
if (in_range_aid_mode) {
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
} else {
// if we were not using range aid in the previous iteration then use a much lower (1/100) threshold to avoid
// switching to range finder too soon (wait for terrain to update).
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
}
return use_range_finder;