From 885bfd1b4e8fb5136af3880dadb731081d294cd0 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Thu, 25 Aug 2016 22:10:22 +1000
Subject: [PATCH] AP_NavEKF2: Handle yaw jumps due to core switches

---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 50 +++++++++++++++++++++++++++--
 libraries/AP_NavEKF2/AP_NavEKF2.h   | 10 +++++-
 2 files changed, 56 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index e23704c919..d4dcfaf823 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -1144,14 +1144,58 @@ bool NavEKF2::getHeightControlLimit(float &height) const
     return core[primary].getHeightControlLimit(height);
 }
 
-// return the amount of yaw angle change due to the last yaw angle reset in radians
+// return the amount of yaw angle change (in radians) due to the last yaw angle reset or core selection switch
 // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
-uint32_t NavEKF2::getLastYawResetAngle(float &yawAng) const
+uint32_t NavEKF2::getLastYawResetAngle(float &yawAngDelta)
 {
     if (!core) {
         return 0;
     }
-    return core[primary].getLastYawResetAngle(yawAng);
+
+    // check for an internal ekf yaw reset
+    float temp_yaw_delta;
+    uint32_t ekf_reset_ms = core[primary].getLastYawResetAngle(temp_yaw_delta);
+    if (ekf_reset_ms != last_ekf_reset_ms) {
+        // record the time of the ekf's internal yaw reset event
+        last_ekf_reset_ms = ekf_reset_ms;
+
+        // record the the ekf's internal yaw reset value
+        yaw_reset_delta = temp_yaw_delta;
+
+        // record the yaw reset event time
+        yaw_reset_time_ms = imuSampleTime_us/1000;
+
+    }
+
+    // check for a core switch and if a switch has occurred, set the yaw reset delta
+    // to the difference in yaw angle between the current and last yaw angle
+    Vector3f eulers_primary;
+    core[primary].getEulerAngles(eulers_primary);
+    if (primary != prev_instance) {
+        // the delta is the difference between the current and previous yaw
+        // This overwrites any yaw reset value recorded from an internal eff reset
+        yaw_reset_delta += wrap_PI(eulers_primary.z - prev_yaw);
+
+        // record the time of the yaw reset event
+        yaw_reset_time_ms = imuSampleTime_us/1000;
+
+        // update the time recorded for the last ekf internal yaw reset forthe primary core to
+        // prevent a yaw ekf reset event being published on the next frame due to the change in time
+        last_ekf_reset_ms = ekf_reset_ms;
+
+    }
+
+    // record the yaw angle from the primary core
+    prev_yaw = eulers_primary.z;
+
+    // record the primary core
+    prev_instance = primary;
+
+    // return the yaw delta from the last event
+    yawAngDelta = yaw_reset_delta;
+
+    // return the time of the last event
+    return yaw_reset_time_ms;
 }
 
 // return the amount of NE position change due to the last position reset in metres
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index cbabb27118..5ba4eb8a07 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -270,7 +270,7 @@ public:
 
     // return the amount of yaw angle change due to the last yaw angle reset in radians
     // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
-    uint32_t getLastYawResetAngle(float &yawAng) const;
+    uint32_t getLastYawResetAngle(float &yawAngDelta);
 
     // return the amount of NE position change due to the last position reset in metres
     // returns the time of the last reset or 0 if no reset has ever occurred
@@ -379,4 +379,12 @@ private:
 
     // time at start of current filter update
     uint64_t imuSampleTime_us;
+
+    // used to keep track of yaw angle changes due to change of primary instance
+    uint8_t prev_instance = 0;          // active core number from the previous time step
+    uint32_t last_ekf_reset_ms= 0;      // last time the active ekf performed a yaw reset (msec)
+    uint32_t last_lane_switch_ms = 0;   // last time there was a lane switch (msec)
+    uint32_t yaw_reset_time_ms = 0;     // last time a yaw reset event was published
+    float yaw_reset_delta = 0.0f;       // the amount of yaw change due to the last published yaw reset (rad)
+    float prev_yaw = 0.0f;              // yaw angle published by the active core from the previous time step (rad)
 };