AP_AHRS: detect and try to cope with bad accels

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -654,8 +654,15 @@ AP_AHRS_DCM::drift_correction(float deltat)
         }
     }
 
-    // convert the error term to body frame
-    error = _dcm_matrix.mul_transpose(error);
+    // if ins is unhealthy then stop attitude drift correction and
+    // hope the gyros are OK for a while. Just slowly reduce _omega_P
+    // to prevent previous bad accels from throwing us off
+    if (!_ins.healthy()) {
+        error.zero();
+    } else {
+        // convert the error term to body frame
+        error = _dcm_matrix.mul_transpose(error);
+    }
 
     if (error.is_nan() || error.is_inf()) {
         // don't allow bad values