AP_NavEKF: Publish amount of in-flight yaw angle reset

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -4385,6 +4385,14 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
         // calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy
         // otherwise use existing heading
         if (!badMag) {
+            // 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;
+            state.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;
+            // calculate an initial quaternion using the new yaw value
             initQuat.from_euler(roll, pitch, yaw);
         } else {
             initQuat = state.quat;
@@ -4639,6 +4647,8 @@ void NavEKF::InitialiseVariables()
     gpsAidingBad = false;
     highYawRate = false;
     yawRateFilt = 0.0f;
+    yawResetAngle = 0.0f;
+    yawResetAngleWaiting = false;
 }
 
 // return true if we should use the airspeed sensor
@@ -5192,5 +5202,20 @@ void NavEKF::alignMagStateDeclination()
     state.earth_magfield.y = magLengthNE * sinf(magDecAng);
 }
 
+// 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 NavEKF::getLastYawResetAngle(float &yawAng)
+{
+    if (yawResetAngleWaiting) {
+        yawAng = yawResetAngle;
+        yawResetAngleWaiting = false;
+        return true;
+    } else {
+        yawAng = yawResetAngle;
+        return false;
+    }
+}
+
 
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF/AP_NavEKF.h

@@ -255,6 +255,11 @@ public:
     // returns a zero rotation quaternion if the INS calculation was not performed on that time step.
     Quaternion getDeltaQuaternion(void) 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);
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -668,6 +673,8 @@ private:
     bool highYawRate;               // true when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
     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
 
     // Used by smoothing of state corrections
     Vector10 gpsIncrStateDelta;    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement