From e3a96094ca3eac7169e758a90cb72032105111a9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 2 Nov 2013 20:25:44 +1100 Subject: [PATCH] AP_Airspeed : Fixes bug that caused airspeed calibration to be sent a zero airspeed This bug resulted in the airspeed ratio going to the maximum value of 4 and staying there. This could lead to a very slow flying model and a stall. --- libraries/AP_Airspeed/AP_Airspeed.cpp | 1 + libraries/AP_Airspeed/AP_Airspeed.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 31f1b6cf43..741b56a9fb 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -170,6 +170,7 @@ void AP_Airspeed::read(void) } airspeed_pressure = get_pressure(); airspeed_pressure = max(airspeed_pressure - _offset, 0); + _last_pressure = airspeed_pressure; _raw_airspeed = sqrtf(airspeed_pressure * _ratio); _airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed; } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 06aeb1ea9e..814fed118a 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -106,7 +106,7 @@ public: // return the differential pressure in Pascal for the last // airspeed reading. Used by the calibration code float get_differential_pressure(void) const { - return max(_last_pressure - _offset, 0); + return max(_last_pressure, 0); } // set the apparent to true airspeed ratio