From 197d31e9cc839b1660b5bb58e681957ec92df3b4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 23 Apr 2020 18:37:12 +1000
Subject: [PATCH] AP_NavEKF3: fixed in-flight yaw reset

---
 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 19 +-------
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp      | 43 +++++++++++--------
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  3 ++
 3 files changed, 29 insertions(+), 36 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
index 0f5b5721c4..1000bc833c 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@@ -97,23 +97,8 @@ void NavEKF3_core::controlMagYawReset()
         // update rotation matrix from body to NED frame
         stateStruct.quat.inverse().rotation_matrix(prevTnb);
 
-        // read the magnetometer data
-        readMagData();
-
-        // rotate the magnetic field into NED axes
-        Vector3f initMagNED = prevTnb * magDataDelayed.mag;
-
-        // calculate heading of mag field rel to body heading
-        float magHeading = atan2f(initMagNED.y, initMagNED.x);
-
-        // get the magnetic declination
-        float magDecAng = MagDeclination();
-
-        // calculate yaw angle delta
-        float yaw_delta = magDecAng - magHeading;
-
-        // update quaternion states and covariances
-        resetQuatStateYawOnly(yaw_delta, sq(MAX(frontend->_yawNoise, 1.0e-2f)), true);
+        // set yaw from a single mag sample
+        setYawFromMag();
 
         // send initial alignment status to console
         if (!yawAlignComplete) {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 852bd88348..54a2540aea 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -1638,6 +1638,28 @@ void NavEKF3_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
     omega.z  = -earthRate*sinf(lat_rad);
 }
 
+// set yaw from a single magnetometer sample
+void NavEKF3_core::setYawFromMag()
+{
+    // read the magnetometer data
+    readMagData();
+
+    // rotate the magnetic field into NED axes
+    Vector3f euler321;
+    stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
+
+    // set the yaw to zero and calculate the zero yaw rotation from body to earth frame
+    Matrix3f Tbn_zeroYaw;
+    Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
+
+    //Vector3f magMeasNED = Tbn_zeroYaw * magDataDelayed.mag;
+    Vector3f magMeasNED = Tbn_zeroYaw * magDataDelayed.mag;
+    float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
+
+    // update quaternion states and covariances
+    resetQuatStateYawOnly(yawAngMeasured, sq(MAX(frontend->_yawNoise, 1.0e-2f)), false);
+}
+
 // update quaternion, mag field states and associated variances using magnetomer and declination data
 void NavEKF3_core::calcQuatAndFieldStates()
 {
@@ -1648,23 +1670,8 @@ void NavEKF3_core::calcQuatAndFieldStates()
     // update rotation matrix from body to NED frame
     stateStruct.quat.inverse().rotation_matrix(prevTnb);
 
-    // read the magnetometer data
-    readMagData();
-
-    // rotate the magnetic field into NED axes
-    Vector3f initMagNED = prevTnb * magDataDelayed.mag;
-
-    // calculate heading of mag field rel to body heading
-    float magHeading = atan2f(initMagNED.y, initMagNED.x);
-
-    // get the magnetic declination
-    float magDecAng = MagDeclination();
-
-    // calculate yaw angle delta
-    float yaw_delta = magDecAng - magHeading;
-
-    // update quaternion states and covariances
-    resetQuatStateYawOnly(yaw_delta, sq(MAX(frontend->_yawNoise, 1.0e-2f)), true);
+    // set yaw from a single mag sample
+    setYawFromMag();
 
     // Rotate Mag measurements into NED to set initial NED magnetic field states
     // Don't do this if the earth field has already been learned
@@ -1686,12 +1693,10 @@ void NavEKF3_core::calcQuatAndFieldStates()
         P[19][19] = P[18][18];
         P[20][20] = P[18][18];
         P[21][21] = P[18][18];
-
     }
 
     // record the fact we have initialised the magnetic field states
     recordMagReset();
-
 }
 
 // zero the attitude covariances, but preserve the variances
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 70735dce5b..9647a2498e 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -738,6 +738,9 @@ private:
     // update quaternion, mag field states and associated variances using magnetomer and declination data
     void calcQuatAndFieldStates();
 
+    // reset yaw based on magnetic field sample
+    void setYawFromMag();
+
     // zero stored variables
     void InitialiseVariables();