From fe85c68344f589a1e8c1cb1e2925984d14f0c6c8 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 24 May 2016 09:46:56 +1000
Subject: [PATCH] AP_NavEKF2: move yaw reset for no compass case

Should not be in a function that performs reset on magnetic field states
---
 libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
index 251caadd38..5a2d5b7ae0 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
@@ -57,13 +57,6 @@ void NavEKF2_core::controlMagYawReset()
         }
     }
 
-    // In-Flight yaw alignment for vehicles that can use a zero sideslip assumption (Planes)
-    // and are not using a compass
-    if (!yawAlignComplete && assume_zero_sideslip() && inFlight) {
-        realignYawGPS();
-        firstMagYawInit = yawAlignComplete;
-    }
-
     // In-Flight reset for vehicles that can use a zero sideslip assumption (Planes)
     // this is done to protect against unrecoverable heading alignment errors due to compass faults
     if (!firstMagYawInit && assume_zero_sideslip() && inFlight) {
@@ -212,9 +205,12 @@ void NavEKF2_core::SelectMagFusion()
             magTestRatio.zero();
             yawTestRatio = 0.0f;
             lastSynthYawTime_ms = imuSampleTime_ms;
-        } else {
-            // Control reset of yaw and magnetic field states
-            controlMagYawReset();
+        }
+        // In-Flight yaw alignment for vehicles that can use a zero sideslip assumption (Planes)
+        // and are not using a compass
+        if (!yawAlignComplete && assume_zero_sideslip() && inFlight) {
+            realignYawGPS();
+            firstMagYawInit = yawAlignComplete;
         }
     }