AP_NavEKF3: Reset to GPS yaw if fusion times out when on ground.

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -569,7 +569,7 @@ bool NavEKF3_core::using_external_yaw(void) const
 #endif
     if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
         yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
-        return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
+        return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
     }
     return false;
 }

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -264,13 +264,18 @@ void NavEKF3_core::SelectMagFusion()
             if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
                 alignYawAngle(yawAngDataDelayed);
                 yaw_source_reset = false;
+                have_fused_gps_yaw = true;
+                lastSynthYawTime_ms = imuSampleTime_ms;
+                last_gps_yaw_fuse_ms = imuSampleTime_ms;
             } else if (tiltAlignComplete && yawAlignComplete) {
-                fuseEulerYaw(yawFusionMethod::GPS);
+                have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS);
+                if (have_fused_gps_yaw) {
+                    last_gps_yaw_fuse_ms = imuSampleTime_ms;
+                }
             }
-            have_fused_gps_yaw = true;
-            last_gps_yaw_fusion_ms = imuSampleTime_ms;
+            last_gps_yaw_ms = imuSampleTime_ms;
         } else if (tiltAlignComplete && !yawAlignComplete) {
-            // External yaw sources can take singificant time to start providing yaw data so
+            // External yaw sources can take significant time to start providing yaw data so
             // wuile waiting, fuse a 'fake' yaw observation at 7Hz to keeop the filter stable
             if(imuSampleTime_ms - lastSynthYawTime_ms > 140) {
                 yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
@@ -284,7 +289,11 @@ void NavEKF3_core::SelectMagFusion()
                 }
                 lastSynthYawTime_ms = imuSampleTime_ms;
             }
+        } else if (tiltAlignComplete && yawAlignComplete && onGround && imuSampleTime_ms - last_gps_yaw_fuse_ms > 10000) {
+            // handle scenario where were were using GPS yaw previously, but the yaw fusion has timed out.
+            yaw_source_reset = true;
         }
+
         if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS) {
             // no fallback
             return;
@@ -305,7 +314,7 @@ void NavEKF3_core::SelectMagFusion()
 
         // we don't have GPS yaw data and are configured for
         // fallback. If we've only just lost GPS yaw
-        if (imuSampleTime_ms - last_gps_yaw_fusion_ms < 10000) {
+        if (imuSampleTime_ms - last_gps_yaw_ms < 10000) {
             // don't fallback to magnetometer fusion for 10s
             return;
         }
@@ -315,7 +324,7 @@ void NavEKF3_core::SelectMagFusion()
             return;
         }
         if (!inFlight) {
-            // don't fall back if not flying
+            // don't fall back if not flying but reset to GPS yaw if it becomes available
             return;
         }
         if (!gps_yaw_mag_fallback_active) {

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1435,7 +1435,8 @@ private:
     */
     bool learnMagBiasFromGPS(void);
 
-    uint32_t last_gps_yaw_fusion_ms;
+    uint32_t last_gps_yaw_ms; // last time the EKF attempted to use the GPS yaw
+    uint32_t last_gps_yaw_fuse_ms; // last time the EKF successfully fused the GPS yaw
     bool gps_yaw_mag_fallback_ok;
     bool gps_yaw_mag_fallback_active;
     uint8_t gps_yaw_fallback_good_counter;