From 13693e1a04a3271c5105867c73bc18128de4240f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 23 Feb 2012 07:59:16 +1100 Subject: [PATCH] DCM: reset more values on renorm blowup when DCM blows up, we need to reset a lot more variables to ensure that any NaN values don't persist --- libraries/AP_DCM/AP_DCM.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 433e3721ab..4a1aae5360 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -211,6 +211,13 @@ AP_DCM::matrix_reset(void) _omega_I.x = 0.0f; _omega_I.y = 0.0f; _omega_I.z = 0.0f; + _omega_P = _omega_I; + _omega_integ_corr = _omega_I; + _omega = _omega_I; + _error_roll_pitch = _omega_I; + _error_yaw = _omega_I; + _errorCourse = 0; + if (_compass != NULL) { _compass->null_offsets_enable(); // This call is needed to restart the nulling // Otherwise the reset in the DCM matrix can mess up