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

AHRS: normalise GA_b before computing error vector only if too large

Browse Source 
When GA_b is small the direction of the vector is unreliable, so
normalising can exacerbate the error in the direction
This commit is contained in:
Jonathan Challinger 2012-07-23 17:15:22 +10:00 committed by Andrew Tridgell
parent 944488afaf
commit 21f0ed00b3
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -459,11 +459,14 @@ AP_AHRS_DCM::drift_correction(float deltat)
}
// calculate the error term in earth frame.
Vector3f GA_b = _ra_sum / _ra_deltat;
GA_b.normalize();
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
Vector3f GA_b = _ra_sum / (_ra_deltat * _gravity);
float length = GA_b.length();
if (length > 1.0) {
GA_b /= length;
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
}
}
Vector3f error = GA_b % GA_e;