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

DCM: separate out the omega_yaw_P from omega_P

Browse Source 
this cleans up the separation of drift rates and proportional
correction from yaw source and accelerometers, allow the yaw
to run at a different rate to the accel correction
This commit is contained in:
Andrew Tridgell 2012-03-08 18:12:46 +11:00
parent f4b1dae7cf
commit b67b0afd10
2 changed files with 142 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
libraries/AP_DCM/AP_DCM.cpp
View File

@ -104,7 +104,7 @@ AP_DCM::matrix_update(float _G_Dt)
_omega_integ_corr = _gyro_vector + _omega_I; _omega_integ_corr = _gyro_vector + _omega_I;
// Equation 16, adding proportional and integral correction terms // Equation 16, adding proportional and integral correction terms
_omega = _omega_integ_corr + _omega_P; _omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
// this is an expansion of the DCM matrix multiply (equation // this is an expansion of the DCM matrix multiply (equation
// 17), with known zero elements removed and the matrix // 17), with known zero elements removed and the matrix
@ -165,13 +165,12 @@ AP_DCM::matrix_reset(bool recover_eulers)
} }
// reset the integration terms // reset the integration terms
_omega_I.x = 0.0f; _omega_I.zero();
_omega_I.y = 0.0f; _omega_P.zero();
_omega_I.z = 0.0f; _omega_yaw_P.zero();
_omega_P = _omega_I; _omega_integ_corr.zero();
_omega_integ_corr = _omega_I; _omega_smoothed.zero();
_omega_smoothed = _omega_I; _omega.zero();
_omega = _omega_I;
// if the caller wants us to try to recover to the current // if the caller wants us to try to recover to the current
// attitude then calculate the dcm matrix from the current // attitude then calculate the dcm matrix from the current
@ -316,6 +315,9 @@ AP_DCM::normalize(void)
// gyro error. The _omega_P value is what pulls our attitude solution // gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an // back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros. // attempt to learn the long term drift rate of the gyros.
//
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
void void
AP_DCM::drift_correction(float deltat) AP_DCM::drift_correction(float deltat)
{ {
@ -357,31 +359,39 @@ AP_DCM::drift_correction(float deltat)
accel.z = -sqrt(zsquared); accel.z = -sqrt(zsquared);
} }
// calculate the error, in m/2^2, between the attitude
// implied by the accelerometers and the attitude
// in the current DCM matrix
error = _dcm_matrix.c % accel; error = _dcm_matrix.c % accel;
// error is in m/s^2 units // error from the above is in m/s^2 units.
// Limit max error to limit max omega_P and omega_I
// Limit max error to limit the effect of noisy values
// on the algorithm. This limits the error to about 11
// degrees
error_norm = error.length(); error_norm = error.length();
if (error_norm > 2) { if (error_norm > 2) {
error *= (2 / error_norm); error *= (2 / error_norm);
} }
// scale the error for the time over which we are // we now want to calculate _omega_P and _omega_I. The
// applying it // _omega_P value is what drags us quickly to the
error *= deltat; // accelerometer reading.
// calculate the new proportional offset
_omega_P = error * _kp_roll_pitch; _omega_P = error * _kp_roll_pitch;
// we limit the change in the integrator to the // the _omega_I is the long term accumulated gyro
// maximum gyro drift rate on each axis // error. This determines how much gyro drift we can
float drift_limit = ToRad(_gyro_drift_rate) * deltat / _ki_roll_pitch; // handle.
error.x = constrain(error.x, -drift_limit, drift_limit); Vector3f omega_I_delta = error * (_ki_roll_pitch * deltat);
error.y = constrain(error.y, -drift_limit, drift_limit);
error.z = constrain(error.z, -drift_limit, drift_limit);
// update gyro drift estimate // limit the slope of omega_I on each axis to
_omega_I += error * _ki_roll_pitch; // 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
@ -394,71 +404,76 @@ AP_DCM::drift_correction(float deltat)
// reference vector. In between times we rely on the gyros for // reference vector. In between times we rely on the gyros for
// yaw. Avoiding this calculation on every call to // yaw. Avoiding this calculation on every call to
// update_DCM() saves a lot of time // update_DCM() saves a lot of time
if (_compass && _compass->use_for_yaw() && if (_compass && _compass->use_for_yaw()) {
_compass->last_update != _compass_last_update) { if (_compass->last_update != _compass_last_update) {
if (_have_initial_yaw) {
// Equation 23, Calculating YAW error
// We make the gyro YAW drift correction based
// on compass magnetic heading
error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update); yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
_compass_last_update = _compass->last_update; if (_have_initial_yaw && yaw_deltat < 2.0) {
} else {
// this is our first estimate of the yaw,
// construct a DCM matrix based on the current
// roll/pitch and the compass heading, but
// first ensure the compass heading has been
// calculated
_compass->calculate(_dcm_matrix);
// now construct a new DCM matrix
_compass->null_offsets_disable();
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading);
_compass->null_offsets_enable();
_have_initial_yaw = true;
_compass_last_update = _compass->last_update;
}
} else if (_gps && _gps->status() == GPS::GPS_OK &&
_gps->last_fix_time != _gps_last_update) {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= GPS_SPEED_MIN) {
if (_have_initial_yaw) {
float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
// Equation 23, Calculating YAW error // Equation 23, Calculating YAW error
error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x); // We make the gyro YAW drift correction based
yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update); // on compass magnetic heading
_gps_last_update = _gps->last_fix_time; error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
} else { _compass_last_update = _compass->last_update;
// when we first start moving, set the } else {
// DCM matrix to the current // this is our first estimate of the yaw,
// roll/pitch values, but with yaw // or the compass has come back online after
// from the GPS // no readings for 2 seconds.
if (_compass) { //
_compass->null_offsets_disable(); // construct a DCM matrix based on the current
} // roll/pitch and the compass heading.
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course)); // First ensure the compass heading has been
if (_compass) { // calculated
_compass->null_offsets_enable(); _compass->calculate(_dcm_matrix);
}
_have_initial_yaw = true; // now construct a new DCM matrix
_compass->null_offsets_disable();
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading);
_compass->null_offsets_enable();
_have_initial_yaw = true;
_compass_last_update = _compass->last_update;
error_course = 0; error_course = 0;
_gps_last_update = _gps->last_fix_time;
} }
} else if (_gps->ground_speed >= GPS_SPEED_RESET) { }
// we are not going fast enough to use GPS for } else if (_gps && _gps->status() == GPS::GPS_OK) {
// course correction, but we won't reset if (_gps->last_fix_time != _gps_last_update) {
// _have_initial_yaw yet, instead we just let // Use GPS Ground course to correct yaw gyro drift
// the gyro handle yaw if (_gps->ground_speed >= GPS_SPEED_MIN) {
error_course = 0; yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
} else { if (_have_initial_yaw && yaw_deltat < 2.0) {
// we are moving very slowly. Reset float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
// _have_initial_yaw and adjust our heading float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
// rapidly next time we get a good GPS ground // Equation 23, Calculating YAW error
// speed error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
error_course = 0; _gps_last_update = _gps->last_fix_time;
_have_initial_yaw = false; } else {
// when we first start moving, set the
// DCM matrix to the current
// roll/pitch values, but with yaw
// from the GPS
if (_compass) {
_compass->null_offsets_disable();
}
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course));
if (_compass) {
_compass->null_offsets_enable();
}
_have_initial_yaw = true;
error_course = 0;
_gps_last_update = _gps->last_fix_time;
}
} else if (_gps->ground_speed >= GPS_SPEED_RESET) {
// we are not going fast enough to use GPS for
// course correction, but we won't reset
// _have_initial_yaw yet, instead we just let
// the gyro handle yaw
error_course = 0;
} else {
// we are moving very slowly. Reset
// _have_initial_yaw and adjust our heading
// rapidly next time we get a good GPS ground
// speed
error_course = 0;
_have_initial_yaw = false;
}
} }
} }
@ -467,22 +482,30 @@ AP_DCM::drift_correction(float deltat)
// only calculate it when we get new data from the yaw // only calculate it when we get new data from the yaw
// reference source // reference source
if (yaw_deltat == 0 || error_course == 0) { if (yaw_deltat == 0 || error_course == 0) {
// nothing to do // we don't have a new reference heading. Slowly
// decay the _omega_yaw_P to ensure that if we have
// lost the yaw reference sensor completely we don't
// keep using a stale offset
_omega_yaw_P *= 0.97;
return; return;
} }
// ensure the course error is scaled from -PI to PI
if (error_course > PI) {
error_course -= 2*PI;
} else if (error_course < -PI) {
error_course += 2*PI;
}
// Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
// this gives us an error in radians
error = _dcm_matrix.c * error_course; error = _dcm_matrix.c * error_course;
// Adding yaw correction to proportional correction vector. We // Adding yaw correction to proportional correction vector. We
// allow the yaw reference source to affect all 3 components // allow the yaw reference source to affect all 3 components
// of _omega_P as we need to be able to correctly hold a // of _omega_yaw_P as we need to be able to correctly hold a
// heading when roll and pitch are non-zero // heading when roll and pitch are non-zero
_omega_P += error * _kp_yaw; _omega_yaw_P = error * _kp_yaw;
// limit maximum gyro drift from yaw reference
float drift_limit = ToRad(_gyro_drift_rate) * yaw_deltat / _ki_yaw;
error.z = constrain(error.z, -drift_limit, drift_limit);
// add yaw correction to integrator correction vector, but // add yaw correction to integrator correction vector, but
// only for the z gyro. We rely on the accelerometers for x // only for the z gyro. We rely on the accelerometers for x
@ -490,7 +513,13 @@ AP_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
_omega_I.z += error.z * _ki_yaw; float omega_Iz_delta = error.z * (_ki_yaw * yaw_deltat);
// limit the slope of omega_I.z to the maximum gyro drift rate
float 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;

