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

AHRS: make the yaw independent drift correction optional and disable

Browse Source 
the new correction algorithm copes poorly with gyro drift, leading to
signification attitude errors in the face of drift
This commit is contained in:
Andrew Tridgell 2012-07-06 13:03:46 +10:00
parent b3fab35b44
commit 9845a55cb1
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -342,7 +342,8 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// yaw back to the right value. We use a gain // yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf // that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani // paper from Bill Premerlani
_omega_yaw_P = error * _P_gain(spin_rate) * _kp_yaw.get();
_omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw.get();
// don't update the drift term if we lost the yaw reference // don't update the drift term if we lost the yaw reference
// for more than 2 seconds // for more than 2 seconds
@ -411,6 +412,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
_have_gps_lock = true; _have_gps_lock = true;
} }
#define USE_BAROMETER_FOR_VERTICAL_VELOCITY 1
#if USE_BAROMETER_FOR_VERTICAL_VELOCITY
/* /*
The barometer for vertical velocity is only enabled if we got The barometer for vertical velocity is only enabled if we got
at least 5 pressure samples for the reading. This ensures we at least 5 pressure samples for the reading. This ensures we
@ -420,6 +423,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// Z velocity is down // Z velocity is down
velocity.z = - _barometer->get_climb_rate(); velocity.z = - _barometer->get_climb_rate();
} }
#endif
// see if this is our first time through - in which case we // see if this is our first time through - in which case we
// just setup the start times and return // just setup the start times and return
@ -443,8 +447,14 @@ AP_AHRS_DCM::drift_correction(float deltat)
// calculate the error term in earth frame. // calculate the error term in earth frame.
Vector3f GA_b = _ra_sum / _ra_deltat; Vector3f GA_b = _ra_sum / _ra_deltat;
GA_b.normalize(); GA_b.normalize();
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
}
Vector3f error = GA_b % GA_e; Vector3f error = GA_b % GA_e;
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z // step 2 calculate earth_error_Z
float earth_error_Z = error.z; float earth_error_Z = error.z;
@ -459,14 +469,14 @@ AP_AHRS_DCM::drift_correction(float deltat)
// step 6 // step 6
error = GA_b % GA_e2; error = GA_b % GA_e2;
error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION
// only use the gps/accelerometers for earth frame yaw correction // only use the gps/accelerometers for earth frame yaw correction
// if we are not using a compass. Otherwise we have two competing // if we are not using a compass. Otherwise we have two competing
// controllers for yaw correction // controllers for yaw correction
if (_compass && _compass->use_for_yaw()) { if (_compass && _compass->use_for_yaw()) {
error.z = 0; error.z = 0;
} else {
error.z = earth_error_Z;
} }
// convert the error term to body frame // convert the error term to body frame