From 8744b0954f22decf489201fd875130c3d4a1eba0 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Sun, 18 Jul 2021 12:44:49 +1000
Subject: [PATCH] AP_NavEKF3: Fix bug preventing copter wind estimation at low
 speed

Also re-tunes process noise default for smoother wind velocity state estimates.
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp         | 2 +-
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index d3a05338d9..bbece351a2 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -39,7 +39,7 @@
 #define FLOW_I_GATE_DEFAULT     300
 #define CHECK_SCALER_DEFAULT    100
 #define FLOW_USE_DEFAULT        1
-#define WIND_P_NSE_DEFAULT      1.0
+#define WIND_P_NSE_DEFAULT      0.5
 
 #elif APM_BUILD_TYPE(APM_BUILD_Rover)
 // rover defaults
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 91375edc54..5f8228b1af 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -66,7 +66,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
         inhibitWindStates = true;
         updateStateIndexLim();
     } else if (inhibitWindStates && canEstimateWind &&
-               sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f)) {
+               (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) {
         inhibitWindStates = false;
         updateStateIndexLim();
         // set states and variances