From f5e5ccff6a8972264a3478a30496b169783b3e20 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 7 Mar 2012 17:12:42 +1100 Subject: [PATCH] DCM: code cleanup and added more comments --- libraries/AP_DCM/AP_DCM.cpp | 63 ++++++++++++++++++++++++++----------- 1 file changed, 44 insertions(+), 19 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 5b4d4f625e..9e5e66de64 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -35,7 +35,7 @@ AP_DCM::set_compass(Compass *compass) // run a full DCM update round // the drift_correction_frequency is how many steps of the algorithm -// to run before doing an accelerometer drift correction step +// to run before doing an accelerometer and yaw drift correction step void AP_DCM::update_DCM(uint8_t drift_correction_frequency) { @@ -58,9 +58,6 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency) // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); - if (_dcm_matrix.is_nan()) { - SITL_debug("NaN matrix\n"); - } // add up the omega vector so we pass a value to the drift // correction averaged over the same time period as the @@ -73,9 +70,9 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency) // see if we will perform drift correction on this call _drift_correction_count++; - if (_drift_correction_count == drift_correction_frequency) { + if (_drift_correction_count >= drift_correction_frequency) { // calculate the average accelerometer vector over - // this time + // since the last drift correction call float scale = 1.0 / _drift_correction_count; _accel_vector = _accel_sum * scale; _accel_sum.zero(); @@ -102,17 +99,25 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency) void AP_DCM::matrix_update(float _G_Dt) { - // Used for _centripetal correction (theoretically better than _omega) + // _omega_integ_corr is used for _centripetal correction + // (theoretically better than _omega) _omega_integ_corr = _gyro_vector + _omega_I; + // Equation 16, adding proportional and integral correction terms _omega = _omega_integ_corr + _omega_P; + // this is an expansion of the DCM matrix multiply (equation + // 17), with known zero elements removed and the matrix + // operations inlined. This runs much faster than the original + // version of this code, as the compiler was doing a terrible + // job of realising that so many of the factors were in common + // or zero. It also uses much less stack, as we no longer need + // additional local matrices + float tmpx = _G_Dt * _omega.x; float tmpy = _G_Dt * _omega.y; float tmpz = _G_Dt * _omega.z; - // this is an expansion of the DCM matrix multiply, with known - // zero elements removed _dcm_matrix.a.x += _dcm_matrix.a.y * tmpz - _dcm_matrix.a.z * tmpy; _dcm_matrix.a.y += _dcm_matrix.a.z * tmpx - _dcm_matrix.a.x * tmpz; _dcm_matrix.a.z += _dcm_matrix.a.x * tmpy - _dcm_matrix.a.y * tmpx; @@ -221,7 +226,8 @@ AP_DCM::check_matrix(void) } } -/**************************************************/ +// renormalise one vector component of the DCM matrix +// this will return false if renormalization fails bool AP_DCM::renorm(Vector3f const &a, Vector3f &result) { @@ -304,7 +310,11 @@ AP_DCM::normalize(void) } -/**************************************************/ +// 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. void AP_DCM::drift_correction(float deltat) { @@ -333,7 +343,9 @@ AP_DCM::drift_correction(float deltat) // sensor reading completely. Logs show that the z accel is // the noisest, plus it has a disproportionate impact on the // drift correction result because of the geometry when we are - // mostly flat + // mostly flat. Dropping it completely seems to make the DCM + // algorithm much more resilient to large amounts of + // accelerometer noise. float zsquared = gravity_squared - ((accel.x * accel.x) + (accel.y * accel.y)); if (zsquared < 0) { _omega_P.zero(); @@ -377,6 +389,10 @@ AP_DCM::drift_correction(float deltat) // 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() && _compass->last_update != _compass_last_update) { if (_have_initial_yaw) { @@ -445,6 +461,10 @@ AP_DCM::drift_correction(float deltat) } } + // 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) { // nothing to do return; @@ -453,27 +473,32 @@ AP_DCM::drift_correction(float deltat) // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft error = _dcm_matrix.c * error_course; - // Adding yaw correction to proportional correction vector. + // Adding yaw correction to proportional correction vector. We + // allow the yaw reference source to affect all 3 components + // of _omega_P as we need to be able to correctly hold a + // heading when roll and pitch are non-zero _omega_P += error * _kp_yaw; - // limit maximum gyro drift + // 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 // only for the z gyro. We rely on the accelerometers for x - // and y gyro drift correction. Using the compass for x/y drift - // correction is too inaccurate, and can lead to incorrect builups in - // the x/y drift + // 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.z += error.z * _ki_yaw; + // we keep the sum of yaw error for reporting via MAVLink. _error_yaw_sum += error_course; _error_yaw_count++; - //Serial.print("*"); } -/**************************************************/ +// calculate the euler angles which will be used for high level +// navigation control void AP_DCM::euler_angles(void) {