48
libraries/AP_DCM/AP_DCM.h
View File

@ -25,10 +25,8 @@ class AP_DCM
public: public:
// Constructors // Constructors
AP_DCM(IMU *imu, GPS *&gps) : AP_DCM(IMU *imu, GPS *&gps) :
_kp_roll_pitch(18.0), _kp_roll_pitch(0.13),
_ki_roll_pitch(0.0006), _kp_yaw(0.8),
_kp_yaw(9.0),
_ki_yaw(0.003),
_gps(gps), _gps(gps),
_imu(imu), _imu(imu),
_dcm_matrix(1, 0, 0, _dcm_matrix(1, 0, 0,
@ -36,7 +34,16 @@ public:
0, 0, 1), 0, 0, 1),
_health(1.), _health(1.),
_toggle(0) _toggle(0)
{} {
// base the ki values by the sensors maximum drift
// rate. The APM2 has gyros which are much less drift
// prone than the APM1, so we should have a lower ki,
// which will make us less prone to increasing omegaI
// incorrectly due to sensor noise
_gyro_drift_limit = imu->get_gyro_drift_rate();
_ki_roll_pitch = _gyro_drift_limit * 3;
_ki_yaw = _gyro_drift_limit * 4;
}
// Accessors // Accessors
@ -46,7 +53,7 @@ public:
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
// return the current drift correction integrator value // return the current drift correction integrator value
Vector3f get_integrator(void) {return _omega_I; } Vector3f get_gyro_drift(void) {return _omega_I; }
float get_health(void) {return _health;} float get_health(void) {return _health;}
void set_centripetal(bool b) {_centripetal = b;} void set_centripetal(bool b) {_centripetal = b;}
@ -55,7 +62,7 @@ public:
// Methods // Methods
void update_DCM(uint8_t drift_correction_frequency=1); void update_DCM(uint8_t drift_correction_frequency=1);
void update_DCM_fast(void); void update(void) { update_DCM(); }
void matrix_reset(bool recover_eulers = false); void matrix_reset(bool recover_eulers = false);
long roll_sensor; // Degrees * 100 long roll_sensor; // Degrees * 100
@ -70,18 +77,6 @@ public:
uint8_t renorm_range_count; uint8_t renorm_range_count;
uint8_t renorm_blowup_count; uint8_t renorm_blowup_count;
float kp_roll_pitch() { return _kp_roll_pitch; }
void kp_roll_pitch(float v) { _kp_roll_pitch = v; }
float ki_roll_pitch() { return _ki_roll_pitch; }
void ki_roll_pitch(float v) { _ki_roll_pitch = v; }
float kp_yaw() { return _kp_yaw; }
void kp_yaw(float v) { _kp_yaw = v; }
float ki_yaw() { return _ki_yaw; }
void ki_yaw(float v) { _ki_yaw = v; }
// status reporting // status reporting
float get_accel_weight(void); float get_accel_weight(void);
float get_renorm_val(void); float get_renorm_val(void);
@ -93,6 +88,7 @@ private:
float _ki_roll_pitch; float _ki_roll_pitch;
float _kp_yaw; float _kp_yaw;
float _ki_yaw; float _ki_yaw;
float _gyro_drift_limit; // radians/s/s
bool _have_initial_yaw; bool _have_initial_yaw;
// Methods // Methods
@ -106,9 +102,6 @@ private:
void drift_correction(float deltat); void drift_correction(float deltat);
void euler_angles(void); void euler_angles(void);
// max rate of gyro drift in degrees/s/s
static const float _gyro_drift_rate = 0.04;
// members // members
Compass * _compass; Compass * _compass;
@ -126,11 +119,12 @@ private:
Vector3f _accel_vector; Vector3f _accel_vector;
Vector3f _accel_sum; Vector3f _accel_sum;
Vector3f _gyro_vector; // Store the gyros turn rate in a vector Vector3f _gyro_vector; // Store the gyros turn rate in a vector
Vector3f _omega_P; // Omega Proportional correction Vector3f _omega_P; // accel Omega Proportional correction
Vector3f _omega_I; // Omega Integrator correction Vector3f _omega_yaw_P; // yaw Omega Proportional correction
Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction Vector3f _omega_I; // Omega Integrator correction
Vector3f _omega; // Corrected Gyro_Vector data Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; // Corrected Gyro_Vector data
Vector3f _omega_sum; Vector3f _omega_sum;
Vector3f _omega_smoothed; Vector3f _omega_smoothed;
float _health; float _health;