From b85c8fa1352f9e65e62dd5d97bb3f798c1f6f4c1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 5 May 2017 07:48:37 +1000 Subject: [PATCH] commander: fix bug in pos vel validity transition This ensures that a mode change will occur immediately when the EKF reports the solution as invalid. --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45b1305667..33714d9176 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3783,7 +3783,7 @@ check_posvel_validity(bool data_valid, float data_accuracy, float required_accur // Check accuracy with hysteresis in both test level and time bool pos_status_changed = false; - if (*valid_state && data_accuracy > required_accuracy * 2.5f) { + if (*valid_state && ((data_accuracy > required_accuracy * 2.5f) || !data_valid)) { pos_inaccurate = true; pos_status_changed = true; *last_fail_time_us = now;