From faa4d2885154ed6a4d54d822614a197195e94d39 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Aug 2023 13:26:18 +1000 Subject: [PATCH] AP_Airspeed: fixed handling of unhealthy airspeed this fixes a bug introduced in https://github.com/ArduPilot/ardupilot/pull/22416 which results in using bad airspeed data on timeout. The prev_health variable is updated by the get_pressure call --- libraries/AP_Airspeed/AP_Airspeed.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 2f8e201e54..341aa7aac9 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -609,6 +609,14 @@ void AP_Airspeed::read(uint8_t i) return; } +#ifndef HAL_BUILD_AP_PERIPH + /* + get the healthy state before we call get_pressure() as + get_pressure() overwrites the healthy state + */ + bool prev_healthy = state[i].healthy; +#endif + float raw_pressure = get_pressure(i); float airspeed_pressure = raw_pressure - get_offset(i); @@ -616,7 +624,6 @@ void AP_Airspeed::read(uint8_t i) state[i].corrected_pressure = airspeed_pressure; #ifndef HAL_BUILD_AP_PERIPH - bool prev_healthy = state[i].healthy; if (state[i].cal.start_ms != 0) { update_calibration(i, raw_pressure); }