diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 39fce9afa5..a39780a99e 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -43,6 +43,15 @@ AP_AHRS_DCM::update(void) // ask the IMU how much time this sensor reading represents delta_t = _ins->get_delta_time(); + // if the update call took more than 0.2 seconds then discard it, + // otherwise we may move too far. This happens when arming motors + // in ArduCopter + if (delta_t > 0.2) { + _ra_sum.zero(); + _ra_deltat = 0; + return; + } + // Get current values for gyros _gyro_vector = _ins->get_gyro(); _accel_vector = _ins->get_accel();