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

commander: do not fail nav check on GPS position glitch

Browse Source 
In case of a diverge of the nav filter, both position and velocity test
ratios are large. If only one of them is too large, the nav filter is
most likely not diverging but is encountering a GNSS position reset and
it is still safe to fly in position mode.
This commit is contained in:
bresch
2021-02-10 13:48:12 +01:00
committed by Daniel Agar
parent b1b032d6e1
commit 5726e8e0a3
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+5 -2
View File
@@ -4016,7 +4016,10 @@ void Commander::estimator_check()
} else {
if (!_nav_test_passed) {
const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f);
// Both test ratios need to pass/fail together to change the nav test status
const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f)
&& (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.0f) && (estimator_status.pos_test_ratio >= 1.0f);
if (innovation_pass) {
_time_last_innov_pass = hrt_absolute_time();
@@ -4032,7 +4035,7 @@ void Commander::estimator_check()
_nav_test_failed = false;
}
} else {
} else if (innovation_fail) {
_time_last_innov_fail = hrt_absolute_time();
if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 1_s) {