From e8066aa9935342075a78300ca66b4a562db31dcf Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 24 Aug 2021 13:38:57 +1000 Subject: [PATCH] AP_AHRS: constrain airspeed sensor airspeed using WIND_MAX fixes regression from d1d790019c1c99e286a4b948cd7aff97dcfbb3ad --- libraries/AP_AHRS/AP_AHRS.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 263041fa25..e5218bc81e 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -805,6 +805,18 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const bool ret = false; if (airspeed_sensor_enabled()) { airspeed_ret = AP::airspeed()->get_airspeed(); + + if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) { + // constrain the airspeed by the ground speed + // and AHRS_WIND_MAX + const float gnd_speed = AP::gps().ground_speed(); + float true_airspeed = airspeed_ret * get_EAS2TAS(); + true_airspeed = constrain_float(true_airspeed, + gnd_speed - _wind_max, + gnd_speed + _wind_max); + airspeed_ret = true_airspeed / get_EAS2TAS(); + } + return true; }