From 5726e8e0a3b4f166ca2f0dd54914f80af6efab37 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 10 Feb 2021 13:48:12 +0100 Subject: [PATCH] commander: do not fail nav check on GPS position glitch 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. --- src/modules/commander/Commander.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d3b504b91b..3313e50bd4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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) {