Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2012-02-23 07:59:16 +11:00
parent 95ef9206dc
commit 2bb58db3f7
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