From ffce1f64cc05477c19a50d370ed5f0626fb7b0f1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 29 Dec 2013 23:20:53 +1100
Subject: [PATCH] AP_NavEKF: added euler angle functions

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 8 +++++++-
 libraries/AP_NavEKF/AP_NavEKF.h   | 2 +-
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 87c51cce58..5468e705d6 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -1868,13 +1868,19 @@ void NavEKF::eul2quat(float quat[4], float eul[3])
     quat[3] = u1*u2*u6-u4*u5*u3;
 }
 
-void NavEKF::quat2eul(float y[3], float u[4])
+void NavEKF::quat2eul(Vector3f &y, float u[4])
 {
     y[0] = atan2f((2*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
     y[1] = -asinf(2*(u[1]*u[3]-u[0]*u[2]));
     y[2] = atan2f((2*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
 }
 
+void NavEKF::getEulerAngles(Vector3f &euler)
+{
+    float q[4] = { states[0], states[1], states[2], states[3] };
+    quat2eul(euler, q);
+}
+
 void NavEKF::calcposNE(float lat, float lon)
 {
     posNE[0] = RADIUS_OF_EARTH * (lat - latRef);
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index eaf0770cbe..e989a6b755 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -101,7 +101,7 @@ private:
 
     void eul2quat(float quat[4], float eul[3]);
 
-    void quat2eul(float eul[3],float quat[4]);
+    void quat2eul(Vector3f &eul, float quat[4]);
 
     void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD);