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

DCM: reset more values on renorm blowup

Browse Source 
when DCM blows up, we need to reset a lot more variables to ensure
that any NaN values don't persist
This commit is contained in:
Andrew Tridgell 2012-02-23 07:59:16 +11:00
parent b231112957
commit 13693e1a04
1 changed files with 7 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_DCM/AP_DCM.cpp
View File

@ -211,6 +211,13 @@ AP_DCM::matrix_reset(void)
_omega_I.x = 0.0f; _omega_I.x = 0.0f;
_omega_I.y = 0.0f; _omega_I.y = 0.0f;
_omega_I.z = 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) { if (_compass != NULL) {
_compass->null_offsets_enable(); // This call is needed to restart the nulling _compass->null_offsets_enable(); // This call is needed to restart the nulling
// Otherwise the reset in the DCM matrix can mess up // Otherwise the reset in the DCM matrix can mess up