AP_NavEKF2: getLastYawResetAngle returns last reset time

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -771,9 +771,8 @@ bool NavEKF2::getHeightControlLimit(float &height) const
 }
 
 // return the amount of yaw angle change due to the last yaw angle reset in radians
-// returns true if a reset yaw angle has been updated and not queried
-// this function should not have more than one client
-bool NavEKF2::getLastYawResetAngle(float &yawAng)
+// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
+uint32_t NavEKF2::getLastYawResetAngle(float &yawAng)
 {
     if (!core) {
         return false;

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -214,9 +214,8 @@ public:
     bool getHeightControlLimit(float &height) const;
 
     // return the amount of yaw angle change due to the last yaw angle reset in radians
-    // returns true if a reset yaw angle has been updated and not queried
-    // this function should not have more than one client
-    bool getLastYawResetAngle(float &yawAng);
+    // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
+    uint32_t getLastYawResetAngle(float &yawAng);
 
     // allow the enable flag to be set by Replay
     void set_enable(bool enable) { _enable.set(enable); }

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -4015,10 +4015,12 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
             // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
             Vector3f tempEuler;
             stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
-            yawResetAngle = wrap_PI(yaw - tempEuler.z);
-            // set the flag to indicate that an in-flight yaw reset has been performed
-            // this will be cleared when the reset value is retrieved
-            yawResetAngleWaiting = true;
+            // this check ensures we accumulate the resets that occur within a single iteration of the EKF
+            if (imuSampleTime_ms != lastYawReset_ms) {
+                yawResetAngle = 0.0f;
+            }
+            yawResetAngle += wrap_PI(yaw - tempEuler.z);
+            lastYawReset_ms = imuSampleTime_ms;
             // calculate an initial quaternion using the new yaw value
             initQuat.from_euler(roll, pitch, yaw);
         } else {
@@ -4230,7 +4232,7 @@ void NavEKF2_core::InitialiseVariables()
     highYawRate = false;
     yawRateFilt = 0.0f;
     yawResetAngle = 0.0f;
-    yawResetAngleWaiting = false;
+    lastYawReset_ms = 0;
     tiltErrFilt = 1.0f;
     tiltAlignComplete = false;
     yawAlignComplete = false;
@@ -4810,18 +4812,11 @@ void NavEKF2_core::alignMagStateDeclination()
 }
 
 // return the amount of yaw angle change due to the last yaw angle reset in radians
-// returns true if a reset yaw angle has been updated and not queried
-// this function should not have more than one client
-bool NavEKF2_core::getLastYawResetAngle(float &yawAng)
+// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
+uint32_t NavEKF2_core::getLastYawResetAngle(float &yawAng)
 {
-    if (yawResetAngleWaiting) {
-        yawAng = yawResetAngle;
-        yawResetAngleWaiting = false;
-        return true;
-    } else {
-        yawAng = yawResetAngle;
-        return false;
-    }
+    yawAng = yawResetAngle;
+    return lastYawReset_ms;
 }
 
 // Fuse compass measurements usinga simple declination observation model that doesn't use magnetic field states

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -207,9 +207,8 @@ public:
     bool getHeightControlLimit(float &height) const;
 
     // return the amount of yaw angle change due to the last yaw angle reset in radians
-    // returns true if a reset yaw angle has been updated and not queried
-    // this function should not have more than one client
-    bool getLastYawResetAngle(float &yawAng);
+    // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
+    uint32_t getLastYawResetAngle(float &yawAng);
 
 private:
     // Reference to the global EKF frontend for parameters
@@ -668,7 +667,7 @@ private:
     float yawRateFilt;              // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
     bool useGpsVertVel;             // true if GPS vertical velocity should be used
     float yawResetAngle;            // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
-    bool yawResetAngleWaiting;      // true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function
+    uint32_t lastYawReset_ms;       // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
     Vector3f tiltErrVec;            // Vector of most recent attitude error correction from Vel,Pos fusion
     float tiltErrFilt;              // Filtered tilt error metric
     bool tiltAlignComplete;         // true when tilt alignment is complete