From 1ea1b500a6e3d5b88e5d1d8b820df9efa2077de0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Mar 2012 20:35:08 +1100 Subject: [PATCH] Quaternion: don't update if we have a very long deltat this can be caused by stopping the system in a debugger --- libraries/AP_Quaternion/AP_Quaternion.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp index cb6cf426c5..a50f9ef86d 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.cpp +++ b/libraries/AP_Quaternion/AP_Quaternion.cpp @@ -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();