From c7077fa9e80c7010de40278b0651cc9738c6d9d2 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 5 Nov 2011 08:59:16 -0600 Subject: [PATCH] In case of DCM renorm "blow-up" reset the drift correction integrators This should aid in recovery if we loose the AHRS solution and have to force to initial orientation. Also removed some old commented out code moved to the IMU object --- libraries/AP_DCM/AP_DCM.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 4cbcdfeb02..c0ed1f2f00 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -125,14 +125,6 @@ AP_DCM::matrix_update(float _G_Dt) Matrix3f update_matrix; Matrix3f temp_matrix; - //Record when you saturate any of the gyros. - /* - if( (fabs(_gyro_vector.x) >= radians(300)) || - (fabs(_gyro_vector.y) >= radians(300)) || - (fabs(_gyro_vector.z) >= radians(300))){ - gyro_sat_count++; - }*/ - _omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega) _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms @@ -232,6 +224,9 @@ AP_DCM::normalize(void) _dcm_matrix.c.x = 0.0f; _dcm_matrix.c.y = 0.0f; _dcm_matrix.c.z = 1.0f; + _omega_I.x = 0.0f; + _omega_I.y = 0.0f; + _omega_I.z = 0.0f; } }