From 7fb60812c25dbe034201862cdf835a22b9de43be Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 17 Jan 2014 23:08:14 +1100
Subject: [PATCH] AP_NavEKF: Added pitch and roll inertial sensor to body trim
 adjustment

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

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 37370c3a99..8f7f72efbf 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -1985,6 +1985,7 @@ void NavEKF::getEulerAngles(Vector3f &euler) const
 {
     Quaternion q(states[0], states[1], states[2], states[3]);
     q.to_euler(&euler.x, &euler.y, &euler.z);
+    euler = euler - _ahrs->get_trim();
 }
 
 void NavEKF::getVelNED(Vector3f &vel) const
@@ -2291,8 +2292,10 @@ void NavEKF::ForceYawAlignment()
 
 void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
 {
+    Vector3f trim = _ahrs->get_trim();
     Quaternion q(states[0], states[1], states[2], states[3]);
     q.rotation_matrix(mat);
+    mat.rotateXYinv(trim);
 }
 
 void  NavEKF::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov) const