From c1b67d295f41a38be05f2585d4408860cc17f42e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 5 Aug 2021 12:24:33 +0900
Subject: [PATCH] AP_NavEKF3: skip GSF reset count check if source actively
 changed

also only fail all compass on emergency reset
---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp   |  2 +-
 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 19 +++++++++++++------
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  3 ++-
 3 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 5f8228b1af..9c416e9b94 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -758,7 +758,7 @@ void NavEKF3_core::runYawEstimatorCorrection()
 
         // action an external reset request
         if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) {
-            EKFGSF_resetMainFilterYaw();
+            EKFGSF_resetMainFilterYaw(true);
         }
     } else {
         EKFGSF_yaw_valid_count = 0;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
index afb34a3b9d..1ad1708180 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@@ -150,7 +150,7 @@ void NavEKF3_core::realignYawGPS()
         // correct yaw angle using GPS ground course if compass yaw bad
         if (badMagYaw) {
             // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
-            if (EKFGSF_resetMainFilterYaw()) {
+            if (EKFGSF_resetMainFilterYaw(true)) {
                 return;
             }
 
@@ -229,7 +229,8 @@ void NavEKF3_core::SelectMagFusion()
 
         // because this type of reset event is not as time critical, require a continuous history of valid estimates
         if ((!yawAlignComplete || yaw_source_reset) && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
-            yawAlignComplete = EKFGSF_resetMainFilterYaw();
+            const bool emergency_reset = (yaw_source != AP_NavEKF_Source::SourceYaw::GSF);
+            yawAlignComplete = EKFGSF_resetMainFilterYaw(emergency_reset);
             yaw_source_reset = false;
         }
 
@@ -1531,15 +1532,19 @@ bool NavEKF3_core::learnMagBiasFromGPS(void)
 }
 
 // Reset states using yaw from EKF-GSF and velocity and position from GPS
-bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
+bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset)
 {
     // Don't do a reset unless permitted by the EK3_GSF_USE and EK3_GSF_RUN parameter masks
     if ((yawEstimator == nullptr)
-        || !(frontend->_gsfUseMask & (1U<<core_index))
-        || EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount) {
+        || !(frontend->_gsfUseMask & (1U<<core_index))) {
         return false;
     };
 
+    // limit the number of emergency resets
+    if (emergency_reset && (EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount)) {
+        return false;
+    }
+
     ftype yawEKFGSF, yawVarianceEKFGSF;
     if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
         // keep roll and pitch and reset yaw
@@ -1560,7 +1565,9 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
         }
 
         // Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
-        allMagSensorsFailed = true;
+        if (emergency_reset) {
+            allMagSensorsFailed = true;
+        }
 
         // record the yaw reset event
         recordYawReset();
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 0a57f2b099..b0c6da6aec 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -951,8 +951,9 @@ private:
     void resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order);
 
     // attempt to reset the yaw to the EKF-GSF value
+    // emergency_reset should be true if this reset is triggered by the loss of the yaw estimate
     // returns false if unsuccessful
-    bool EKFGSF_resetMainFilterYaw();
+    bool EKFGSF_resetMainFilterYaw(bool emergency_reset);
 
     // returns true on success and populates yaw (in radians) and yawVariance (rad^2)
     bool EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const;