From 3e4c38f6e89a9c513fa5ab2d5e80377273248e24 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 24 Nov 2020 19:18:36 +1100 Subject: [PATCH] ekf2: Fix bug causing excessive lane selection switching --- src/modules/ekf2/EKF2Selector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index e501407fc2..d1991cba25 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -281,7 +281,7 @@ bool EKF2Selector::UpdateErrorScores() const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio; // reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes - const float threshold = _gyro_fault_detected ? fmaxf(_param_ekf2_sel_err_red.get(), 0.05f) : 0.0f; + const float threshold = _gyro_fault_detected ? 0.0f : fmaxf(_param_ekf2_sel_err_red.get(), 0.05f); if (error_delta > 0 || error_delta < -threshold) { _instance[i].relative_test_ratio += error_delta;