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; } }