From 810f0fb95b5a219e2e3de20f85e54ce634d92c16 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 4 Jul 2022 19:03:29 +1000 Subject: [PATCH] AP_Airspeed: Add hysteresis to consistency check --- libraries/AP_Airspeed/AP_Airspeed_Health.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp index 464e752f3b..4ebaedd339 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp @@ -56,7 +56,12 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i) } bool data_is_inconsistent; if ((AP_Airspeed::OptionsMask::USE_EKF_CONSISTENCY & _options) != 0) { - data_is_inconsistent = state[i].failures.test_ratio > MAX(_wind_gate, 0.0f); + float gate_size = MAX(_wind_gate, 0.0f); + if (param[i].use == 0) { + // require a smaller inconsistency for a disabled sensor to be declared consistent + gate_size *= 0.7f; + } + data_is_inconsistent = state[i].failures.test_ratio > gate_size; } else { data_is_inconsistent = false; }