AP_NavEKF3: add EKFGSF_getYaw to reduce duplicate code

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -690,14 +690,10 @@ void NavEKF3_core::runYawEstimatorCorrection()
             float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
             yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
 
-            // after velocity data has been fused the yaw variance esitmate will have been refreshed and
+            // after velocity data has been fused the yaw variance estimate will have been refreshed and
             // is used maintain a history of validity
-            float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
-            bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
-                                is_positive(yawVarianceEKFGSF) &&
-                                yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
-                                (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
-            if (canUseEKFGSF) {
+            float gsfYaw, gsfYawVariance;
+            if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
                 if (EKFGSF_yaw_valid_count <  GSF_YAW_VALID_HISTORY_THRESHOLD) {
                     EKFGSF_yaw_valid_count++;
                 }

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -231,15 +231,10 @@ void NavEKF3_core::SelectMagFusion()
         }
 
         if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
-            float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
-            bool canUseEKFGSF = yawEstimator != nullptr &&
-                                yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
-                                is_positive(yawVarianceEKFGSF) &&
-                                yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
-                                (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
             // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement
             // for non fixed wing platform types
-            const bool didUseEKFGSF = yawAlignComplete && canUseEKFGSF && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
+            float gsfYaw, gsfYawVariance;
+            const bool didUseEKFGSF = yawAlignComplete && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
 
             // fallback methods
             if (!didUseEKFGSF) {
@@ -853,17 +848,7 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
 
     float gsfYaw, gsfYawVariance;
     if (method == yawFusionMethod::GSF) {
-        bool canUseGsfYaw;
-        if (yawEstimator != nullptr) {
-            float velInnovLength;
-            canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
-                        is_positive(gsfYawVariance) &&
-                        gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
-                        (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
-        } else {
-            canUseGsfYaw = false;
-        }
-        if (!canUseGsfYaw) {
+        if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
             return false;
         }
     }
@@ -1523,6 +1508,25 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
 
 }
 
+// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
+bool NavEKF3_core::EKFGSF_getYaw(float& yaw, float& yawVariance)
+{
+    // return immediately if no yaw estimator
+    if (yawEstimator == nullptr) {
+        return false;
+    }
+
+    float velInnovLength;
+    if (yawEstimator->getYawData(yaw, yawVariance) &&
+        is_positive(yawVariance) &&
+        yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
+        (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
+        return true;
+    }
+
+    return false;
+}
+
 void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order)
 {
     Quaternion quatBeforeReset = stateStruct.quat;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -956,6 +956,9 @@ private:
     // returns false if unsuccessful
     bool EKFGSF_resetMainFilterYaw();
 
+    // returns true on success and populates yaw (in radians) and yawVariance (rad^2)
+    bool EKFGSF_getYaw(float &yaw, float &yawVariance);
+
     // Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
     void FuseDragForces();
     void SelectDragFusion();