mirror of https://github.com/ArduPilot/ardupilot
570 lines
17 KiB
C++
570 lines
17 KiB
C++
/*
|
|
APM_AHRS_DCM.cpp
|
|
|
|
AHRS system using DCM matrices
|
|
|
|
Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
|
|
|
|
Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
|
|
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU Lesser General Public License
|
|
as published by the Free Software Foundation; either version 2.1
|
|
of the License, or (at your option) any later version.
|
|
*/
|
|
#include <FastSerial.h>
|
|
#include <AP_AHRS.h>
|
|
|
|
// this is the speed in cm/s above which we first get a yaw lock with
|
|
// the GPS
|
|
#define GPS_SPEED_MIN 300
|
|
|
|
// this is the speed in cm/s at which we stop using drift correction
|
|
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
|
|
#define GPS_SPEED_RESET 100
|
|
|
|
// the limit (in degrees/second) beyond which we stop integrating
|
|
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
|
|
// which results in false gyro drift. See
|
|
// http://gentlenav.googlecode.com/files/fastRotations.pdf
|
|
#define SPIN_RATE_LIMIT 20
|
|
|
|
// table of user settable parameters
|
|
const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
|
|
// @Param: YAW_P
|
|
// @DisplayName: Yaw P
|
|
// @Description: This controls the weight the compass has on the overall heading
|
|
// @Range: 0 .4
|
|
// @Increment: .01
|
|
AP_GROUPINFO("YAW_P", 0, AP_AHRS_DCM, _kp_yaw),
|
|
AP_GROUPINFO("RP_P", 1, AP_AHRS_DCM, _kp),
|
|
AP_GROUPEND
|
|
};
|
|
|
|
// run a full DCM update round
|
|
void
|
|
AP_AHRS_DCM::update(void)
|
|
{
|
|
float delta_t;
|
|
|
|
// tell the IMU to grab some data
|
|
_imu->update();
|
|
|
|
// ask the IMU how much time this sensor reading represents
|
|
delta_t = _imu->get_delta_time();
|
|
|
|
// Get current values for gyros
|
|
_gyro_vector = _imu->get_gyro();
|
|
_accel_vector = _imu->get_accel();
|
|
|
|
// Integrate the DCM matrix using gyro inputs
|
|
matrix_update(delta_t);
|
|
|
|
// Normalize the DCM matrix
|
|
normalize();
|
|
|
|
// Perform drift correction
|
|
drift_correction(delta_t);
|
|
|
|
// paranoid check for bad values in the DCM matrix
|
|
check_matrix();
|
|
|
|
// Calculate pitch, roll, yaw for stabilization and navigation
|
|
euler_angles();
|
|
}
|
|
|
|
// update the DCM matrix using only the gyros
|
|
void
|
|
AP_AHRS_DCM::matrix_update(float _G_Dt)
|
|
{
|
|
// note that we do not include the P terms in _omega. This is
|
|
// because the spin_rate is calculated from _omega.length(),
|
|
// and including the P terms would give positive feedback into
|
|
// the _P_gain() calculation, which can lead to a very large P
|
|
// value
|
|
_omega = _gyro_vector + _omega_I;
|
|
|
|
_dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt);
|
|
}
|
|
|
|
|
|
/*
|
|
reset the DCM matrix and omega. Used on ground start, and on
|
|
extreme errors in the matrix
|
|
*/
|
|
void
|
|
AP_AHRS_DCM::reset(bool recover_eulers)
|
|
{
|
|
// reset the integration terms
|
|
_omega_I.zero();
|
|
_omega_P.zero();
|
|
_omega_yaw_P.zero();
|
|
_omega.zero();
|
|
|
|
// if the caller wants us to try to recover to the current
|
|
// attitude then calculate the dcm matrix from the current
|
|
// roll/pitch/yaw values
|
|
if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
|
|
_dcm_matrix.from_euler(roll, pitch, yaw);
|
|
} else {
|
|
// otherwise make it flat
|
|
_dcm_matrix.from_euler(0, 0, 0);
|
|
}
|
|
}
|
|
|
|
/*
|
|
check the DCM matrix for pathological values
|
|
*/
|
|
void
|
|
AP_AHRS_DCM::check_matrix(void)
|
|
{
|
|
if (_dcm_matrix.is_nan()) {
|
|
//Serial.printf("ERROR: DCM matrix NAN\n");
|
|
SITL_debug("ERROR: DCM matrix NAN\n");
|
|
renorm_blowup_count++;
|
|
reset(true);
|
|
return;
|
|
}
|
|
// some DCM matrix values can lead to an out of range error in
|
|
// the pitch calculation via asin(). These NaN values can
|
|
// feed back into the rest of the DCM matrix via the
|
|
// error_course value.
|
|
if (!(_dcm_matrix.c.x < 1.0 &&
|
|
_dcm_matrix.c.x > -1.0)) {
|
|
// We have an invalid matrix. Force a normalisation.
|
|
renorm_range_count++;
|
|
normalize();
|
|
|
|
if (_dcm_matrix.is_nan() ||
|
|
fabs(_dcm_matrix.c.x) > 10) {
|
|
// normalisation didn't fix the problem! We're
|
|
// in real trouble. All we can do is reset
|
|
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
|
|
// _dcm_matrix.c.x);
|
|
SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
|
|
_dcm_matrix.c.x);
|
|
renorm_blowup_count++;
|
|
reset(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
// renormalise one vector component of the DCM matrix
|
|
// this will return false if renormalization fails
|
|
bool
|
|
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
|
|
{
|
|
float renorm_val;
|
|
|
|
// numerical errors will slowly build up over time in DCM,
|
|
// causing inaccuracies. We can keep ahead of those errors
|
|
// using the renormalization technique from the DCM IMU paper
|
|
// (see equations 18 to 21).
|
|
|
|
// For APM we don't bother with the taylor expansion
|
|
// optimisation from the paper as on our 2560 CPU the cost of
|
|
// the sqrt() is 44 microseconds, and the small time saving of
|
|
// the taylor expansion is not worth the potential of
|
|
// additional error buildup.
|
|
|
|
// Note that we can get significant renormalisation values
|
|
// when we have a larger delta_t due to a glitch eleswhere in
|
|
// APM, such as a I2c timeout or a set of EEPROM writes. While
|
|
// we would like to avoid these if possible, if it does happen
|
|
// we don't want to compound the error by making DCM less
|
|
// accurate.
|
|
|
|
renorm_val = 1.0 / a.length();
|
|
|
|
// keep the average for reporting
|
|
_renorm_val_sum += renorm_val;
|
|
_renorm_val_count++;
|
|
|
|
if (!(renorm_val < 2.0 && renorm_val > 0.5)) {
|
|
// this is larger than it should get - log it as a warning
|
|
renorm_range_count++;
|
|
if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) {
|
|
// we are getting values which are way out of
|
|
// range, we will reset the matrix and hope we
|
|
// can recover our attitude using drift
|
|
// correction before we hit the ground!
|
|
//Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
|
|
// renorm_val);
|
|
SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n",
|
|
renorm_val);
|
|
renorm_blowup_count++;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
result = a * renorm_val;
|
|
return true;
|
|
}
|
|
|
|
/*************************************************
|
|
Direction Cosine Matrix IMU: Theory
|
|
William Premerlani and Paul Bizard
|
|
|
|
Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
|
|
to approximations rather than identities. In effect, the axes in the two frames of reference no
|
|
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
|
|
simple matter to stay ahead of it.
|
|
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
|
|
*/
|
|
void
|
|
AP_AHRS_DCM::normalize(void)
|
|
{
|
|
float error;
|
|
Vector3f t0, t1, t2;
|
|
|
|
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
|
|
|
|
t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19
|
|
t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19
|
|
t2 = t0 % t1; // c= a x b // eq.20
|
|
|
|
if (!renorm(t0, _dcm_matrix.a) ||
|
|
!renorm(t1, _dcm_matrix.b) ||
|
|
!renorm(t2, _dcm_matrix.c)) {
|
|
// Our solution is blowing up and we will force back
|
|
// to last euler angles
|
|
reset(true);
|
|
}
|
|
}
|
|
|
|
|
|
// produce a yaw error value. The returned value is proportional
|
|
// to sin() of the current heading error in earth frame
|
|
float
|
|
AP_AHRS_DCM::yaw_error_compass(void)
|
|
{
|
|
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
|
|
// get the mag vector in the earth frame
|
|
Vector3f rb = _dcm_matrix * mag;
|
|
|
|
rb.normalize();
|
|
if (rb.is_inf()) {
|
|
// not a valid vector
|
|
return 0.0;
|
|
}
|
|
|
|
// get the earths magnetic field (only X and Y components needed)
|
|
Vector3f mag_earth = Vector3f(cos(_compass->get_declination()),
|
|
sin(_compass->get_declination()), 0);
|
|
|
|
// calculate the error term in earth frame
|
|
Vector3f error = rb % mag_earth;
|
|
|
|
return error.z;
|
|
}
|
|
|
|
// produce a yaw error value using the GPS. The returned value is proportional
|
|
// to sin() of the current heading error in earth frame
|
|
float
|
|
AP_AHRS_DCM::yaw_error_gps(void)
|
|
{
|
|
return sin(ToRad(_gps->ground_course * 0.01) - yaw);
|
|
}
|
|
|
|
|
|
// the _P_gain raises the gain of the PI controller
|
|
// when we are spinning fast. See the fastRotations
|
|
// paper from Bill.
|
|
float
|
|
AP_AHRS_DCM::_P_gain(float spin_rate)
|
|
{
|
|
if (spin_rate < ToDeg(50)) {
|
|
return 1.0;
|
|
}
|
|
if (spin_rate > ToDeg(500)) {
|
|
return 10.0;
|
|
}
|
|
return spin_rate/ToDeg(50);
|
|
}
|
|
|
|
// 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++;
|
|
}
|
|
|
|
|
|
|
|
|
|
// 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 drift correction implementation is based on a paper
|
|
// by Bill Premerlani from here:
|
|
// http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
|
|
void
|
|
AP_AHRS_DCM::drift_correction(float deltat)
|
|
{
|
|
Vector3f velocity;
|
|
uint32_t last_correction_time;
|
|
|
|
// perform yaw drift correction if we have a new yaw reference
|
|
// vector
|
|
drift_correction_yaw();
|
|
|
|
// integrate the accel vector in the earth frame between GPS readings
|
|
_ra_sum += _dcm_matrix * (_accel_vector * deltat);
|
|
|
|
// keep a sum of the deltat values, so we know how much time
|
|
// we have integrated over
|
|
_ra_deltat += deltat;
|
|
|
|
if (_gps == NULL || _gps->status() != GPS::GPS_OK) {
|
|
// no GPS, or no lock. We assume zero velocity. This at
|
|
// least means we can cope with gyro drift while sitting
|
|
// on a bench with no GPS lock
|
|
if (_ra_deltat < 0.1) {
|
|
// not enough time has accumulated
|
|
return;
|
|
}
|
|
velocity.zero();
|
|
_last_velocity.zero();
|
|
last_correction_time = millis();
|
|
_have_gps_lock = false;
|
|
} else {
|
|
if (_gps->last_fix_time == _ra_sum_start) {
|
|
// we don't have a new GPS fix - nothing more to do
|
|
return;
|
|
}
|
|
velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0);
|
|
last_correction_time = _gps->last_fix_time;
|
|
if (_have_gps_lock == false) {
|
|
// if we didn't have GPS lock in the last drift
|
|
// correction interval then set the velocities equal
|
|
_last_velocity = velocity;
|
|
}
|
|
_have_gps_lock = true;
|
|
}
|
|
|
|
#define USE_BAROMETER_FOR_VERTICAL_VELOCITY 1
|
|
#if USE_BAROMETER_FOR_VERTICAL_VELOCITY
|
|
/*
|
|
The barometer for vertical velocity is only enabled if we got
|
|
at least 5 pressure samples for the reading. This ensures we
|
|
don't use very noisy climb rate data
|
|
*/
|
|
if (_barometer != NULL && _barometer->get_pressure_samples() >= 5) {
|
|
// Z velocity is down
|
|
velocity.z = - _barometer->get_climb_rate();
|
|
}
|
|
#endif
|
|
|
|
// see if this is our first time through - in which case we
|
|
// just setup the start times and return
|
|
if (_ra_sum_start == 0) {
|
|
_ra_sum_start = last_correction_time;
|
|
_last_velocity = velocity;
|
|
return;
|
|
}
|
|
|
|
// equation 9: get the corrected acceleration vector in earth frame. Units
|
|
// are m/s/s
|
|
Vector3f GA_e;
|
|
float v_scale = 1.0/(_ra_deltat*_gravity);
|
|
GA_e = Vector3f(0, 0, -1.0) + ((velocity - _last_velocity) * v_scale);
|
|
GA_e.normalize();
|
|
if (GA_e.is_inf()) {
|
|
// wait for some non-zero acceleration information
|
|
return;
|
|
}
|
|
|
|
// calculate the error term in earth frame.
|
|
Vector3f GA_b = _ra_sum / _ra_deltat;
|
|
GA_b.normalize();
|
|
if (GA_b.is_inf()) {
|
|
// wait for some non-zero acceleration information
|
|
return;
|
|
}
|
|
Vector3f error = GA_b % GA_e;
|
|
|
|
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
|
|
#if YAW_INDEPENDENT_DRIFT_CORRECTION
|
|
// step 2 calculate earth_error_Z
|
|
float earth_error_Z = error.z;
|
|
|
|
// equation 10
|
|
float tilt = sqrt(sq(GA_e.x) + sq(GA_e.y));
|
|
|
|
// equation 11
|
|
float theta = atan2(GA_b.y, GA_b.x);
|
|
|
|
// equation 12
|
|
Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z);
|
|
|
|
// step 6
|
|
error = GA_b % GA_e2;
|
|
error.z = earth_error_Z;
|
|
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION
|
|
|
|
// only use the gps/accelerometers for earth frame yaw correction
|
|
// if we are not using a compass. Otherwise we have two competing
|
|
// controllers for yaw correction
|
|
if (_compass && _compass->use_for_yaw()) {
|
|
error.z = 0;
|
|
}
|
|
|
|
// convert the error term to body frame
|
|
error = _dcm_matrix.mul_transpose(error);
|
|
|
|
_error_rp_sum += error.length();
|
|
_error_rp_count++;
|
|
|
|
// base the P gain on the spin rate
|
|
float spin_rate = _omega.length();
|
|
|
|
// 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 * _P_gain(spin_rate) * _kp;
|
|
|
|
// accumulate some integrator error
|
|
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
|
|
_omega_I_sum += error * _ki * _ra_deltat;
|
|
_omega_I_sum_time += _ra_deltat;
|
|
}
|
|
|
|
if (_omega_I_sum_time >= 5) {
|
|
// limit the rate of change of omega_I to the hardware
|
|
// reported maximum gyro drift rate. This ensures that
|
|
// short term errors don't cause a buildup of omega_I
|
|
// beyond the physical limits of the device
|
|
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
|
|
_omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit);
|
|
_omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit);
|
|
_omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit);
|
|
_omega_I += _omega_I_sum;
|
|
_omega_I_sum.zero();
|
|
_omega_I_sum_time = 0;
|
|
}
|
|
|
|
// zero our accumulator ready for the next GPS step
|
|
_ra_sum.zero();
|
|
_ra_deltat = 0;
|
|
_ra_sum_start = last_correction_time;
|
|
|
|
// remember the velocity for next time
|
|
_last_velocity = velocity;
|
|
}
|
|
|
|
|
|
// calculate the euler angles which will be used for high level
|
|
// navigation control
|
|
void
|
|
AP_AHRS_DCM::euler_angles(void)
|
|
{
|
|
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
|
|
|
|
roll_sensor = degrees(roll) * 100;
|
|
pitch_sensor = degrees(pitch) * 100;
|
|
yaw_sensor = degrees(yaw) * 100;
|
|
|
|
if (yaw_sensor < 0)
|
|
yaw_sensor += 36000;
|
|
}
|
|
|
|
/* reporting of DCM state for MAVLink */
|
|
|
|
// average error_roll_pitch since last call
|
|
float AP_AHRS_DCM::get_error_rp(void)
|
|
{
|
|
if (_error_rp_count == 0) {
|
|
// this happens when telemetry is setup on two
|
|
// serial ports
|
|
return _error_rp_last;
|
|
}
|
|
_error_rp_last = _error_rp_sum / _error_rp_count;
|
|
_error_rp_sum = 0;
|
|
_error_rp_count = 0;
|
|
return _error_rp_last;
|
|
}
|
|
|
|
// average error_yaw since last call
|
|
float AP_AHRS_DCM::get_error_yaw(void)
|
|
{
|
|
if (_error_yaw_count == 0) {
|
|
// this happens when telemetry is setup on two
|
|
// serial ports
|
|
return _error_yaw_last;
|
|
}
|
|
_error_yaw_last = _error_yaw_sum / _error_yaw_count;
|
|
_error_yaw_sum = 0;
|
|
_error_yaw_count = 0;
|
|
return _error_yaw_last;
|
|
}
|