//
// drift_correction_yaw - yaw drift correction using the compass
//
void
AP_AHRS_MPU6000::drift_correction_yaw(void)
{
static float yaw_last_uncorrected = HEADING_UNKNOWN;
static float yaw_corrected = HEADING_UNKNOWN;
float yaw_delta = 0;
bool new_value = false;
float yaw_error;
static float heading;
// we assume the DCM matrix is completely uncorrect for yaw
// retrieve how much heading has changed according to non-corrected dcm
if( yaw_last_uncorrected != HEADING_UNKNOWN ) {
yaw_delta = wrap_PI(yaw - yaw_last_uncorrected); // the change in heading according to the gyros only since the last interation
yaw_last_uncorrected = yaw;
}
// initialise yaw_corrected (if necessary)
if( yaw_corrected != HEADING_UNKNOWN ) {
yaw_corrected = yaw;
}else{
yaw_corrected = wrap_PI(yaw_corrected + yaw_delta); // best guess of current yaw is previous iterations corrected yaw + yaw change from gyro
_dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw
}
// if we have new compass data
if( _compass && _compass->use_for_yaw() ) {
if (_compass->last_update != _compass_last_update) {
_compass_last_update = _compass->last_update;
heading = _compass->calculate_heading(_dcm_matrix);
if( !_have_initial_yaw ) {
yaw_corrected = heading;
_have_initial_yaw = true;
_dcm_matrix.from_euler(roll, pitch, yaw_corrected); // rebuild dcm matrix with best guess at current yaw
}
new_value = true;
}
}
// perform the yaw correction
if( new_value ) {
yaw_error = yaw_error_compass();
// the yaw error is a vector in earth frame
//Vector3f error = Vector3f(0,0, yaw_error);
// convert the error vector to body frame
//error = _dcm_matrix.mul_transpose(error);
// Update the differential component to reduce the error (it´s like a P control)
yaw_corrected += wrap_PI(yaw_error * _kp_yaw.get()); // probably completely wrong
// rebuild the dcm matrix yet again
_dcm_matrix.from_euler(roll, pitch, yaw_corrected);
}
}
// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
bool new_value = false;
float yaw_error;
float yaw_deltat;
if (AP_AHRS_DCM::use_compass()) {
/*
we are using compass for yaw
*/
if (_compass->last_update_usec() != _compass_last_update) {
yaw_deltat = (_compass->last_update_usec() - _compass_last_update) * 1.0e-6f;
_compass_last_update = _compass->last_update_usec();
// we force an additional compass read()
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_flags.have_initial_yaw && _compass->read()) {
float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass();
// also update the _gps_last_update, so if we later
// disable the compass due to significant yaw error we
// don't suddenly change yaw with a reset
_gps_last_update = _gps.last_fix_time_ms();
}
} else if (_flags.fly_forward && have_gps()) {
/*
we are using GPS for yaw
*/
if (_gps.last_fix_time_ms() != _gps_last_update &&
_gps.ground_speed() >= GPS_SPEED_MIN) {
yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps.last_fix_time_ms();
new_value = true;
float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f);
float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
yaw_error = sinf(yaw_error_rad);
/* reset yaw to match GPS heading under any of the
following 3 conditions:
1) if we have reached GPS_SPEED_MIN and have never had
yaw information before
2) if the last time we got yaw information from the GPS
is more than 20 seconds ago, which means we may have
suffered from considerable gyro drift
3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
and our yaw error is over 60 degrees, which means very
poor yaw. This can happen on bungee launch when the
operator pulls back the plane rapidly enough then on
release the GPS heading changes very rapidly
*/
if (!_flags.have_initial_yaw ||
yaw_deltat > 20 ||
(_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
// reset DCM matrix based on current yaw
_dcm_matrix.from_euler(roll, pitch, gps_course_rad);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
yaw_error = 0;
}
}
}
if (!new_value) {
// we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss
// of our yaw source
_omega_yaw_P *= 0.97f;
return;
}
// convert the error vector to body frame
float error_z = _dcm_matrix.c.z * yaw_error;
// the spin rate changes the P gain, and disables the
// integration at higher rates
float spin_rate = _omega.length();
// sanity check _kp_yaw
if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
_kp_yaw = AP_AHRS_YAW_P_MIN;
}
// update the proportional control to drag the
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani
// We also adjust the gain depending on the rate of change of horizontal velocity which
// is proportional to how observable the heading is from the acceerations and GPS velocity
// The accelration derived heading will be more reliable in turns than compass or GPS
_omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain();
if (use_fast_gains()) {
_omega_yaw_P.z *= 8;
}
// don't update the drift term if we lost the yaw reference
// for more than 2 seconds
if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
// also add to the I term
_omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
}
_error_yaw = 0.8f * _error_yaw + 0.2f * fabsf(yaw_error);
}
// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
bool new_value = false;
float yaw_error;
float yaw_deltat;
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
_compass_last_update = _compass->last_update;
if (!_have_initial_yaw) {
float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass();
}
} else if (_fly_forward && _gps && _gps->status() == GPS::GPS_OK) {
if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3;
_gps_last_update = _gps->last_fix_time;
if (!_have_initial_yaw) {
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01));
_omega_yaw_P.zero();
_have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_gps();
}
}
if (!new_value) {
// we don't have any new yaw information
// slowly decay _omega_yaw_P to cope with loss
// of our yaw source
_omega_yaw_P *= 0.97;
return;
}
// the yaw error is a vector in earth frame
Vector3f error = Vector3f(0,0, yaw_error);
// convert the error vector to body frame
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
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani
_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
// for more than 2 seconds
if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
// also add to the I term
_omega_I_sum.z += error.z * _ki_yaw * yaw_deltat;
}
_error_yaw_sum += fabs(yaw_error);
_error_yaw_count++;
}