From b92eca7b66966ce0741a124aadea0c63cdec2236 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Sat, 12 Dec 2020 07:22:44 +1100
Subject: [PATCH] AP_NavEKF3: Select startup aiding mode after landing without
 yaw sensor

---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 22 ++++++++++++---------
 1 file changed, 13 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index bb7dcf3d53..f8421c6dc3 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -206,6 +206,19 @@ void NavEKF3_core::setAidingMode()
     // Check that the gyro bias variance has converged
     checkGyroCalStatus();
 
+    // Handle the special case where we are on ground and disarmed without a yaw measurement
+    // and navigating. This can occur if not using a magnetometer and yaw was aligned using GPS
+    // during the previous flight.
+    if (yaw_source_last == AP_NavEKF_Source::SourceYaw::NONE &&
+        !motorsArmed &&
+        onGround &&
+        PV_AidingMode != AID_NONE)
+    {
+        PV_AidingMode = AID_NONE;
+        yawAlignComplete = false;
+        finalInflightYawInit = false;
+    }
+
     // Determine if we should change aiding mode
     switch (PV_AidingMode) {
         case AID_NONE: {
@@ -286,15 +299,6 @@ void NavEKF3_core::setAidingMode()
                                      (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
             }
 
-            // This is a special case for when we are a fixed wing aircraft vehicle that has landed and
-            // has no yaw measurement that works when static. Declare the yaw as unaligned (unknown)
-            // and declare attitude aiding loss so that we fall back into a non-aiding mode
-            if (assume_zero_sideslip() && onGround && !use_compass() && !using_external_yaw()) {
-                yawAlignComplete = false;
-                finalInflightYawInit = false;
-                attAidLossCritical = true;
-            }
-
             if (attAidLossCritical) {
                 // if the loss of attitude data is critical, then put the filter into a constant position mode
                 PV_AidingMode = AID_NONE;