Browse Source

AP_AHRS: removed limit on normalisation of accel reference vectors

this could lead to a bias in the accel drift correction
mission-4.1.18
Andrew Tridgell 11 years ago
parent
commit
cb52b6f367
  1. 6
      libraries/AP_AHRS/AP_AHRS_DCM.cpp

6
libraries/AP_AHRS/AP_AHRS_DCM.cpp

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

Loading…
Cancel
Save