2010-10-30 13:17:16 -03:00
|
|
|
|
/*
|
2012-03-11 04:59:53 -03:00
|
|
|
|
APM_AHRS_DCM.cpp
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AHRS system using DCM matrices
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-11 04:59:53 -03:00
|
|
|
|
Based on DCM code by Doug Weibel, Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
|
2010-10-30 13:17:16 -03:00
|
|
|
|
|
2012-03-11 04:59:53 -03:00
|
|
|
|
Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
|
2010-10-30 13:17:16 -03:00
|
|
|
|
|
2012-03-11 04:59:53 -03:00
|
|
|
|
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.
|
2010-10-30 13:17:16 -03:00
|
|
|
|
*/
|
2012-03-11 04:59:53 -03:00
|
|
|
|
#include <FastSerial.h>
|
|
|
|
|
#include <AP_AHRS.h>
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-02-24 20:30:59 -04:00
|
|
|
|
// this is the speed in cm/s above which we first get a yaw lock with
|
|
|
|
|
// the GPS
|
|
|
|
|
#define GPS_SPEED_MIN 300
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-02-24 20:30:59 -04:00
|
|
|
|
// 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
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// run a full DCM update round
|
2010-10-24 15:37:10 -03:00
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::update(void)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2011-09-15 00:04:08 -03:00
|
|
|
|
float delta_t;
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// tell the IMU to grab some data
|
2010-12-30 03:52:35 -04:00
|
|
|
|
_imu->update();
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// ask the IMU how much time this sensor reading represents
|
2011-09-15 00:04:08 -03:00
|
|
|
|
delta_t = _imu->get_delta_time();
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// Get current values for gyros
|
|
|
|
|
_gyro_vector = _imu->get_gyro();
|
2012-03-11 04:59:53 -03:00
|
|
|
|
_accel_vector = _imu->get_accel();
|
2011-09-11 15:03:55 -03:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// Integrate the DCM matrix using gyro inputs
|
|
|
|
|
matrix_update(delta_t);
|
2011-09-11 15:03:55 -03:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// Normalize the DCM matrix
|
|
|
|
|
normalize();
|
2011-09-11 15:03:55 -03:00
|
|
|
|
|
2012-03-11 04:59:53 -03:00
|
|
|
|
// Perform drift correction
|
|
|
|
|
drift_correction(delta_t);
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// paranoid check for bad values in the DCM matrix
|
|
|
|
|
check_matrix();
|
2010-11-14 22:15:16 -04:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// Calculate pitch, roll, yaw for stabilization and navigation
|
|
|
|
|
euler_angles();
|
2010-11-17 17:20:20 -04:00
|
|
|
|
}
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// update the DCM matrix using only the gyros
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::matrix_update(float _G_Dt)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// _omega_integ_corr is used for _centripetal correction
|
|
|
|
|
// (theoretically better than _omega)
|
2012-03-07 00:09:17 -04:00
|
|
|
|
_omega_integ_corr = _gyro_vector + _omega_I;
|
2012-03-07 02:12:42 -04:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// Equation 16, adding proportional and integral correction terms
|
2012-03-08 03:12:46 -04:00
|
|
|
|
_omega = _omega_integ_corr + _omega_P + _omega_yaw_P;
|
2012-03-07 00:09:17 -04:00
|
|
|
|
|
2012-03-19 03:24:15 -03:00
|
|
|
|
// this is a replacement of the DCM matrix multiply (equation
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// 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
|
2012-03-19 03:24:15 -03:00
|
|
|
|
// two additional local matrices
|
|
|
|
|
|
|
|
|
|
Vector3f r = _omega * _G_Dt;
|
|
|
|
|
_dcm_matrix.rotate(r);
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// adjust an accelerometer vector for known acceleration forces
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::accel_adjust(Vector3f &accel)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2012-03-01 07:52:47 -04:00
|
|
|
|
float veloc;
|
2012-03-07 09:00:26 -04:00
|
|
|
|
// compensate for linear acceleration. This makes a
|
|
|
|
|
// surprisingly large difference in the pitch estimate when
|
|
|
|
|
// turning, plus on takeoff and landing
|
2012-03-07 00:09:17 -04:00
|
|
|
|
float acceleration = _gps->acceleration();
|
|
|
|
|
accel.x -= acceleration;
|
2011-12-06 19:56:16 -04:00
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// compensate for centripetal acceleration
|
2012-03-07 09:00:26 -04:00
|
|
|
|
veloc = _gps->ground_speed * 0.01;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
// We are working with a modified version of equation 26 as
|
|
|
|
|
// our IMU object reports acceleration in the positive axis
|
|
|
|
|
// direction as positive
|
2010-12-01 03:58:04 -04:00
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
// Equation 26 broken up into separate pieces
|
2012-03-11 04:59:53 -03:00
|
|
|
|
accel.y -= _omega_integ_corr.z * veloc;
|
|
|
|
|
accel.z += _omega_integ_corr.y * veloc;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
2011-12-13 06:32:50 -04:00
|
|
|
|
/*
|
|
|
|
|
reset the DCM matrix and omega. Used on ground start, and on
|
|
|
|
|
extreme errors in the matrix
|
|
|
|
|
*/
|
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::reset(bool recover_eulers)
|
2011-12-13 06:32:50 -04:00
|
|
|
|
{
|
2012-01-13 00:48:07 -04:00
|
|
|
|
if (_compass != NULL) {
|
2012-03-11 01:30:49 -04:00
|
|
|
|
_compass->null_offsets_disable();
|
2012-01-13 00:48:07 -04:00
|
|
|
|
}
|
2012-02-23 07:58:41 -04:00
|
|
|
|
|
|
|
|
|
// reset the integration terms
|
2012-03-08 03:12:46 -04:00
|
|
|
|
_omega_I.zero();
|
|
|
|
|
_omega_P.zero();
|
|
|
|
|
_omega_yaw_P.zero();
|
|
|
|
|
_omega_integ_corr.zero();
|
|
|
|
|
_omega.zero();
|
2012-02-22 16:59:16 -04:00
|
|
|
|
|
2012-02-23 07:58:41 -04:00
|
|
|
|
// 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)) {
|
2012-03-10 02:07:07 -04:00
|
|
|
|
_dcm_matrix.from_euler(roll, pitch, yaw);
|
2012-02-23 07:58:41 -04:00
|
|
|
|
} else {
|
|
|
|
|
// otherwise make it flat
|
2012-03-10 02:07:07 -04:00
|
|
|
|
_dcm_matrix.from_euler(0, 0, 0);
|
2012-02-23 07:58:41 -04:00
|
|
|
|
}
|
|
|
|
|
|
2012-01-13 00:48:07 -04:00
|
|
|
|
if (_compass != NULL) {
|
|
|
|
|
_compass->null_offsets_enable(); // This call is needed to restart the nulling
|
|
|
|
|
// Otherwise the reset in the DCM matrix can mess up
|
|
|
|
|
// the nulling
|
|
|
|
|
}
|
2011-12-13 06:32:50 -04:00
|
|
|
|
}
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-02-22 17:00:25 -04:00
|
|
|
|
/*
|
|
|
|
|
check the DCM matrix for pathological values
|
|
|
|
|
*/
|
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::check_matrix(void)
|
2012-02-22 17:00:25 -04:00
|
|
|
|
{
|
2012-02-23 07:58:41 -04:00
|
|
|
|
if (_dcm_matrix.is_nan()) {
|
|
|
|
|
//Serial.printf("ERROR: DCM matrix NAN\n");
|
|
|
|
|
SITL_debug("ERROR: DCM matrix NAN\n");
|
|
|
|
|
renorm_blowup_count++;
|
2012-03-11 04:59:53 -03:00
|
|
|
|
reset(true);
|
2012-02-23 07:58:41 -04:00
|
|
|
|
return;
|
|
|
|
|
}
|
2012-02-22 17:00:25 -04:00
|
|
|
|
// 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.
|
2012-02-22 20:38:51 -04:00
|
|
|
|
if (!(_dcm_matrix.c.x < 1.0 &&
|
|
|
|
|
_dcm_matrix.c.x > -1.0)) {
|
2012-02-22 17:00:25 -04:00
|
|
|
|
// We have an invalid matrix. Force a normalisation.
|
|
|
|
|
renorm_range_count++;
|
|
|
|
|
normalize();
|
2012-02-23 07:58:41 -04:00
|
|
|
|
|
2012-02-23 19:45:47 -04:00
|
|
|
|
if (_dcm_matrix.is_nan() ||
|
2012-02-23 07:58:41 -04:00
|
|
|
|
fabs(_dcm_matrix.c.x) > 10) {
|
2012-02-22 17:00:25 -04:00
|
|
|
|
// normalisation didn't fix the problem! We're
|
|
|
|
|
// in real trouble. All we can do is reset
|
2012-02-23 07:58:41 -04:00
|
|
|
|
//Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
|
|
|
|
|
// _dcm_matrix.c.x);
|
2012-02-22 17:00:25 -04:00
|
|
|
|
SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
|
|
|
|
|
_dcm_matrix.c.x);
|
|
|
|
|
renorm_blowup_count++;
|
2012-03-11 04:59:53 -03:00
|
|
|
|
reset(true);
|
2012-02-22 17:00:25 -04:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// renormalise one vector component of the DCM matrix
|
|
|
|
|
// this will return false if renormalization fails
|
2012-03-07 00:09:17 -04:00
|
|
|
|
bool
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2011-06-15 09:24:51 -03:00
|
|
|
|
float renorm_val;
|
|
|
|
|
|
2012-02-17 01:15:27 -04:00
|
|
|
|
// 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.
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
renorm_val = 1.0 / a.length();
|
2012-02-17 01:15:27 -04:00
|
|
|
|
|
2012-03-01 00:22:39 -04:00
|
|
|
|
// keep the average for reporting
|
|
|
|
|
_renorm_val_sum += renorm_val;
|
|
|
|
|
_renorm_val_count++;
|
|
|
|
|
|
2012-02-22 20:38:51 -04:00
|
|
|
|
if (!(renorm_val < 2.0 && renorm_val > 0.5)) {
|
2012-02-17 01:15:27 -04:00
|
|
|
|
// this is larger than it should get - log it as a warning
|
|
|
|
|
renorm_range_count++;
|
2012-02-22 20:38:51 -04:00
|
|
|
|
if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) {
|
2012-02-17 01:15:27 -04:00
|
|
|
|
// 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!
|
2012-02-23 07:58:41 -04:00
|
|
|
|
//Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
|
|
|
|
|
// renorm_val);
|
2012-02-17 01:15:27 -04:00
|
|
|
|
SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n",
|
|
|
|
|
renorm_val);
|
|
|
|
|
renorm_blowup_count++;
|
2012-03-07 00:09:17 -04:00
|
|
|
|
return false;
|
2012-02-17 01:15:27 -04:00
|
|
|
|
}
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
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 <EFBFBD>renormalization<EFBFBD>.
|
|
|
|
|
*/
|
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::normalize(void)
|
2012-03-07 00:09:17 -04:00
|
|
|
|
{
|
|
|
|
|
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
|
2012-03-11 04:59:53 -03:00
|
|
|
|
reset(true);
|
2012-03-07 00:09:17 -04:00
|
|
|
|
}
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// 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.
|
2012-03-08 03:12:46 -04:00
|
|
|
|
//
|
|
|
|
|
// This function also updates _omega_yaw_P with a yaw correction term
|
|
|
|
|
// from our yaw reference vector
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::drift_correction(float deltat)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2011-12-09 03:42:15 -04:00
|
|
|
|
float error_course = 0;
|
2012-03-02 20:53:31 -04:00
|
|
|
|
Vector3f accel;
|
2012-02-29 22:48:35 -04:00
|
|
|
|
Vector3f error;
|
2012-03-03 07:19:50 -04:00
|
|
|
|
float error_norm = 0;
|
2012-03-01 07:56:42 -04:00
|
|
|
|
const float gravity_squared = (9.80665*9.80665);
|
2012-03-07 00:09:17 -04:00
|
|
|
|
float yaw_deltat = 0;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
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);
|
|
|
|
|
}
|
|
|
|
|
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-10-24 15:37:10 -03:00
|
|
|
|
//*****Roll and Pitch***************
|
|
|
|
|
|
2012-03-01 07:56:42 -04:00
|
|
|
|
// calculate the z component of the accel vector assuming it
|
|
|
|
|
// has a length of 9.8. This discards the z accelerometer
|
|
|
|
|
// sensor reading completely. Logs show that the z accel is
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// the noisest, plus it has a disproportionate impact on the
|
|
|
|
|
// drift correction result because of the geometry when we are
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// mostly flat. Dropping it completely seems to make the DCM
|
|
|
|
|
// algorithm much more resilient to large amounts of
|
|
|
|
|
// accelerometer noise.
|
2012-03-02 20:53:31 -04:00
|
|
|
|
float zsquared = gravity_squared - ((accel.x * accel.x) + (accel.y * accel.y));
|
2012-03-01 07:56:42 -04:00
|
|
|
|
if (zsquared < 0) {
|
2012-03-07 00:09:17 -04:00
|
|
|
|
_omega_P.zero();
|
2012-03-01 07:56:42 -04:00
|
|
|
|
} else {
|
2012-03-02 20:53:31 -04:00
|
|
|
|
if (accel.z > 0) {
|
|
|
|
|
accel.z = sqrt(zsquared);
|
2012-03-01 07:56:42 -04:00
|
|
|
|
} else {
|
2012-03-02 20:53:31 -04:00
|
|
|
|
accel.z = -sqrt(zsquared);
|
2012-03-01 07:56:42 -04:00
|
|
|
|
}
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// calculate the error, in m/2^2, between the attitude
|
|
|
|
|
// implied by the accelerometers and the attitude
|
|
|
|
|
// in the current DCM matrix
|
2012-03-02 20:53:31 -04:00
|
|
|
|
error = _dcm_matrix.c % accel;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// error from the above is in m/s^2 units.
|
|
|
|
|
|
|
|
|
|
// Limit max error to limit the effect of noisy values
|
|
|
|
|
// on the algorithm. This limits the error to about 11
|
|
|
|
|
// degrees
|
2012-03-01 07:56:42 -04:00
|
|
|
|
error_norm = error.length();
|
|
|
|
|
if (error_norm > 2) {
|
|
|
|
|
error *= (2 / error_norm);
|
|
|
|
|
}
|
|
|
|
|
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// we now want to calculate _omega_P and _omega_I. The
|
|
|
|
|
// _omega_P value is what drags us quickly to the
|
|
|
|
|
// accelerometer reading.
|
2012-03-07 00:09:17 -04:00
|
|
|
|
_omega_P = error * _kp_roll_pitch;
|
|
|
|
|
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// the _omega_I is the long term accumulated gyro
|
|
|
|
|
// error. This determines how much gyro drift we can
|
|
|
|
|
// handle.
|
|
|
|
|
Vector3f omega_I_delta = error * (_ki_roll_pitch * deltat);
|
2012-03-07 00:09:17 -04:00
|
|
|
|
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// limit the slope of omega_I on each axis to
|
|
|
|
|
// the maximum drift rate
|
|
|
|
|
float drift_limit = _gyro_drift_limit * deltat;
|
|
|
|
|
omega_I_delta.x = constrain(omega_I_delta.x, -drift_limit, drift_limit);
|
|
|
|
|
omega_I_delta.y = constrain(omega_I_delta.y, -drift_limit, drift_limit);
|
|
|
|
|
omega_I_delta.z = constrain(omega_I_delta.z, -drift_limit, drift_limit);
|
|
|
|
|
|
|
|
|
|
_omega_I += omega_I_delta;
|
2012-03-01 07:56:42 -04:00
|
|
|
|
}
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-01 00:22:39 -04:00
|
|
|
|
// these sums support the reporting of the DCM state via MAVLink
|
|
|
|
|
_error_rp_sum += error_norm;
|
|
|
|
|
_error_rp_count++;
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// yaw drift correction
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// 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
|
2012-03-08 03:12:46 -04:00
|
|
|
|
if (_compass && _compass->use_for_yaw()) {
|
|
|
|
|
if (_compass->last_update != _compass_last_update) {
|
2012-03-07 00:09:17 -04:00
|
|
|
|
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
|
2012-03-08 03:12:46 -04:00
|
|
|
|
if (_have_initial_yaw && yaw_deltat < 2.0) {
|
2012-02-23 20:44:21 -04:00
|
|
|
|
// Equation 23, Calculating YAW error
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// 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();
|
2012-03-10 02:07:07 -04:00
|
|
|
|
_dcm_matrix.from_euler(roll, pitch, _compass->heading);
|
2012-03-08 03:12:46 -04:00
|
|
|
|
_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) {
|
2012-03-07 00:09:17 -04:00
|
|
|
|
yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
|
2012-03-08 03:12:46 -04:00
|
|
|
|
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();
|
|
|
|
|
}
|
2012-03-10 02:07:07 -04:00
|
|
|
|
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
|
2012-03-08 03:12:46 -04:00
|
|
|
|
if (_compass) {
|
|
|
|
|
_compass->null_offsets_enable();
|
|
|
|
|
}
|
|
|
|
|
_have_initial_yaw = true;
|
|
|
|
|
error_course = 0;
|
|
|
|
|
_gps_last_update = _gps->last_fix_time;
|
2012-02-25 01:02:20 -04:00
|
|
|
|
}
|
2012-03-08 03:12:46 -04:00
|
|
|
|
} 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
|
2010-11-02 01:34:49 -03:00
|
|
|
|
error_course = 0;
|
2012-03-08 03:12:46 -04:00
|
|
|
|
_have_initial_yaw = false;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
}
|
|
|
|
|
}
|
2010-12-01 03:58:04 -04:00
|
|
|
|
}
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// 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
|
2012-03-07 00:09:17 -04:00
|
|
|
|
if (yaw_deltat == 0 || error_course == 0) {
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// 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;
|
2012-03-07 00:09:17 -04:00
|
|
|
|
return;
|
|
|
|
|
}
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// 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;
|
|
|
|
|
}
|
|
|
|
|
|
2012-03-07 00:09:17 -04:00
|
|
|
|
// Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// this gives us an error in radians
|
2012-03-07 00:09:17 -04:00
|
|
|
|
error = _dcm_matrix.c * error_course;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// Adding yaw correction to proportional correction vector. We
|
|
|
|
|
// allow the yaw reference source to affect all 3 components
|
2012-03-08 03:12:46 -04:00
|
|
|
|
// of _omega_yaw_P as we need to be able to correctly hold a
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// heading when roll and pitch are non-zero
|
2012-03-08 03:12:46 -04:00
|
|
|
|
_omega_yaw_P = error * _kp_yaw;
|
2012-03-07 00:09:17 -04:00
|
|
|
|
|
|
|
|
|
// add yaw correction to integrator correction vector, but
|
|
|
|
|
// only for the z gyro. We rely on the accelerometers for x
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// 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
|
2012-03-08 03:12:46 -04:00
|
|
|
|
float omega_Iz_delta = error.z * (_ki_yaw * yaw_deltat);
|
|
|
|
|
|
|
|
|
|
// limit the slope of omega_I.z to the maximum gyro drift rate
|
|
|
|
|
float drift_limit = _gyro_drift_limit * yaw_deltat;
|
|
|
|
|
omega_Iz_delta = constrain(omega_Iz_delta, -drift_limit, drift_limit);
|
|
|
|
|
|
|
|
|
|
_omega_I.z += omega_Iz_delta;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// we keep the sum of yaw error for reporting via MAVLink.
|
2012-03-01 00:22:39 -04:00
|
|
|
|
_error_yaw_sum += error_course;
|
|
|
|
|
_error_yaw_count++;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
2012-03-07 02:12:42 -04:00
|
|
|
|
// calculate the euler angles which will be used for high level
|
|
|
|
|
// navigation control
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2012-03-11 04:59:53 -03:00
|
|
|
|
AP_AHRS_DCM::euler_angles(void)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2012-03-10 02:07:07 -04:00
|
|
|
|
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
|
roll_sensor = degrees(roll) * 100;
|
|
|
|
|
pitch_sensor = degrees(pitch) * 100;
|
2012-02-23 07:58:41 -04:00
|
|
|
|
yaw_sensor = degrees(yaw) * 100;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
|
if (yaw_sensor < 0)
|
|
|
|
|
yaw_sensor += 36000;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
2012-03-01 00:22:39 -04:00
|
|
|
|
/* reporting of DCM state for MAVLink */
|
|
|
|
|
|
|
|
|
|
// average error_roll_pitch since last call
|
2012-03-11 04:59:53 -03:00
|
|
|
|
float AP_AHRS_DCM::get_error_rp(void)
|
2012-03-01 00:22:39 -04:00
|
|
|
|
{
|
|
|
|
|
if (_error_rp_count == 0) {
|
2012-03-28 18:57:53 -03:00
|
|
|
|
// this happens when telemetry is setup on two
|
|
|
|
|
// serial ports
|
|
|
|
|
return _error_rp_last;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
}
|
2012-03-28 18:57:53 -03:00
|
|
|
|
_error_rp_last = _error_rp_sum / _error_rp_count;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
_error_rp_sum = 0;
|
|
|
|
|
_error_rp_count = 0;
|
2012-03-28 18:57:53 -03:00
|
|
|
|
return _error_rp_last;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// average error_yaw since last call
|
2012-03-11 04:59:53 -03:00
|
|
|
|
float AP_AHRS_DCM::get_error_yaw(void)
|
2012-03-01 00:22:39 -04:00
|
|
|
|
{
|
|
|
|
|
if (_error_yaw_count == 0) {
|
2012-03-28 18:57:53 -03:00
|
|
|
|
// this happens when telemetry is setup on two
|
|
|
|
|
// serial ports
|
|
|
|
|
return _error_yaw_last;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
}
|
2012-03-28 18:57:53 -03:00
|
|
|
|
_error_yaw_last = _error_yaw_sum / _error_yaw_count;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
_error_yaw_sum = 0;
|
|
|
|
|
_error_yaw_count = 0;
|
2012-03-28 18:57:53 -03:00
|
|
|
|
return _error_yaw_last;
|
2012-03-01 00:22:39 -04:00
|
|
|
|
}
|