From b657563b896025eacc50b694258630e0f601be43 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Dec 2022 07:53:13 +1100 Subject: [PATCH] Plane: ensure smoothed airspeed is > 0 prevent possible divide by zero --- ArduPlane/navigation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 0b1f7cf906..1cf0f7d522 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -143,9 +143,9 @@ void Plane::calc_airspeed_errors() // Get the airspeed_estimate, update smoothed airspeed estimate // NOTE: we use the airspeed estimate function not direct sensor // as TECS may be using synthetic airspeed - float airspeed_measured = 0; + float airspeed_measured = 0.1; if (ahrs.airspeed_estimate(airspeed_measured)) { - smoothed_airspeed = smoothed_airspeed * 0.8f + airspeed_measured * 0.2f; + smoothed_airspeed = MAX(0.1, smoothed_airspeed * 0.8f + airspeed_measured * 0.2f); } // low pass filter speed scaler, with 1Hz cutoff, at 10Hz