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

AHRS: check for bad values in the error before they can affect DCM 

this should fix the DCM renorm errors in autotest, probably caused by
bad climb rates
This commit is contained in:
Andrew Tridgell 2012-08-18 13:53:59 +10:00
parent adda41a2ec
commit 01ae8f8771
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -553,6 +553,12 @@ AP_AHRS_DCM::drift_correction(float deltat)
// convert the error term to body frame // convert the error term to body frame
error = _dcm_matrix.mul_transpose(error); error = _dcm_matrix.mul_transpose(error);
if (error.is_nan() || error.is_inf()) {
// don't allow bad values
check_matrix();
return;
}
_error_rp_sum += error.length(); _error_rp_sum += error.length();
_error_rp_count++; _error_rp_count++;