From dac59cb5da185f4cbb5fe81e946dbafe2d900a8e Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 9 Aug 2016 11:33:51 -0700 Subject: [PATCH] AP_NavEKF2: use exact matrix for trim rotation --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index f20a00b7ea..8b2e48abf4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -136,9 +136,8 @@ void NavEKF2_core::getTiltError(float &ang) const // return the transformation matrix from XYZ (body) to NED axes void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const { - Vector3f trim = _ahrs->get_trim(); outputDataNew.quat.rotation_matrix(mat); - mat.rotateXYinv(trim); + mat = mat * _ahrs->get_rotation_vehicle_body_to_autopilot_body(); } // return the quaternions defining the rotation from NED to XYZ (body) axes