From 5dc29699ab1da9c6055ff16ebfd6ab89cc4fec33 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 27 Apr 2015 21:42:28 +1000
Subject: [PATCH] AP_NavEKF: Publish the INS delta quaternion

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 14 ++++++++++++++
 libraries/AP_NavEKF/AP_NavEKF.h   |  4 ++++
 2 files changed, 18 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 5b627322dd..aaf4556cfc 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/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
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index e080358326..7dbc350e17 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/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: