2011-12-03 21:56:41 -04:00
|
|
|
|
|
|
|
|
|
#define RADX100 0.000174532925
|
|
|
|
|
#define DEGX100 5729.57795
|
2010-10-30 13:17:16 -03:00
|
|
|
|
/*
|
|
|
|
|
APM_DCM_FW.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
|
|
|
|
|
Code by Doug Weibel, Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2010-10-30 13:17:16 -03:00
|
|
|
|
This library works with the ArduPilot Mega and "Oilpan"
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-10-30 13:17:16 -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.
|
|
|
|
|
|
|
|
|
|
Methods:
|
2011-09-15 00:04:08 -03:00
|
|
|
|
update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data
|
2010-12-01 03:58:04 -04:00
|
|
|
|
get_gyro() : Returns gyro vector corrected for bias
|
|
|
|
|
get_accel() : Returns accelerometer vector
|
2010-11-27 00:30:11 -04:00
|
|
|
|
get_dcm_matrix() : Returns dcm matrix
|
2010-10-30 13:17:16 -03:00
|
|
|
|
|
|
|
|
|
*/
|
2010-12-01 03:58:04 -04:00
|
|
|
|
#include <AP_DCM.h>
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
|
#define OUTPUTMODE 1 // This is just used for debugging, remove later
|
2010-10-24 15:37:10 -03:00
|
|
|
|
#define ToRad(x) (x*0.01745329252) // *pi/180
|
|
|
|
|
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
|
|
|
|
|
2011-06-12 20:49:01 -03:00
|
|
|
|
//#define Kp_ROLLPITCH 0.05967 // .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain
|
|
|
|
|
//#define Ki_ROLLPITCH 0.00001278 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
|
|
|
|
|
//#define Ki_ROLLPITCH 0.0 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
|
|
|
|
|
|
|
|
|
|
//#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
|
2011-07-08 00:57:12 -03:00
|
|
|
|
//#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
|
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
|
|
|
|
|
2010-12-04 02:24:21 -04:00
|
|
|
|
void
|
|
|
|
|
AP_DCM::set_compass(Compass *compass)
|
|
|
|
|
{
|
|
|
|
|
_compass = compass;
|
|
|
|
|
}
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
|
|
|
|
/**************************************************/
|
|
|
|
|
void
|
2011-09-15 00:04:08 -03:00
|
|
|
|
AP_DCM::update_DCM_fast(void)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2011-09-15 00:04:08 -03:00
|
|
|
|
float delta_t;
|
2012-03-01 07:52:47 -04:00
|
|
|
|
Vector3f accel;
|
2011-09-15 00:04:08 -03:00
|
|
|
|
|
2010-12-30 03:52:35 -04:00
|
|
|
|
_imu->update();
|
2010-12-01 03:58:04 -04:00
|
|
|
|
_gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors
|
2012-03-01 07:52:47 -04:00
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
// add the current accel vector into our averaging filter
|
2012-03-01 07:52:47 -04:00
|
|
|
|
accel = _imu->get_accel();
|
2012-03-02 20:53:31 -04:00
|
|
|
|
_accel_sum += accel;
|
|
|
|
|
_accel_sum_count++;
|
|
|
|
|
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2011-09-15 00:04:08 -03:00
|
|
|
|
delta_t = _imu->get_delta_time();
|
|
|
|
|
|
|
|
|
|
matrix_update(delta_t); // Integrate the DCM matrix
|
2011-06-26 19:54:08 -03:00
|
|
|
|
|
2011-09-11 15:03:55 -03:00
|
|
|
|
switch(_toggle++){
|
|
|
|
|
case 0:
|
|
|
|
|
normalize(); // Normalize the DCM matrix
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case 1:
|
|
|
|
|
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case 2:
|
2012-03-02 20:53:31 -04:00
|
|
|
|
_accel_vector = _accel_sum / _accel_sum_count;
|
|
|
|
|
_accel_sum_count = 0;
|
2012-03-03 05:23:23 -04:00
|
|
|
|
_accel_sum.zero();
|
2011-09-11 15:03:55 -03:00
|
|
|
|
drift_correction(); // Normalize the DCM matrix
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case 3:
|
|
|
|
|
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case 4:
|
|
|
|
|
euler_yaw();
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
default:
|
|
|
|
|
euler_rp(); // Calculate pitch, roll, yaw for stabilization and navigation
|
|
|
|
|
_toggle = 0;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**************************************************/
|
|
|
|
|
void
|
2011-09-15 00:04:08 -03:00
|
|
|
|
AP_DCM::update_DCM(void)
|
2011-09-11 15:03:55 -03:00
|
|
|
|
{
|
2011-09-15 00:04:08 -03:00
|
|
|
|
float delta_t;
|
2012-03-01 07:52:47 -04:00
|
|
|
|
Vector3f accel;
|
2011-09-15 00:04:08 -03:00
|
|
|
|
|
2011-09-11 15:03:55 -03:00
|
|
|
|
_imu->update();
|
|
|
|
|
_gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors
|
2012-03-01 07:52:47 -04:00
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
// update_DCM() doesn't do averaging over the accel vectors,
|
|
|
|
|
// just a mild lowpass filter
|
2012-03-01 07:52:47 -04:00
|
|
|
|
accel = _imu->get_accel();
|
2012-03-02 20:53:31 -04:00
|
|
|
|
_accel_vector = (accel * 0.5) + (_accel_vector * 0.5);
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2011-09-15 00:04:08 -03:00
|
|
|
|
delta_t = _imu->get_delta_time();
|
|
|
|
|
|
|
|
|
|
matrix_update(delta_t); // Integrate the DCM matrix
|
2011-09-11 15:03:55 -03:00
|
|
|
|
normalize(); // Normalize the DCM matrix
|
|
|
|
|
drift_correction(); // Perform drift correction
|
2010-10-24 15:37:10 -03:00
|
|
|
|
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
|
|
|
|
|
}
|
|
|
|
|
|
2010-11-02 01:34:49 -03:00
|
|
|
|
/**************************************************/
|
2010-11-14 22:15:16 -04:00
|
|
|
|
|
2010-11-17 17:20:20 -04:00
|
|
|
|
//For Debugging
|
|
|
|
|
/*
|
2010-11-14 22:15:16 -04:00
|
|
|
|
void
|
|
|
|
|
printm(const char *l, Matrix3f &m)
|
2010-11-17 17:20:20 -04:00
|
|
|
|
{ Serial.println(" "); Serial.println(l);
|
|
|
|
|
Serial.print(m.a.x, 12); Serial.print(" "); Serial.print(m.a.y, 12); Serial.print(" "); Serial.println(m.a.z, 12);
|
|
|
|
|
Serial.print(m.b.x, 12); Serial.print(" "); Serial.print(m.b.y, 12); Serial.print(" "); Serial.println(m.b.z, 12);
|
2011-06-12 20:49:01 -03:00
|
|
|
|
Serial.print(m.c.x, 12); Serial.print(" "); Serial.print(m.c.y, 12); Serial.print(" "); Serial.println(m.c.z, 12);
|
2010-11-17 17:20:20 -04:00
|
|
|
|
Serial.print(*(uint32_t *)&(m.a.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.a.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.a.z), HEX);
|
|
|
|
|
Serial.print(*(uint32_t *)&(m.b.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.b.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.b.z), HEX);
|
|
|
|
|
Serial.print(*(uint32_t *)&(m.c.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.c.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.c.z), HEX);
|
|
|
|
|
}
|
|
|
|
|
*/
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-10-24 15:37:10 -03:00
|
|
|
|
/**************************************************/
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2010-12-01 03:58:04 -04:00
|
|
|
|
AP_DCM::matrix_update(float _G_Dt)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2010-11-14 22:15:16 -04:00
|
|
|
|
Matrix3f update_matrix;
|
2010-11-17 17:20:20 -04:00
|
|
|
|
Matrix3f temp_matrix;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-12-02 18:09:25 -04:00
|
|
|
|
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega)
|
2012-03-01 07:52:47 -04:00
|
|
|
|
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
|
|
|
|
|
_omega_smoothed = (_omega_smoothed * 0.5) + (_omega_integ_corr * 0.5);
|
2010-12-01 18:52:11 -04:00
|
|
|
|
|
2011-06-12 20:49:01 -03:00
|
|
|
|
#if OUTPUTMODE == 1
|
2011-06-26 19:54:08 -03:00
|
|
|
|
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
|
|
|
|
|
|
2010-11-14 22:15:16 -04:00
|
|
|
|
update_matrix.a.x = 0;
|
|
|
|
|
update_matrix.b.y = 0;
|
|
|
|
|
update_matrix.c.z = 0;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
#else // Uncorrected data (no drift correction)
|
2010-11-14 22:15:16 -04:00
|
|
|
|
update_matrix.a.x = 0;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
update_matrix.a.y = -_G_Dt * _gyro_vector.z;
|
2010-11-14 22:15:16 -04:00
|
|
|
|
update_matrix.a.z = _G_Dt * _gyro_vector.y;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
update_matrix.b.x = _G_Dt * _gyro_vector.z;
|
2010-11-14 22:15:16 -04:00
|
|
|
|
update_matrix.b.y = 0;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
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;
|
2010-11-14 22:15:16 -04:00
|
|
|
|
update_matrix.c.z = 0;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
#endif
|
2010-11-17 17:20:20 -04:00
|
|
|
|
|
|
|
|
|
temp_matrix = _dcm_matrix * update_matrix;
|
|
|
|
|
_dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
// adjust an accelerometer vector for centripetal force
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2012-03-02 20:53:31 -04:00
|
|
|
|
AP_DCM::accel_adjust(Vector3f &accel)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2012-03-01 07:52:47 -04:00
|
|
|
|
float veloc;
|
2011-12-06 19:56:16 -04:00
|
|
|
|
|
2012-03-01 07:52:47 -04:00
|
|
|
|
veloc = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units
|
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
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
accel.y -= _omega_smoothed.z * veloc;
|
|
|
|
|
accel.z += _omega_smoothed.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-02-23 07:58:41 -04:00
|
|
|
|
AP_DCM::matrix_reset(bool recover_eulers)
|
2011-12-13 06:32:50 -04:00
|
|
|
|
{
|
2012-01-13 00:48:07 -04:00
|
|
|
|
if (_compass != NULL) {
|
|
|
|
|
_compass->null_offsets_disable();
|
|
|
|
|
}
|
2012-02-23 07:58:41 -04:00
|
|
|
|
|
|
|
|
|
// reset the integration terms
|
2011-12-13 06:32:50 -04:00
|
|
|
|
_omega_I.x = 0.0f;
|
|
|
|
|
_omega_I.y = 0.0f;
|
|
|
|
|
_omega_I.z = 0.0f;
|
2012-02-22 16:59:16 -04:00
|
|
|
|
_omega_P = _omega_I;
|
|
|
|
|
_omega_integ_corr = _omega_I;
|
2012-03-01 07:52:47 -04:00
|
|
|
|
_omega_smoothed = _omega_I;
|
2012-02-22 16:59:16 -04:00
|
|
|
|
_omega = _omega_I;
|
|
|
|
|
|
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-02-23 20:23:49 -04:00
|
|
|
|
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, yaw);
|
2012-02-23 07:58:41 -04:00
|
|
|
|
} else {
|
|
|
|
|
// otherwise make it flat
|
2012-02-23 20:23:49 -04:00
|
|
|
|
rotation_matrix_from_euler(_dcm_matrix, 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
|
|
|
|
|
AP_DCM::check_matrix(void)
|
|
|
|
|
{
|
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++;
|
|
|
|
|
matrix_reset(true);
|
|
|
|
|
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-02-23 07:58:41 -04:00
|
|
|
|
matrix_reset(true);
|
2012-02-22 17:00:25 -04:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2010-12-01 15:53:40 -04:00
|
|
|
|
/*************************************************
|
|
|
|
|
Direction Cosine Matrix IMU: Theory
|
|
|
|
|
William Premerlani and Paul Bizard
|
|
|
|
|
|
2011-06-12 20:49:01 -03:00
|
|
|
|
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
|
2010-12-01 15:53:40 -04:00
|
|
|
|
simple matter to stay ahead of it.
|
|
|
|
|
We call the process of enforcing the orthogonality conditions <EFBFBD>renormalization<EFBFBD>.
|
|
|
|
|
*/
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2010-12-01 03:58:04 -04:00
|
|
|
|
AP_DCM::normalize(void)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
|
|
|
|
float error = 0;
|
|
|
|
|
Vector3f temporary[3];
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-10-24 15:37:10 -03:00
|
|
|
|
int problem = 0;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-10-24 15:37:10 -03:00
|
|
|
|
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
|
|
|
|
|
|
2011-06-12 20:49:01 -03:00
|
|
|
|
temporary[0] = _dcm_matrix.b;
|
|
|
|
|
temporary[1] = _dcm_matrix.a;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error)); // eq.19
|
|
|
|
|
temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error)); // eq.19
|
|
|
|
|
|
|
|
|
|
temporary[2] = temporary[0] % temporary[1]; // c= a x b // eq.20
|
|
|
|
|
|
2010-11-14 22:15:16 -04:00
|
|
|
|
_dcm_matrix.a = renorm(temporary[0], problem);
|
|
|
|
|
_dcm_matrix.b = renorm(temporary[1], problem);
|
|
|
|
|
_dcm_matrix.c = renorm(temporary[2], problem);
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-10-24 15:37:10 -03:00
|
|
|
|
if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
|
2012-02-23 07:58:41 -04:00
|
|
|
|
matrix_reset(true);
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**************************************************/
|
|
|
|
|
Vector3f
|
2010-12-01 03:58:04 -04:00
|
|
|
|
AP_DCM::renorm(Vector3f const &a, int &problem)
|
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.
|
|
|
|
|
|
|
|
|
|
renorm_val = 1.0 / sqrt(a * a);
|
|
|
|
|
|
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!
|
|
|
|
|
problem = 1;
|
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++;
|
|
|
|
|
}
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
2012-02-17 01:15:27 -04:00
|
|
|
|
return (a * renorm_val);
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**************************************************/
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2010-12-01 03:58:04 -04:00
|
|
|
|
AP_DCM::drift_correction(void)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2011-12-09 03:42:15 -04:00
|
|
|
|
float error_course = 0;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
float accel_weight;
|
|
|
|
|
float integrator_magnitude;
|
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-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
|
|
|
|
|
// the noisest, and it seems that using just the x and y accel
|
|
|
|
|
// values gives a better attitude estimate than including the
|
|
|
|
|
// z accel
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
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) {
|
|
|
|
|
accel_weight = 0;
|
|
|
|
|
} 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-01 07:56:42 -04:00
|
|
|
|
// this is arbitrary, and can be removed once we get
|
|
|
|
|
// ki and kp right
|
|
|
|
|
accel_weight = 0.6;
|
2010-12-01 18:52:11 -04:00
|
|
|
|
|
2012-03-01 07:56:42 -04:00
|
|
|
|
_health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1);
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-02 20:53:31 -04:00
|
|
|
|
error = _dcm_matrix.c % accel;
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2012-03-01 07:56:42 -04:00
|
|
|
|
// error_roll_pitch are in Accel m/s^2 units
|
|
|
|
|
// Limit max error_roll_pitch to limit max omega_P and omega_I
|
|
|
|
|
error_norm = error.length();
|
|
|
|
|
if (error_norm > 2) {
|
|
|
|
|
error *= (2 / error_norm);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_omega_P = error * (_kp_roll_pitch * accel_weight);
|
|
|
|
|
_omega_I += error * (_ki_roll_pitch * accel_weight);
|
|
|
|
|
}
|
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
|
|
|
|
|
_accel_weight_sum += accel_weight;
|
|
|
|
|
_accel_weight_count++;
|
|
|
|
|
_error_rp_sum += error_norm;
|
|
|
|
|
_error_rp_count++;
|
|
|
|
|
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
|
|
|
|
//*****YAW***************
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-02-24 23:11:31 -04:00
|
|
|
|
if (_compass && _compass->use_for_yaw()) {
|
2012-02-23 20:44:21 -04:00
|
|
|
|
if (_have_initial_yaw) {
|
2012-02-23 20:30:09 -04:00
|
|
|
|
// 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);
|
|
|
|
|
} else {
|
|
|
|
|
// this is our first estimate of the yaw,
|
|
|
|
|
// construct a DCM matrix based on the current
|
|
|
|
|
// roll/pitch and the compass heading, but
|
|
|
|
|
|
|
|
|
|
// first ensure the compass heading has been
|
|
|
|
|
// calculated
|
|
|
|
|
_compass->calculate(_dcm_matrix);
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-02-23 20:30:09 -04:00
|
|
|
|
// now construct a new DCM matrix
|
2012-02-25 01:02:20 -04:00
|
|
|
|
_compass->null_offsets_disable();
|
2012-02-23 20:30:09 -04:00
|
|
|
|
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading);
|
2012-02-25 01:02:20 -04:00
|
|
|
|
_compass->null_offsets_enable();
|
2012-02-23 20:44:21 -04:00
|
|
|
|
_have_initial_yaw = true;
|
2012-02-23 20:30:09 -04:00
|
|
|
|
}
|
|
|
|
|
} else if (_gps && _gps->status() == GPS::GPS_OK) {
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2010-10-24 15:37:10 -03:00
|
|
|
|
// Use GPS Ground course to correct yaw gyro drift
|
2012-02-24 20:30:59 -04:00
|
|
|
|
if (_gps->ground_speed >= GPS_SPEED_MIN) {
|
2012-02-23 20:44:21 -04:00
|
|
|
|
if (_have_initial_yaw) {
|
|
|
|
|
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);
|
2010-11-02 01:34:49 -03:00
|
|
|
|
} else {
|
2012-02-23 20:30:09 -04:00
|
|
|
|
// when we first start moving, set the
|
|
|
|
|
// DCM matrix to the current
|
|
|
|
|
// roll/pitch values, but with yaw
|
|
|
|
|
// from the GPS
|
2012-02-26 07:23:29 -04:00
|
|
|
|
if (_compass) {
|
2012-02-25 01:02:20 -04:00
|
|
|
|
_compass->null_offsets_disable();
|
|
|
|
|
}
|
2012-02-23 20:30:09 -04:00
|
|
|
|
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course));
|
2012-02-26 07:23:29 -04:00
|
|
|
|
if (_compass) {
|
2012-02-25 01:02:20 -04:00
|
|
|
|
_compass->null_offsets_enable();
|
|
|
|
|
}
|
2012-02-23 20:44:21 -04:00
|
|
|
|
_have_initial_yaw = true;
|
2010-11-02 01:34:49 -03:00
|
|
|
|
error_course = 0;
|
2011-06-12 20:49:01 -03:00
|
|
|
|
}
|
2012-02-24 20:30:59 -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;
|
2010-11-14 22:15:16 -04:00
|
|
|
|
} else {
|
2012-02-24 20:30:59 -04:00
|
|
|
|
// we are moving very slowly. Reset
|
|
|
|
|
// _have_initial_yaw and adjust our heading
|
|
|
|
|
// rapidly next time we get a good GPS ground
|
|
|
|
|
// speed
|
2010-10-24 15:37:10 -03:00
|
|
|
|
error_course = 0;
|
2012-02-23 20:44:21 -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-02-29 22:48:35 -04:00
|
|
|
|
error = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
2011-06-12 20:49:01 -03:00
|
|
|
|
|
2012-02-29 22:48:35 -04:00
|
|
|
|
_omega_P += error * _kp_yaw; // Adding yaw correction to proportional correction vector.
|
|
|
|
|
_omega_I += error * _ki_yaw; // adding yaw correction to integrator correction vector.
|
2010-10-24 15:37:10 -03:00
|
|
|
|
|
2011-11-05 12:01:20 -03:00
|
|
|
|
// Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second
|
2010-10-24 15:37:10 -03:00
|
|
|
|
integrator_magnitude = _omega_I.length();
|
2011-11-05 12:01:20 -03:00
|
|
|
|
if (integrator_magnitude > radians(30)) {
|
2011-12-03 21:56:41 -04:00
|
|
|
|
_omega_I *= (radians(30) / integrator_magnitude);
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
2012-03-01 00:22:39 -04:00
|
|
|
|
|
|
|
|
|
_error_yaw_sum += error_course;
|
|
|
|
|
_error_yaw_count++;
|
|
|
|
|
|
2010-12-02 18:09:25 -04:00
|
|
|
|
//Serial.print("*");
|
2010-10-24 15:37:10 -03:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**************************************************/
|
2011-06-12 20:49:01 -03:00
|
|
|
|
void
|
2010-12-01 03:58:04 -04:00
|
|
|
|
AP_DCM::euler_angles(void)
|
2010-10-24 15:37:10 -03:00
|
|
|
|
{
|
2012-02-22 17:00:25 -04:00
|
|
|
|
check_matrix();
|
|
|
|
|
|
2012-02-23 20:32:20 -04:00
|
|
|
|
calculate_euler_angles(_dcm_matrix, &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
|
|
|
|
}
|
|
|
|
|
|
2011-09-11 15:03:55 -03:00
|
|
|
|
void
|
|
|
|
|
AP_DCM::euler_rp(void)
|
|
|
|
|
{
|
2012-02-22 17:00:25 -04:00
|
|
|
|
check_matrix();
|
2012-02-23 20:32:20 -04:00
|
|
|
|
calculate_euler_angles(_dcm_matrix, &roll, &pitch, NULL);
|
2011-12-03 21:56:41 -04:00
|
|
|
|
roll_sensor = roll * DEGX100; //degrees(roll) * 100;
|
|
|
|
|
pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100;
|
2011-09-11 15:03:55 -03:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
AP_DCM::euler_yaw(void)
|
|
|
|
|
{
|
2012-02-23 20:32:20 -04:00
|
|
|
|
calculate_euler_angles(_dcm_matrix, NULL, NULL, &yaw);
|
2011-12-03 21:56:41 -04:00
|
|
|
|
yaw_sensor = yaw * DEGX100; //degrees(yaw) * 100;
|
2011-09-11 15:03:55 -03:00
|
|
|
|
|
|
|
|
|
if (yaw_sensor < 0)
|
|
|
|
|
yaw_sensor += 36000;
|
|
|
|
|
}
|
2012-03-01 00:22:39 -04:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* reporting of DCM state for MAVLink */
|
|
|
|
|
|
|
|
|
|
// average accel_weight since last call
|
|
|
|
|
float AP_DCM::get_accel_weight(void)
|
|
|
|
|
{
|
|
|
|
|
float ret;
|
|
|
|
|
if (_accel_weight_count == 0) {
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
ret = _accel_weight_sum / _accel_weight_count;
|
|
|
|
|
_accel_weight_sum = 0;
|
|
|
|
|
_accel_weight_count = 0;
|
|
|
|
|
return ret;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// average renorm_val since last call
|
|
|
|
|
float AP_DCM::get_renorm_val(void)
|
|
|
|
|
{
|
|
|
|
|
float ret;
|
|
|
|
|
if (_renorm_val_count == 0) {
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
ret = _renorm_val_sum / _renorm_val_count;
|
|
|
|
|
_renorm_val_sum = 0;
|
|
|
|
|
_renorm_val_count = 0;
|
|
|
|
|
return ret;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// average error_roll_pitch since last call
|
|
|
|
|
float AP_DCM::get_error_rp(void)
|
|
|
|
|
{
|
|
|
|
|
float ret;
|
|
|
|
|
if (_error_rp_count == 0) {
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
ret = _error_rp_sum / _error_rp_count;
|
|
|
|
|
_error_rp_sum = 0;
|
|
|
|
|
_error_rp_count = 0;
|
|
|
|
|
return ret;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// average error_yaw since last call
|
|
|
|
|
float AP_DCM::get_error_yaw(void)
|
|
|
|
|
{
|
|
|
|
|
float ret;
|
|
|
|
|
if (_error_yaw_count == 0) {
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
ret = _error_yaw_sum / _error_yaw_count;
|
|
|
|
|
_error_yaw_sum = 0;
|
|
|
|
|
_error_yaw_count = 0;
|
|
|
|
|
return ret;
|
|
|
|
|
}
|