AP_NavEKF: Publish the INS delta quaternion

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -677,6 +677,11 @@ bool NavEKF::InitialiseFilterBootstrap(void)
 // Update Filter States - this should be called whenever new IMU data is available
 void NavEKF::UpdateFilter()
 {
+    // zero the delta quaternion used by the strapdown navigation because it is published
+    // and we need to return a zero rotation of the INS fails to update it
+    memset(&correctedDelAngQuat[0], 0, sizeof(correctedDelAngQuat));
+    correctedDelAngQuat[0] = 1.0f;
+
     // don't run filter updates if states have not been initialised
     if (!statesInitialised) {
         return;
@@ -5101,4 +5106,13 @@ bool NavEKF::getHeightControlLimit(float &height) const
     }
 }
 
+// provides the delta quaternion that was used by the INS calculation to rotate from the previous orientation to the orientation at the current time
+// the delta quaternion returned will be a zero rotation if the INS calculation was not performed on that time step
+Quaternion NavEKF::getDeltaQuaternion(void) const
+{
+    // Note: correctedDelAngQuat is reset to a zero rotation at the start of every update cycle in UpdateFilter()
+    return correctedDelAngQuat;
+}
+
+
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF/AP_NavEKF.h

@@ -247,6 +247,10 @@ public:
     // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
     bool getHeightControlLimit(float &height) const;
 
+    // provides the quaternion that was used by the INS calculation to rotate from the previous orientation to the orientaion at the current time step
+    // returns a zero rotation quaternion if the INS calculation was not performed on that time step.
+    Quaternion getDeltaQuaternion(void) const;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private: