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

AP_AHRS: Prevent DCM reporting good tilt error when inverted

This commit is contained in:
Paul Riseborough 2015-04-22 11:15:48 +10:00 committed by Randy Mackay
parent ea8217bd3a
commit 5184bca87f
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -647,6 +647,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// running at 800Hz and the MPU6000 running at 1kHz, by combining
// the two the effects of aliasing are greatly reduced.
Vector3f error[INS_MAX_INSTANCES];
float error_dirn[INS_MAX_INSTANCES];
Vector3f GA_b[INS_MAX_INSTANCES];
int8_t besti = -1;
float best_error = 0;
@ -673,11 +674,18 @@ AP_AHRS_DCM::drift_correction(float deltat)
continue;
}
error[i] = GA_b[i] % GA_e;
// Take dot product to catch case vectors are opposite sign and parallel
error_dirn[i] = GA_b[i] * GA_e;
float error_length = error[i].length();
if (besti == -1 || error_length < best_error) {
besti = i;
best_error = error_length;
}
// Catch case where orientation is 180 degrees out
if (error_dirn[besti] < 0.0f) {
best_error = 1.0f;
}
}
if (besti == -1) {