Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add DCM library 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@725 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
deweibel 2010-10-24 18:37:10 +00:00
parent 271d4736a8
commit 3e30d2566d
2 changed files with 404 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

314
libraries/AP_DCM/AP_DCM_FW.cpp Normal file
View File

@ -0,0 +1,314 @@
#include "AP_DCM_FW.h"
#define OUTPUTMODE 1 // This is just used for debugging, remove later
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
#define Kp_ROLLPITCH 0.0014 // .0014 Pitch&Roll Drift Correction Proportional Gain
#define Ki_ROLLPITCH 0.0000003 // 0.0000003 Pitch&Roll Drift Correction Integrator Gain
#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
#define SPEEDFILT 400 // centimeters/second
#define ADC_CONSTRAINT 900
// Constructors ////////////////////////////////////////////////////////////////
AP_DCM_FW::AP_DCM_FW(GPS *GPS) :
_gps(GPS),
_compass(0),
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1),
_course_over_ground_x(0),
_course_over_ground_y(1)
{
AP_IMU _imu();
}
AP_DCM_FW::AP_DCM_FW(GPS *GPS, APM_Compass_Class *withCompass) :
_gps(GPS),
_compass(withCompass),
_dcm_matrix(1, 0, 0,
0, 1, 0,
0, 0, 1),
_course_over_ground_x(0),
_course_over_ground_y(1)
{
AP_IMU _imu();
}
/**************************************************/
void
AP_DCM_FW::update_DCM(float _G_Dt)
{
_gyro_vector = _imu.get_gyro(); // Get current values for IMU sensors
_accel_vector = _imu.get_accel(); // Get current values for IMU sensors
matrix_update(_G_Dt); // Integrate the DCM matrix
normalize(); // Normalize the DCM matrix
drift_correction(); // Perform drift correction
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
}
/**************************************************/
void
AP_DCM_FW::quick_init(void)
{
_imu.quick_init();
}
/**************************************************/
void
AP_DCM_FW::init(void)
{
_imu.init();
}
/**************************************************/
long
AP_DCM_FW::get_roll_sensor(void)
{ return degrees(roll) * 100;}
/**************************************************/
long
AP_DCM_FW::get_pitch_sensor(void)
{ return degrees(pitch) * 100;}
/**************************************************/
long
AP_DCM_FW::get_yaw_sensor(void)
{ return degrees(yaw) * 100;}
/**************************************************/
float
AP_DCM_FW::get_roll(void)
{ return roll;}
/**************************************************/
float
AP_DCM_FW::get_pitch(void)
{ return pitch;}
/**************************************************/
float
AP_DCM_FW::get_yaw(void)
{ return yaw;}
/**************************************************/
void
AP_DCM_FW::matrix_update(float _G_Dt)
{
Matrix3f _update_matrix;
static int8_t timer;
//Record when you saturate any of the gyros.
if((abs(_gyro_vector.x) >= radians(300)) ||
(abs(_gyro_vector.y) >= radians(300)) ||
(abs(_gyro_vector.z) >= radians(300)))
gyro_sat_count++;
_omega_integ_corr = _gyro_vector + _omega_I; // Used for centrep correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
_accel_adjust(); // Remove centrifugal 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
// update
_dcm_matrix += _dcm_matrix * _update_matrix; // Equation 17
}
/**************************************************/
void
AP_DCM_FW::_accel_adjust(void)
{
Vector3f _veloc, _temp;
float _vel;
_veloc.x = _gps->ground_speed / 100;
//_accel_vector += _omega_integ_corr % _veloc; // Equation 26 This line is giving the compiler a problem so we break it up below
_temp.y = _omega_integ_corr.z * _veloc.x; // only computing the non-zero terms
_temp.z = -1.0f * _omega_integ_corr.y * _veloc.x;
_accel_vector -= _temp;
}
/**************************************************/
void
AP_DCM_FW::normalize(void)
{
float error = 0;
Vector3f temporary[3];
int problem = 0;
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
temporary[0] = _dcm_matrix.b;
temporary[1] = _dcm_matrix.a;
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
_dcm_matrix.a = _renorm(temporary[0], problem);
_dcm_matrix.b = _renorm(temporary[1], problem);
_dcm_matrix.c = _renorm(temporary[2], problem);
if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
_dcm_matrix.a.x = 1.0f;
_dcm_matrix.a.y = 0.0f;
_dcm_matrix.a.z = 0.0f;
_dcm_matrix.b.x = 0.0f;
_dcm_matrix.b.y = 1.0f;
_dcm_matrix.b.z = 0.0f;
_dcm_matrix.c.x = 0.0f;
_dcm_matrix.c.y = 0.0f;
_dcm_matrix.c.z = 1.0f;
}
}
/**************************************************/
Vector3f
AP_DCM_FW::_renorm(Vector3f const &a, int &problem)
{
float renorm;
renorm = a * a;
if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK to use Taylor expansion
renorm = 0.5 * (3 - renorm); // eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1.0 / sqrt(renorm);
renorm_sqrt_count++;
} else {
problem = 1;
renorm_blowup_count++;
}
return(a * renorm);
}
/**************************************************/
void
AP_DCM_FW::drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
float mag_heading_x;
float mag_heading_y;
float error_course = 0;
static float scaled_omega_P[3];
static float scaled_omega_I[3];
float accel_magnitude;
float accel_weight;
float integrator_magnitude;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
accel_magnitude = _accel_vector.length();
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
accel_weight = constrain(1 - 2 * abs(1 - accel_magnitude), 0, 1); //
// We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
imu_health = imu_health + 0.02 * (accel_weight-.5);
imu_health = constrain(imu_health, 0, 1);
// adjust the ground of reference
_error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation???
// error_roll_pitch are in Accel ADC units
// Limit max error_roll_pitch to limit max omega_P and omega_I
_error_roll_pitch.x = constrain(_error_roll_pitch.x, -50, 50);
_error_roll_pitch.y = constrain(_error_roll_pitch.y, -50, 50);
_error_roll_pitch.z = constrain(_error_roll_pitch.z, -50, 50);
_omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight);
_omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight);
//*****YAW***************
if (_compass) {
// 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); // Equation 23, Calculating YAW error
} else {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= SPEEDFILT) {
_course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
_course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
// Optimization: Pass these in as arguments to update so they don't have to be calculated here and the AP code
error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x); // Equation 23, Calculating YAW error
} else {
error_course = 0;
}
}
_error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
_omega_P += _error_yaw * Kp_YAW; // Adding yaw correction to proportional correction vector.
_omega_I += _error_yaw * Ki_YAW; // adding yaw correction to integrator correction vector.
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
integrator_magnitude = _omega_I.length();
if (integrator_magnitude > radians(300)) {
_omega_I *= (0.5f * radians(300) / integrator_magnitude); // Why do we have this discontinuous? EG, why the 0.5?
}
}
/**************************************************/
void
AP_DCM_FW::euler_angles(void)
{
#if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes)
roll = atan2(_accel_vector.y, _accel_vector.z); // atan2(acc_y, acc_z)
pitch = -asin((_accel_vector.x) / (double)GRAVITY); // asin(acc_x)
yaw = 0;
#else
pitch = -asin(_dcm_matrix.c.x);
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
#endif
}
/**************************************************/

