From 1179c08473528f0307bd920719867dca18ed108f Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 21 May 2016 11:39:33 +1000
Subject: [PATCH] AP_NavEKF2: Handle case where we are in-flight and haven't
 yet aligned the yaw

This happens if planes are flown without a compass
---
 libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
index 61aaad4df1..b386c45b61 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
@@ -57,6 +57,13 @@ 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) {