Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

In case of DCM renorm "blow-up" reset the drift correction integrators

Browse Source 
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
This commit is contained in:
Doug Weibel 2011-11-05 08:59:16 -06:00
parent c265555b69
commit c7077fa9e8
1 changed files with 3 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_DCM/AP_DCM.cpp
View File

@ -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;
}
}