aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DCM: buffer omega_I changes over 10 seconds

Browse Source 
this buffers up _omega_I changes in _omega_I_sum over a period of 10
seconds, applying the slope limit only when _omega_I_sum is
transferred to _omega_I.

The result is a huge improvement in the ability of _omega_I to track
gyro drift over the long term.
This commit is contained in:
Andrew Tridgell 2012-04-16 12:14:19 +10:00
parent 398a608b83
commit 6d837891b0
2 changed files with 27 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -328,16 +328,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
// the _omega_I is the long term accumulated gyro // the _omega_I is the long term accumulated gyro
// error. This determines how much gyro drift we can // error. This determines how much gyro drift we can
// handle. // handle.
Vector3f omega_I_delta = error * (_ki_roll_pitch * deltat); _omega_I_sum += error * (_ki_roll_pitch * deltat);
_omega_I_sum_time += deltat;
// limit the slope of omega_I on each axis to
// the maximum drift rate
float drift_limit = _gyro_drift_limit * deltat;
omega_I_delta.x = constrain(omega_I_delta.x, -drift_limit, drift_limit);
omega_I_delta.y = constrain(omega_I_delta.y, -drift_limit, drift_limit);
omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit);
_omega_I += omega_I_delta;
// these sums support the reporting of the DCM state via MAVLink // these sums support the reporting of the DCM state via MAVLink
_error_rp_sum += error_norm; _error_rp_sum += error_norm;
@ -458,17 +450,33 @@ AP_AHRS_DCM::drift_correction(float deltat)
// x/y drift correction is too inaccurate, and can lead to // x/y drift correction is too inaccurate, and can lead to
// incorrect builups in the x/y drift. We rely on the // incorrect builups in the x/y drift. We rely on the
// accelerometers to get the x/y components right // accelerometers to get the x/y components right
float omega_Iz_delta = error.z * (_ki_yaw * yaw_deltat); _omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);
// limit the slope of omega_I.z to the maximum gyro drift rate
drift_limit = _gyro_drift_limit * yaw_deltat;
omega_Iz_delta = constrain(omega_Iz_delta, -drift_limit, drift_limit);
_omega_I.z += omega_Iz_delta;
// we keep the sum of yaw error for reporting via MAVLink. // we keep the sum of yaw error for reporting via MAVLink.
_error_yaw_sum += error_course; _error_yaw_sum += error_course;
_error_yaw_count++; _error_yaw_count++;
if (_omega_I_sum_time > 10) {
// every 10 seconds we apply the accumulated
// _omega_I_sum changes to _omega_I. We do this to
// prevent short term feedback between the P and I
// terms of the controller. The _omega_I vector is
// supposed to refect long term gyro drift, but with
// high noise it can start to build up due to short
// term interactions. By summing it over 10 seconds we
// prevent the short term interactions and can apply
// the slope limit more accurately
float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
_omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
_omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);
_omega_I += _omega_I_sum;
// zero the sum
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
} }

2
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -69,6 +69,8 @@ private:
Vector3f _omega_P; // accel Omega Proportional correction Vector3f _omega_P; // accel Omega Proportional correction
Vector3f _omega_yaw_P; // yaw Omega Proportional correction Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega_I_sum; // summation vector for omegaI
float _omega_I_sum_time;
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data Vector3f _omega; // Corrected Gyro_Vector data