From 5f24603ceb639638fb15f10f97cc2d6a330fe7f7 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 31 Jan 2015 19:26:35 +1100
Subject: [PATCH] AP_NavEKF: Publish small EKF quaternion and gyro bias outputs

---
 libraries/AP_NavEKF/AP_SmallEKF.cpp | 11 +++++++++++
 libraries/AP_NavEKF/AP_SmallEKF.h   |  6 ++++++
 2 files changed, 17 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_SmallEKF.cpp b/libraries/AP_NavEKF/AP_SmallEKF.cpp
index 90abcc2387..57adc40a98 100644
--- a/libraries/AP_NavEKF/AP_SmallEKF.cpp
+++ b/libraries/AP_NavEKF/AP_SmallEKF.cpp
@@ -894,5 +894,16 @@ void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector
     gyroBias = state.delAngBias / dtIMU;
 }
 
+// get gyro bias data
+void SmallEKF::getGyroBias(Vector3f &gyroBias) const
+{
+    gyroBias = state.delAngBias / dtIMU;
+}
+
+// get quaternion data
+void SmallEKF::getQuat(Quaternion &quat) const
+{
+    quat = state.quat;
+}
 
 #endif // HAL_CPU_CLASS
diff --git a/libraries/AP_NavEKF/AP_SmallEKF.h b/libraries/AP_NavEKF/AP_SmallEKF.h
index 5beb8b26c8..73a6e9932c 100644
--- a/libraries/AP_NavEKF/AP_SmallEKF.h
+++ b/libraries/AP_NavEKF/AP_SmallEKF.h
@@ -86,6 +86,12 @@ public:
     // get some debug information
     void getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const;
 
+    // get gyro bias data
+    void getGyroBias(Vector3f &gyroBias) const;
+
+    // get quaternion data
+    void getQuat(Quaternion &quat) const;
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private: