mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
update - See ArducopterMega in Branch for implementation details
git-svn-id: https://arducopter.googlecode.com/svn/trunk@986 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
efa2c199f9
commit
d209f3d59e
@ -10,21 +10,15 @@
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Methods:
|
||||
quick_init() : For air restart
|
||||
init() : For ground start. Calibrates the IMU
|
||||
update_DCM(_G_Dt) : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data
|
||||
get_gyros() : Returns gyro vector corrected for bias
|
||||
get_accels() : Returns accelerometer vector
|
||||
get_gyro() : Returns gyro vector corrected for bias
|
||||
get_accel() : Returns accelerometer vector
|
||||
get_dcm_matrix() : Returns dcm matrix
|
||||
|
||||
*/
|
||||
#include <AP_DCM_FW.h>
|
||||
#include <AP_DCM.h>
|
||||
|
||||
#define OUTPUTMODE 1 // This is just used for debugging, remove later
|
||||
#define TRUE 1
|
||||
#define FALSE 0
|
||||
|
||||
|
||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
|
||||
@ -33,43 +27,19 @@
|
||||
#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
|
||||
#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
|
||||
|
||||
|
||||
#define SPEEDFILT 300 // centimeters/second
|
||||
#define ADC_CONSTRAINT 900
|
||||
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
|
||||
AP_DCM_FW::AP_DCM_FW(AP_ADC * adc, 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),
|
||||
_imu(adc)
|
||||
{
|
||||
}
|
||||
|
||||
AP_DCM_FW::AP_DCM_FW(AP_ADC * adc, 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),
|
||||
_imu(adc)
|
||||
{
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::update_DCM(float _G_Dt)
|
||||
AP_DCM::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
|
||||
_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
|
||||
@ -78,41 +48,26 @@ AP_DCM_FW::update_DCM(float _G_Dt)
|
||||
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
|
||||
}
|
||||
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::quick_init(uint16_t *_offset_address)
|
||||
Vector3f
|
||||
AP_DCM::get_gyro(void)
|
||||
{
|
||||
_imu.quick_init(_offset_address);
|
||||
}
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::gyro_init(uint16_t *_offset_address)
|
||||
{
|
||||
_imu.gyro_init(_offset_address);
|
||||
}
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::init(uint16_t *_offset_address)
|
||||
{
|
||||
_imu.init(_offset_address);
|
||||
}
|
||||
|
||||
return _omega_integ_corr;
|
||||
} // We return the raw gyro vector corrected for bias
|
||||
|
||||
/**************************************************/
|
||||
Vector3f
|
||||
AP_DCM_FW::get_gyros(void)
|
||||
{ return _omega_integ_corr;} // We return the raw gyro vector corrected for bias
|
||||
|
||||
/**************************************************/
|
||||
Vector3f
|
||||
AP_DCM_FW::get_accels(void)
|
||||
{ return _accel_vector;}
|
||||
AP_DCM::get_accel(void)
|
||||
{
|
||||
return _accel_vector;
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
Matrix3f
|
||||
AP_DCM_FW::get_dcm_matrix(void)
|
||||
{ return _dcm_matrix;}
|
||||
AP_DCM::get_dcm_matrix(void)
|
||||
{
|
||||
return _dcm_matrix;
|
||||
}
|
||||
|
||||
|
||||
//For Debugging
|
||||
@ -129,14 +84,9 @@ printm(const char *l, Matrix3f &m)
|
||||
}
|
||||
*/
|
||||
|
||||
/**************************************************/
|
||||
AP_IMU *
|
||||
AP_DCM_FW::get_imu(void)
|
||||
{ return &_imu;}
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::matrix_update(float _G_Dt)
|
||||
AP_DCM::matrix_update(float _G_Dt)
|
||||
{
|
||||
Matrix3f update_matrix;
|
||||
Matrix3f temp_matrix;
|
||||
@ -182,7 +132,7 @@ AP_DCM_FW::matrix_update(float _G_Dt)
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::accel_adjust(void)
|
||||
AP_DCM::accel_adjust(void)
|
||||
{
|
||||
Vector3f veloc, temp;
|
||||
float vel;
|
||||
@ -197,13 +147,12 @@ AP_DCM_FW::accel_adjust(void)
|
||||
temp.z = -1.0f * _omega_integ_corr.y * veloc.x; // After looking at the compiler issue lets remove _veloc and simlify
|
||||
|
||||
_accel_vector -= temp;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::normalize(void)
|
||||
AP_DCM::normalize(void)
|
||||
{
|
||||
float error = 0;
|
||||
Vector3f temporary[3];
|
||||
@ -238,7 +187,7 @@ AP_DCM_FW::normalize(void)
|
||||
|
||||
/**************************************************/
|
||||
Vector3f
|
||||
AP_DCM_FW::renorm(Vector3f const &a, int &problem)
|
||||
AP_DCM::renorm(Vector3f const &a, int &problem)
|
||||
{
|
||||
float renorm;
|
||||
|
||||
@ -259,18 +208,18 @@ AP_DCM_FW::renorm(Vector3f const &a, int &problem)
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::drift_correction(void)
|
||||
AP_DCM::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 error_course;
|
||||
float accel_magnitude;
|
||||
float accel_weight;
|
||||
float integrator_magnitude;
|
||||
static bool in_motion = FALSE;
|
||||
static float scaled_omega_P[3];
|
||||
static float scaled_omega_I[3];
|
||||
static bool in_motion = false;
|
||||
Matrix3f rot_mat;
|
||||
|
||||
//*****Roll and Pitch***************
|
||||
@ -283,8 +232,7 @@ AP_DCM_FW::drift_correction(void)
|
||||
accel_weight = constrain(1 - 2 * fabs(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);
|
||||
_imu->set_health(0.02 * (accel_weight-.5));
|
||||
|
||||
// adjust the ground of reference
|
||||
_error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation???
|
||||
@ -332,13 +280,13 @@ AP_DCM_FW::drift_correction(void)
|
||||
rot_mat.c.z = 1.0;
|
||||
|
||||
_dcm_matrix = rot_mat * _dcm_matrix;
|
||||
in_motion = TRUE;
|
||||
in_motion = true;
|
||||
error_course = 0;
|
||||
|
||||
}
|
||||
} else {
|
||||
error_course = 0;
|
||||
in_motion = FALSE;
|
||||
in_motion = false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -358,7 +306,7 @@ AP_DCM_FW::drift_correction(void)
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::euler_angles(void)
|
||||
AP_DCM::euler_angles(void)
|
||||
{
|
||||
#if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes)
|
||||
roll = atan2(_accel_vector.y, -_accel_vector.z); // atan2(acc_y, acc_z)
|
||||
@ -373,10 +321,12 @@ AP_DCM_FW::euler_angles(void)
|
||||
roll_sensor = degrees(roll) * 100;
|
||||
pitch_sensor = degrees(pitch) * 100;
|
||||
yaw_sensor = degrees(yaw) * 100;
|
||||
if (yaw_sensor < 0) yaw_sensor += 36000;
|
||||
|
||||
if (yaw_sensor < 0)
|
||||
yaw_sensor += 36000;
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
|
||||
|
||||
|
||||
95
libraries/AP_DCM/AP_DCM.h
Normal file
95
libraries/AP_DCM/AP_DCM.h
Normal file
@ -0,0 +1,95 @@
|
||||
#ifndef AP_DCM_h
|
||||
#define AP_DCM_h
|
||||
|
||||
#include <FastSerial.h>
|
||||
#include <AP_Math.h>
|
||||
#include <inttypes.h>
|
||||
#include "WProgram.h"
|
||||
#include <APM_Compass.h>
|
||||
#include <AP_ADC.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <AP_IMU.h>
|
||||
|
||||
|
||||
class AP_DCM
|
||||
{
|
||||
public:
|
||||
// Constructors
|
||||
AP_DCM(AP_IMU *imu, GPS *gps) :
|
||||
_imu(imu),
|
||||
_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_DCM(AP_IMU *imu, GPS *gps, APM_Compass_Class *withCompass) :
|
||||
_imu(imu),
|
||||
_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)
|
||||
{}
|
||||
|
||||
// Accessors
|
||||
Vector3f get_gyro(void);
|
||||
Vector3f get_accel(void);
|
||||
Matrix3f get_dcm_matrix(void);
|
||||
|
||||
// Methods
|
||||
void update_DCM(float _G_Dt);
|
||||
|
||||
long roll_sensor; // Degrees * 100
|
||||
long pitch_sensor; // Degrees * 100
|
||||
long yaw_sensor; // Degrees * 100
|
||||
|
||||
float roll; // Radians
|
||||
float pitch; // Radians
|
||||
float yaw; // Radians
|
||||
|
||||
uint8_t gyro_sat_count;
|
||||
uint8_t renorm_sqrt_count;
|
||||
uint8_t renorm_blowup_count;
|
||||
|
||||
private:
|
||||
// Methods
|
||||
void read_adc_raw(void);
|
||||
void accel_adjust(void);
|
||||
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;
|
||||
|
||||
Matrix3f _dcm_matrix;
|
||||
|
||||
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
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@ -1,79 +0,0 @@
|
||||
#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 <AP_ADC.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <AP_IMU.h>
|
||||
|
||||
|
||||
class AP_DCM_FW
|
||||
{
|
||||
public:
|
||||
// Constructors
|
||||
AP_DCM_FW(AP_ADC * adc, GPS *gps); // Constructor - no magnetometer
|
||||
AP_DCM_FW(AP_ADC * adc, GPS *gps, APM_Compass_Class *withCompass); // Constructor for case with magnetometer
|
||||
|
||||
// Accessors
|
||||
Vector3f get_gyros(void);
|
||||
Vector3f get_accels(void);
|
||||
Matrix3f get_dcm_matrix(void);
|
||||
AP_IMU * get_imu(void);
|
||||
|
||||
// Methods
|
||||
void quick_init(uint16_t *_offset_address);
|
||||
void gyro_init(uint16_t *_offset_address);
|
||||
void init(uint16_t *_offset_address);
|
||||
void update_DCM(float _G_Dt);
|
||||
|
||||
long roll_sensor; // Degrees * 100
|
||||
long pitch_sensor; // Degrees * 100
|
||||
long yaw_sensor; // Degrees * 100
|
||||
float roll; // Radians
|
||||
float pitch; // Radians
|
||||
float yaw; // Radians
|
||||
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 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;
|
||||
|
||||
Matrix3f _dcm_matrix;
|
||||
|
||||
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
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user