Browse Source

AP_NavEKF2: remove unused quat2Tbn method

c415-sdk
Peter Barker 4 years ago committed by Andrew Tridgell
parent
commit
53305ba2a4
  1. 7
      libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
  2. 3
      libraries/AP_NavEKF2/AP_NavEKF2_core.h

7
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@ -1502,13 +1502,6 @@ void NavEKF2_core::StoreQuatRotate(const Quaternion &deltaQuat) @@ -1502,13 +1502,6 @@ void NavEKF2_core::StoreQuatRotate(const Quaternion &deltaQuat)
outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;
}
// calculate nav to body quaternions from body to nav rotation matrix
void NavEKF2_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 NavEKF2_core::ForceSymmetry()
{

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h

@ -594,9 +594,6 @@ private: @@ -594,9 +594,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;

Loading…
Cancel
Save