mirror of https://github.com/ArduPilot/ardupilot
AHRS: implement spin rate limits
this follows the method that Bill developed in his fastRotations paper. We've demonstrated that this is indeed needed in APM, as we were able to produce the 'dizzy' effects in both the ArduPlane and ArduCopter simulator
This commit is contained in:
parent
3ffecc18d8
commit
0fa1f29cf3
|
@ -23,6 +23,12 @@
|
||||||
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
|
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
|
||||||
#define GPS_SPEED_RESET 100
|
#define GPS_SPEED_RESET 100
|
||||||
|
|
||||||
|
// the limit (in degrees/second) beyond which we stop integrating
|
||||||
|
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
|
||||||
|
// which results in false gyro drift. See
|
||||||
|
// http://gentlenav.googlecode.com/files/fastRotations.pdf
|
||||||
|
#define SPIN_RATE_LIMIT 20
|
||||||
|
|
||||||
// table of user settable parameters
|
// table of user settable parameters
|
||||||
const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
|
const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
|
||||||
// @Param: YAW_P
|
// @Param: YAW_P
|
||||||
|
@ -257,6 +263,18 @@ AP_AHRS_DCM::yaw_error_gps(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
float
|
||||||
|
AP_AHRS_DCM::_P_gain(float spin_rate)
|
||||||
|
{
|
||||||
|
if (spin_rate < ToDeg(50)) {
|
||||||
|
return 1.0;
|
||||||
|
}
|
||||||
|
if (spin_rate > ToDeg(500)) {
|
||||||
|
return 10.0;
|
||||||
|
}
|
||||||
|
return spin_rate/ToDeg(50);
|
||||||
|
}
|
||||||
|
|
||||||
// yaw drift correction using the compass or GPS
|
// yaw drift correction using the compass or GPS
|
||||||
// this function prodoces the _omega_yaw_P vector, and also
|
// this function prodoces the _omega_yaw_P vector, and also
|
||||||
// contributes to the _omega_I.z long term yaw drift estimate
|
// contributes to the _omega_I.z long term yaw drift estimate
|
||||||
|
@ -309,13 +327,19 @@ AP_AHRS_DCM::drift_correction_yaw(float deltat)
|
||||||
// convert the error vector to body frame
|
// convert the error vector to body frame
|
||||||
error = _dcm_matrix.mul_transpose(error);
|
error = _dcm_matrix.mul_transpose(error);
|
||||||
|
|
||||||
|
// the spin rate changes the P gain, and disables the
|
||||||
|
// integration at higher rates
|
||||||
|
float spin_rate = _omega.length();
|
||||||
|
|
||||||
// update the proportional control to drag the
|
// update the proportional control to drag the
|
||||||
// yaw back to the right value
|
// yaw back to the right value. We use a gain
|
||||||
_omega_yaw_P = error * _kp_yaw.get();
|
// that depends on the spin rate. See the fastRotations.pdf
|
||||||
|
// paper from Bill Premerlani
|
||||||
|
_omega_yaw_P = error * _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
|
||||||
if (yaw_deltat < 2.0) {
|
if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
|
||||||
// also add to the I term
|
// also add to the I term
|
||||||
_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
|
_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
|
||||||
}
|
}
|
||||||
|
@ -426,14 +450,19 @@ AP_AHRS_DCM::drift_correction(float deltat)
|
||||||
// convert the error term to body frame
|
// convert the error term to body frame
|
||||||
error = _dcm_matrix.mul_transpose(error);
|
error = _dcm_matrix.mul_transpose(error);
|
||||||
|
|
||||||
|
// base the P gain on the spin rate
|
||||||
|
float spin_rate = _omega.length();
|
||||||
|
|
||||||
// we now want to calculate _omega_P and _omega_I. The
|
// we now want to calculate _omega_P and _omega_I. The
|
||||||
// _omega_P value is what drags us quickly to the
|
// _omega_P value is what drags us quickly to the
|
||||||
// accelerometer reading.
|
// accelerometer reading.
|
||||||
_omega_P = error * _kp;
|
_omega_P = error * _P_gain(spin_rate) * _kp;
|
||||||
|
|
||||||
// accumulate some integrator error
|
// accumulate some integrator error
|
||||||
|
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
|
||||||
_omega_I_sum += error * _ki * _ra_deltat;
|
_omega_I_sum += error * _ki * _ra_deltat;
|
||||||
_omega_I_sum_time += _ra_deltat;
|
_omega_I_sum_time += _ra_deltat;
|
||||||
|
}
|
||||||
|
|
||||||
if (_omega_I_sum_time >= 5) {
|
if (_omega_I_sum_time >= 5) {
|
||||||
// limit the rate of change of omega_I to the hardware
|
// limit the rate of change of omega_I to the hardware
|
||||||
|
|
|
@ -75,6 +75,9 @@ private:
|
||||||
float _omega_I_sum_time;
|
float _omega_I_sum_time;
|
||||||
Vector3f _omega; // Corrected Gyro_Vector data
|
Vector3f _omega; // Corrected Gyro_Vector data
|
||||||
|
|
||||||
|
// P term gain based on spin rate
|
||||||
|
float _P_gain(float spin_rate);
|
||||||
|
|
||||||
// state to support status reporting
|
// state to support status reporting
|
||||||
float _renorm_val_sum;
|
float _renorm_val_sum;
|
||||||
uint16_t _renorm_val_count;
|
uint16_t _renorm_val_count;
|
||||||
|
|
Loading…
Reference in New Issue