Browse Source

AP_AHRS: Prevent DCM reporting good tilt error when inverted

mission-4.1.18
Paul Riseborough 10 years ago committed by Randy Mackay
parent
commit
5184bca87f
  1. 8
      libraries/AP_AHRS/AP_AHRS_DCM.cpp

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp

@ -647,6 +647,7 @@ AP_AHRS_DCM::drift_correction(float deltat) @@ -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) @@ -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) {

Loading…
Cancel
Save