From 70b0bb475ffb43981b51e4f41baf441325940c44 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Nov 2013 13:54:16 +1100 Subject: [PATCH] AP_AHRS: detect and try to cope with bad accels --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 2dcd3cdf3a..b0240e6690 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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