90
libraries/AP_DCM/AP_DCM_FW.h Normal file
View File

@ -0,0 +1,90 @@
#ifndef AP_DCM_FW_h
#define AP_DCM_FW_h
#include <FastSerial.h>
#include <AP_Math.h>
#include <inttypes.h>
#include "WProgram.h"
#include <APM_Compass.h>
#include <APM_ADC.h>
#include <AP_GPS.h>
#include "AP_IMU.h"
class AP_DCM_FW
{
public:
// Constructors
AP_DCM_FW(GPS *GPS); // Constructor - no magnetometer
AP_DCM_FW(GPS *GPS, APM_Compass_Class *withCompass); // Constructor for case with magnetometer
// Accessors
long get_roll_sensor(void); // Degrees * 100
long get_pitch_sensor(void); // Degrees * 100
long get_yaw_sensor(void); // Degrees * 100
float get_roll(void); // Radians
float get_pitch(void); // Radians
float get_yaw(void); // Radians
// Methods
void quick_init(void);
void init(void);
void update_DCM(float _G_Dt);
float imu_health; //Metric based on accel gain deweighting
uint8_t gyro_sat_count;
uint8_t adc_constraints;
uint8_t renorm_sqrt_count;
uint8_t renorm_blowup_count;
private:
// Methods
void read_adc_raw(void);
void _accel_adjust(void);
float _gyro_temp_comp(int i, int temp) const;
float read_adc(int select);
void matrix_update(float _G_Dt);
void normalize(void);
Vector3f _renorm(Vector3f const &a, int &problem);
void drift_correction(void);
void euler_angles(void);
// members
APM_Compass_Class *_compass;
GPS *_gps;
AP_IMU _imu;
long roll_sensor; // degrees * 100
long pitch_sensor; // degrees * 100
long yaw_sensor; // degrees * 100
float roll; // radians
float pitch; // radians
float yaw; // radians
Matrix3f _dcm_matrix;
float _adc_in[6]; // array that store the 6 ADC channels used by IMU
float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers
Vector3f _accel_vector; // Store the acceleration in a vector
Vector3f _gyro_vector; //Store the gyros turn rate in a vector
Vector3f _omega_P; //Omega Proportional correction
Vector3f _omega_I; //Omega Integrator correction
Vector3f _omega_integ_corr; //Partially corrected Gyro_Vector data - used for centrepetal correction
Vector3f _omega; //Corrected Gyro_Vector data
Vector3f _error_roll_pitch;
Vector3f _error_yaw;
float _errorCourse;
float _course_over_ground_x; //Course overground X axis
float _course_over_ground_y; //Course overground Y axis
// constants
static const uint8_t _sensors[6];
static const int _sensor_signs[9];
static const uint8_t _gyro_temp_ch = 3; // The ADC channel reading the gyro temperature
static const float _gyro_temp_curve[3][3];
};
#endif