From ad4c29748e9d2e194236eddfe0a78d9db1bf9af9 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 10 May 2022 11:31:36 +1000
Subject: [PATCH] AP_NavEKF: getYawData also provides number of clipping models

In the case of the compass calibrator we do not want to use the GSF
result if any model is degenerate.  We've had a compass calibrate in
flight 180-degrees out from what it should have.
---
 libraries/AP_NavEKF/EKFGSF_yaw.cpp | 10 ++++++++--
 libraries/AP_NavEKF/EKFGSF_yaw.h   | 12 +++++++++---
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp
index 8a938c6d0c..e58150600f 100644
--- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp
+++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp
@@ -167,7 +167,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
             if (!state_update_failed) {
                 // Calculate weighting for each model assuming a normal error distribution
                 const ftype min_weight = 1e-5f;
-                uint8_t n_clips = 0;
+                n_clips = 0;
                 for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
                     newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx];
                     if (newWeight[mdl_idx] < min_weight) {
@@ -625,13 +625,19 @@ Matrix3F EKFGSF_yaw::updateRotMat(const Matrix3F &R, const Vector3F &g) const
     return ret;
 }
 
-bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance) const
+// returns true if a yaw estimate is available.  yaw and its variance
+// is returned, as well as the number of models which are *not* being
+// used to snthesise the yaw.
+bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance, uint8_t *_n_clips) const
 {
     if (!vel_fuse_running) {
         return false;
     }
     yaw = GSF.yaw;
     yawVariance = GSF.yaw_variance;
+    if (_n_clips != nullptr) {
+        *_n_clips = n_clips;
+    }
     return true;
 }
 
diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.h b/libraries/AP_NavEKF/EKFGSF_yaw.h
index d022213c11..2487aea1a3 100644
--- a/libraries/AP_NavEKF/EKFGSF_yaw.h
+++ b/libraries/AP_NavEKF/EKFGSF_yaw.h
@@ -31,9 +31,11 @@ public:
     // set the gyro bias in rad/sec
     void setGyroBias(Vector3f &gyroBias);
 
-    // get yaw estimated and corresponding variance
-    // return false if yaw estimation is inactive
-    bool getYawData(ftype &yaw, ftype &yawVariance) const;
+    // get yaw estimated and corresponding variance return false if
+    // yaw estimation is inactive.  n_clips will contain the number of
+    // models which were *not* used to create the yaw and yawVariance
+    // return values.
+    bool getYawData(ftype &yaw, ftype &yawVariance, uint8_t *n_clips=nullptr) const;
 
     // get the length of the weighted average velocity innovation vector
     // return false if not available
@@ -136,4 +138,8 @@ private:
     // Returns the probability for a selected model assuming a Gaussian error distribution
     // Used by the Guassian Sum Filter to calculate the weightings when combining the outputs from the bank of EKF's
     ftype gaussianDensity(const uint8_t mdl_idx) const;
+
+    // number of models whose weights underflowed due to excessive
+    // innovation variances:
+    uint8_t n_clips;
 };