From e28cbed1e9bad6d1001ff362c38009b09e21357c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 13 Nov 2012 15:33:10 +1100
Subject: [PATCH] AP_AHRS: prevent a large delta_t from affecting DCM solution

this should fix large changes in attitude directly after arming
ArduCopter
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

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();