From 844ed9571883b1727fcdb2e9198c29046313bc08 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Thu, 29 Oct 2015 10:28:11 +1100
Subject: [PATCH] AP_NavEKF2: Add method to rotate output quaternion history

---
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 11 +++++++++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.h   |  3 +++
 2 files changed, 14 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 616b3b8c40..da10fb69c3 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -1223,6 +1223,17 @@ void NavEKF2_core::StoreQuatReset()
     outputDataDelayed.quat = outputDataNew.quat;
 }
 
+// Rotate the stored output quaternion history through a quaternion rotation
+void NavEKF2_core::StoreQuatRotate(Quaternion deltaQuat)
+{
+    outputDataNew.quat = outputDataNew.quat*deltaQuat;
+    // write current measurement to entire table
+    for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
+        storedOutput[i].quat = storedOutput[i].quat*deltaQuat;
+    }
+    outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;
+}
+
 // recall output data from the FIFO
 void NavEKF2_core::RecallOutput()
 {
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 1ba4aafa94..eff1b1c340 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -428,6 +428,9 @@ private:
     // Reset the stored output quaternion history to current EKF state
     void StoreQuatReset(void);
 
+    // Rotate the stored output quaternion history through a quaternion rotation
+    void StoreQuatRotate(Quaternion deltaQuat);
+
     // recall output data from the FIFO
     void RecallOutput();