Browse Source

Quaternion: don't update if we have a very long deltat

this can be caused by stopping the system in a debugger
mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
c7ef72cf28
  1. 7
      libraries/AP_Quaternion/AP_Quaternion.cpp

7
libraries/AP_Quaternion/AP_Quaternion.cpp

@ -257,6 +257,13 @@ void AP_Quaternion::update(void) @@ -257,6 +257,13 @@ void AP_Quaternion::update(void)
_imu->update();
deltat = _imu->get_delta_time();
if (deltat > 1.0) {
// if we stop updating for 1s, we should discard this
// input data. This can happen if you are running the
// code under a debugger, and using this data point
// will just throw off your attitude by a huge amount
return;
}
// get current IMU state
gyro = _imu->get_gyro();

Loading…
Cancel
Save