From 33673c954dc674ba3a274ab7cb3617fe11339952 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 2 Jan 2014 13:05:49 +1100
Subject: [PATCH] AP_NavEKF: implemented getRotationBodyToNED()

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index a2c561ee08..924deadc08 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -2110,4 +2110,11 @@ void NavEKF::calcEarthRateNED(Vector3f &omega, float latitude) const
     omega.z  = -earthRate*sinf(latitude);
 }
 
+void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
+{
+    Quaternion q(states[0], states[1], states[2], states[3]);
+    q.rotation_matrix(mat);
+}
+
+
 #endif // HAL_CPU_CLASS