void
AP_DCM::matrix_update(float _G_Dt)
{
Matrix3f update_matrix;
Matrix3f temp_matrix;
//Record when you saturate any of the gyros.
if((fabs(_gyro_vector.x) >= radians(300)) ||
(fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300))){
gyro_sat_count++;
}
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centripetal){
accel_adjust(); // Remove _centripetal acceleration.
}
#if OUTPUTMODE == 1
update_matrix.a.x = 0;
update_matrix.a.y = -_G_Dt * _omega.z; // -delta Theta z
update_matrix.a.z = _G_Dt * _omega.y; // delta Theta y
update_matrix.b.x = _G_Dt * _omega.z; // delta Theta z
update_matrix.b.y = 0;
update_matrix.b.z = -_G_Dt * _omega.x; // -delta Theta x
update_matrix.c.x = -_G_Dt * _omega.y; // -delta Theta y
update_matrix.c.y = _G_Dt * _omega.x; // delta Theta x
update_matrix.c.z = 0;
#else // Uncorrected data (no drift correction)
update_matrix.a.x = 0;
update_matrix.a.y = -_G_Dt * _gyro_vector.z;
update_matrix.a.z = _G_Dt * _gyro_vector.y;
update_matrix.b.x = _G_Dt * _gyro_vector.z;
update_matrix.b.y = 0;
update_matrix.b.z = -_G_Dt * _gyro_vector.x;
update_matrix.c.x = -_G_Dt * _gyro_vector.y;
update_matrix.c.y = _G_Dt * _gyro_vector.x;
update_matrix.c.z = 0;
#endif
temp_matrix = _dcm_matrix * update_matrix;
_dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17
}
void
AP_DCM::matrix_update(float _G_Dt)
{
Matrix3f update_matrix;
Matrix3f temp_matrix;
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centripetal){
accel_adjust(); // Remove _centripetal acceleration.
}
#if OUTPUTMODE == 1
float tmp = _G_Dt * _omega.x;
update_matrix.b.z = -tmp; // -delta Theta x
update_matrix.c.y = tmp; // delta Theta x
tmp = _G_Dt * _omega.y;
update_matrix.c.x = -tmp; // -delta Theta y
update_matrix.a.z = tmp; // delta Theta y
tmp = _G_Dt * _omega.z;
update_matrix.b.x = tmp; // delta Theta z
update_matrix.a.y = -tmp; // -delta Theta z
update_matrix.a.x = 0;
update_matrix.b.y = 0;
update_matrix.c.z = 0;
#else // Uncorrected data (no drift correction)
update_matrix.a.x = 0;
update_matrix.a.y = -_G_Dt * _gyro_vector.z;
update_matrix.a.z = _G_Dt * _gyro_vector.y;
update_matrix.b.x = _G_Dt * _gyro_vector.z;
update_matrix.b.y = 0;
update_matrix.b.z = -_G_Dt * _gyro_vector.x;
update_matrix.c.x = -_G_Dt * _gyro_vector.y;
update_matrix.c.y = _G_Dt * _gyro_vector.x;
update_matrix.c.z = 0;
#endif
temp_matrix = _dcm_matrix * update_matrix;
_dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17
}
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// 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
AP_AHRS_DCM::drift_correction(float deltat)
{
float error_course = 0;
Vector3f accel;
Vector3f error;
float error_norm = 0;
float yaw_deltat = 0;
accel = _accel_vector;
// if enabled, use the GPS to correct our accelerometer vector
// for centripetal forces
if(_centripetal &&
_gps != NULL &&
_gps->status() == GPS::GPS_OK) {
accel_adjust(accel);
}
//*****Roll and Pitch***************
// normalise the accelerometer vector to a standard length
// this is important to reduce the impact of noise on the
// drift correction, as very noisy vectors tend to have
// abnormally high lengths. By normalising the length we
// reduce their impact.
float accel_length = accel.length();
accel *= (_gravity / accel_length);
if (accel.is_inf()) {
// we can't do anything useful with this sample
_omega_P.zero();
return;
}
// 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;
// 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();
if (error_norm > 2) {
error *= (2 / error_norm);
}
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _kp_roll_pitch;
// the _omega_I is the long term accumulated gyro
// error. This determines how much gyro drift we can
// handle.
_omega_I_sum += error * (_ki_roll_pitch * deltat);
_omega_I_sum_time += deltat;
// these sums support the reporting of the DCM state via MAVLink
_error_rp_sum += error_norm;
_error_rp_count++;
// yaw drift correction
// we only do yaw drift correction when we get a new yaw
// reference vector. In between times we rely on the gyros for
// yaw. Avoiding this calculation on every call to
// update_DCM() saves a lot of time
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
// 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);
_compass_last_update = _compass->last_update;
} else {
// this is our first estimate of the yaw,
// or the compass has come back online after
// no readings for 2 seconds.
//
// construct a DCM matrix based on the current
// roll/pitch and the compass heading.
// First ensure the compass heading has been
// calculated
_compass->calculate(_dcm_matrix);
// now construct a new DCM matrix
_compass->null_offsets_disable();
_dcm_matrix.from_euler(roll, pitch, _compass->heading);
_compass->null_offsets_enable();
_have_initial_yaw = true;
_compass_last_update = _compass->last_update;
error_course = 0;
}
}
} else if (_gps && _gps->status() == GPS::GPS_OK) {
if (_gps->last_fix_time != _gps_last_update) {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
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
error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
_gps_last_update = _gps->last_fix_time;
} 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();
}
_dcm_matrix.from_euler(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;
}
}
}
// see if there is any error in our heading relative to the
// yaw reference. This will be zero most of the time, as we
// only calculate it when we get new data from the yaw
// reference source
if (yaw_deltat == 0 || error_course == 0) {
// 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;
goto check_sum_time;
}
// 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
// this gives us an error in radians
error = _dcm_matrix.c * error_course;
// Adding yaw correction to proportional correction vector. We
// allow the yaw reference source to affect all 3 components
// of _omega_yaw_P as we need to be able to correctly hold a
// heading when roll and pitch are non-zero
_omega_yaw_P = error * _kp_yaw.get();
// add yaw correction to integrator correction vector, but
// only for the z gyro. We rely on the accelerometers for x
// and y gyro drift correction. Using the compass or GPS for
// x/y drift correction is too inaccurate, and can lead to
// incorrect builups in the x/y drift. We rely on the
// accelerometers to get the x/y components right
_omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);
// we keep the sum of yaw error for reporting via MAVLink.
_error_yaw_sum += error_course;
_error_yaw_count++;
check_sum_time:
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;
}
}
