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