From 398f73144241e11c44dd12c5d41138c754812c85 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
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_NavEKF.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index b4aa642a1b..703f4489d3 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -623,6 +623,18 @@ bool AP_AHRS_NavEKF::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;
     }