Browse Source

AP_Mount: trim out dead getDebug method

master
Peter Barker 6 years ago committed by Peter Barker
parent
commit
126065e95c
  1. 13
      libraries/AP_Mount/SoloGimbalEKF.cpp
  2. 3
      libraries/AP_Mount/SoloGimbalEKF.h

13
libraries/AP_Mount/SoloGimbalEKF.cpp

@ -930,19 +930,6 @@ void SoloGimbalEKF::fixCovariance() @@ -930,19 +930,6 @@ void SoloGimbalEKF::fixCovariance()
}
}
// return data for debugging EKF
void SoloGimbalEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const
{
tilt = TiltCorrection;
velocity = state.velocity;
state.quat.to_euler(euler.x, euler.y, euler.z);
if (dtIMU < 1.0e-6f) {
gyroBias.zero();
} else {
gyroBias = state.delAngBias / dtIMU;
}
}
// get gyro bias data
void SoloGimbalEKF::getGyroBias(Vector3f &gyroBias) const
{

3
libraries/AP_Mount/SoloGimbalEKF.h

@ -82,9 +82,6 @@ public: @@ -82,9 +82,6 @@ public:
void reset();
// get some debug information
void getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const;
// get gyro bias data
void getGyroBias(Vector3f &gyroBias) const;

Loading…
Cancel
Save