Browse Source

AP_NavEKF: prevent division by zero in SmallEKF

mission-4.1.18
Andrew Tridgell 10 years ago
parent
commit
55041c7a7a
  1. 8
      libraries/AP_NavEKF/AP_SmallEKF.cpp

8
libraries/AP_NavEKF/AP_SmallEKF.cpp

@ -893,13 +893,21 @@ void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector @@ -893,13 +893,21 @@ void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector
tilt = TiltCorrection;
velocity = state.velocity;
state.quat.to_euler(euler.x, euler.y, euler.z);
if (dtIMU < 1.0e-6) {
gyroBias.zero();
} else {
gyroBias = state.delAngBias / dtIMU;
}
}
// get gyro bias data
void SmallEKF::getGyroBias(Vector3f &gyroBias) const
{
if (dtIMU < 1.0e-6) {
gyroBias.zero();
} else {
gyroBias = state.delAngBias / dtIMU;
}
}
// get quaternion data

Loading…
Cancel
Save