From 8a2f1fdb3dd373d5b6f65114fdef2dfe794c6a6e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 11 Jun 2018 08:45:11 +1000 Subject: [PATCH] AP_Airspeed: fixed airspeed filtering issue this fixes an issue where an airspeed sensor that becomes unhealthy can have an undue effect after the sensor becomes healthy again. In a recent flight log the MS4525 airspeed sensor went unhealthy for a few seconds, and at the same time gave a reading of 12m/s. The plane was flying at 24m/s. While the sensor was unhealthy the code correctly switched to the airspeed estimate, which was fine. When the airspeed sensor become healthy again the IIR filter in AP_Airspeed meant that the speed read at 12m/s initially, then came up to 24m/s over a couple of seconds. This caused the VTOL motors to come on for a few seconds. --- libraries/AP_Airspeed/AP_Airspeed.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index fc280074b6..6236ad79b0 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -347,6 +347,7 @@ void AP_Airspeed::read(uint8_t i) if (!enabled(i) || !sensor[i]) { return; } + bool prev_healthy = state[i].healthy; float raw_pressure = get_pressure(i); if (state[i].cal.start_ms != 0) { update_calibration(i, raw_pressure); @@ -358,7 +359,14 @@ void AP_Airspeed::read(uint8_t i) state[i].corrected_pressure = airspeed_pressure; // filter before clamping positive - state[i].filtered_pressure = 0.7f * state[i].filtered_pressure + 0.3f * airspeed_pressure; + if (!prev_healthy) { + // if the previous state was not healthy then we should not + // use an IIR filter, otherwise a bad reading will last for + // some time after the sensor becomees healthy again + state[i].filtered_pressure = airspeed_pressure; + } else { + state[i].filtered_pressure = 0.7f * state[i].filtered_pressure + 0.3f * airspeed_pressure; + } /* we support different pitot tube setups so user can choose if