From ef505df78aa3acd64cc578bb8ad8417e8c5acdbb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 16 Oct 2020 14:20:27 +1100 Subject: [PATCH] AP_NavEKF3: remove unused quat2Tbn method --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 7 ------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 3 --- 2 files changed, 10 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 1e1d15017c..1c976d2010 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1544,13 +1544,6 @@ void NavEKF3_core::StoreQuatRotate(const Quaternion &deltaQuat) outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat; } -// calculate nav to body quaternions from body to nav rotation matrix -void NavEKF3_core::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const -{ - // Calculate the body to nav cosine matrix - quat.rotation_matrix(Tbn); -} - // force symmetry on the covariance matrix to prevent ill-conditioning void NavEKF3_core::ForceSymmetry() { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index f736e2c78f..d2b650224f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -715,9 +715,6 @@ private: // Rotate the stored output quaternion history through a quaternion rotation void StoreQuatRotate(const Quaternion &deltaQuat); - // calculate nav to body quaternions from body to nav rotation matrix - void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const; - // calculate the NED earth spin vector in rad/sec void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;