mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-19 14:23:57 -04:00
corrections to imu and dcm libs, addition of vector member to compass class for magnetic vector
git-svn-id: https://arducopter.googlecode.com/svn/trunk@844 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
deweibel
parent
04b893ea43
commit
52ed7fac8c
@ -82,6 +82,9 @@ AP_Compass::update()
|
||||
mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis
|
||||
mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis
|
||||
}
|
||||
mag.x = mag_X;
|
||||
mag.y = mag_Y;
|
||||
mag.z = mag_Z
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@ -2,6 +2,7 @@
|
||||
#define Compass_h
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <AP_Math>
|
||||
|
||||
class Compass
|
||||
{
|
||||
@ -10,6 +11,7 @@ class Compass
|
||||
virtual void update();
|
||||
virtual void calculate(float roll, float pitch);
|
||||
|
||||
Vector3f mag;
|
||||
int16_t mag_X;
|
||||
int16_t mag_Y;
|
||||
int16_t mag_Z;
|
||||
|
||||
@ -82,33 +82,33 @@ AP_DCM_FW::update_DCM(float _G_Dt)
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::quick_init(void)
|
||||
AP_DCM_FW::quick_init(uint16_t *_offset_address)
|
||||
{
|
||||
_imu.quick_init();
|
||||
_imu.quick_init(_offset_address);
|
||||
}
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::init(void)
|
||||
AP_DCM_FW::init(uint16_t *_offset_address)
|
||||
{
|
||||
_imu.init();
|
||||
_imu.init(_offset_address);
|
||||
}
|
||||
|
||||
|
||||
/**************************************************/
|
||||
long
|
||||
AP_DCM_FW::get_roll_sensor(void)
|
||||
{ return degrees(roll) * 100;}
|
||||
{ return degrees(_roll) * 100;}
|
||||
|
||||
/**************************************************/
|
||||
long
|
||||
AP_DCM_FW::get_pitch_sensor(void)
|
||||
{ return degrees(pitch) * 100;}
|
||||
{ return degrees(_pitch) * 100;}
|
||||
|
||||
/**************************************************/
|
||||
long
|
||||
AP_DCM_FW::get_yaw_sensor(void)
|
||||
{
|
||||
long yaw_sensor = degrees(yaw) * 100;
|
||||
long yaw_sensor = degrees(_yaw) * 100;
|
||||
if (yaw_sensor < 0) yaw_sensor += 36000;
|
||||
return yaw_sensor;
|
||||
}
|
||||
@ -116,17 +116,17 @@ AP_DCM_FW::get_yaw_sensor(void)
|
||||
/**************************************************/
|
||||
float
|
||||
AP_DCM_FW::get_roll(void)
|
||||
{ return roll;}
|
||||
{ return _roll;}
|
||||
|
||||
/**************************************************/
|
||||
float
|
||||
AP_DCM_FW::get_pitch(void)
|
||||
{ return pitch;}
|
||||
{ return _pitch;}
|
||||
|
||||
/**************************************************/
|
||||
float
|
||||
AP_DCM_FW::get_yaw(void)
|
||||
{ return yaw;}
|
||||
{ return _yaw;}
|
||||
|
||||
/**************************************************/
|
||||
Vector3f
|
||||
@ -138,12 +138,30 @@ Vector3f
|
||||
AP_DCM_FW::get_accels(void)
|
||||
{ return _accel_vector;}
|
||||
|
||||
/**************************************************/
|
||||
Matrix3f
|
||||
AP_DCM_FW::get_dcm_matrix(void)
|
||||
{ return _dcm_matrix;}
|
||||
|
||||
|
||||
/* For Debugging
|
||||
void
|
||||
printm(const char *l, Matrix3f &m)
|
||||
{ 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);
|
||||
Serial.print(m.c.x, 12); Serial.print(" "); Serial.print(m.c.y, 12); Serial.print(" "); Serial.println(m.c.z, 12);
|
||||
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);
|
||||
}
|
||||
*/
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::matrix_update(float _G_Dt)
|
||||
{
|
||||
Matrix3f _update_matrix;
|
||||
static int8_t timer;
|
||||
Matrix3f update_matrix;
|
||||
|
||||
//Record when you saturate any of the gyros.
|
||||
if((abs(_gyro_vector.x) >= radians(300)) ||
|
||||
@ -154,115 +172,52 @@ AP_DCM_FW::matrix_update(float _G_Dt)
|
||||
_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.
|
||||
|
||||
|
||||
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;
|
||||
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;
|
||||
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
|
||||
|
||||
Serial.println("update matrix before");
|
||||
Serial.println(_update_matrix.a.x);
|
||||
Serial.println(_update_matrix.a.y);
|
||||
Serial.println(_update_matrix.a.z);
|
||||
Serial.println(_update_matrix.b.x);
|
||||
Serial.println(_update_matrix.b.y);
|
||||
Serial.println(_update_matrix.b.z);
|
||||
Serial.println(_update_matrix.c.x);
|
||||
Serial.println(_update_matrix.c.y);
|
||||
Serial.println(_update_matrix.c.z);
|
||||
Serial.println("dcm matrix before");
|
||||
Serial.println(_dcm_matrix.a.x);
|
||||
Serial.println(_dcm_matrix.a.y);
|
||||
Serial.println(_dcm_matrix.a.z);
|
||||
Serial.println(_dcm_matrix.b.x);
|
||||
Serial.println(_dcm_matrix.b.y);
|
||||
Serial.println(_dcm_matrix.b.z);
|
||||
Serial.println(_dcm_matrix.c.x);
|
||||
Serial.println(_dcm_matrix.c.y);
|
||||
Serial.println(_dcm_matrix.c.z);
|
||||
// update
|
||||
_update_matrix = _dcm_matrix * _update_matrix; // Equation 17
|
||||
|
||||
Serial.println("update matrix middle");
|
||||
Serial.println(_update_matrix.a.x,12);
|
||||
Serial.println(_update_matrix.a.y,12);
|
||||
Serial.println(_update_matrix.a.z,12);
|
||||
Serial.println(_update_matrix.b.x,12);
|
||||
Serial.println(_update_matrix.b.y,12);
|
||||
Serial.println(_update_matrix.b.z,12);
|
||||
Serial.println(_update_matrix.c.x,12);
|
||||
Serial.println(_update_matrix.c.y,12);
|
||||
Serial.println(_update_matrix.c.z,12);
|
||||
Serial.println("dcm matrix middle");
|
||||
Serial.println(_dcm_matrix.a.x,12);
|
||||
Serial.println(_dcm_matrix.a.y,12);
|
||||
Serial.println(_dcm_matrix.a.z,12);
|
||||
Serial.println(_dcm_matrix.b.x,12);
|
||||
Serial.println(_dcm_matrix.b.y,12);
|
||||
Serial.println(_dcm_matrix.b.z,12);
|
||||
Serial.println(_dcm_matrix.c.x,12);
|
||||
Serial.println(_dcm_matrix.c.y,12);
|
||||
Serial.println(_dcm_matrix.c.z,12);
|
||||
|
||||
_dcm_matrix = _dcm_matrix + _update_matrix; // Equation 17
|
||||
_dcm_matrix = _dcm_matrix + update_matrix; // Equation 17
|
||||
|
||||
Serial.println("update matrix after");
|
||||
Serial.println(_update_matrix.a.x);
|
||||
Serial.println(_update_matrix.a.y);
|
||||
Serial.println(_update_matrix.a.z);
|
||||
Serial.println(_update_matrix.b.x);
|
||||
Serial.println(_update_matrix.b.y);
|
||||
Serial.println(_update_matrix.b.z);
|
||||
Serial.println(_update_matrix.c.x);
|
||||
Serial.println(_update_matrix.c.y);
|
||||
Serial.println(_update_matrix.c.z);
|
||||
Serial.println("dcm matrix after");
|
||||
Serial.println(_dcm_matrix.a.x);
|
||||
Serial.println(_dcm_matrix.a.y);
|
||||
Serial.println(_dcm_matrix.a.z);
|
||||
Serial.println(_dcm_matrix.b.x);
|
||||
Serial.println(_dcm_matrix.b.y);
|
||||
Serial.println(_dcm_matrix.b.z);
|
||||
Serial.println(_dcm_matrix.c.x);
|
||||
Serial.println(_dcm_matrix.c.y);
|
||||
Serial.println(_dcm_matrix.c.z);
|
||||
}
|
||||
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_DCM_FW::_accel_adjust(void)
|
||||
AP_DCM_FW::accel_adjust(void)
|
||||
{
|
||||
Vector3f _veloc, _temp;
|
||||
float _vel;
|
||||
Vector3f veloc, temp;
|
||||
float vel;
|
||||
|
||||
_veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units
|
||||
veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units
|
||||
|
||||
// We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive
|
||||
|
||||
//_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; // After looking at the compiler issue lets remove _veloc and simlify
|
||||
temp.x = 0;
|
||||
temp.y = _omega_integ_corr.z * veloc.x; // only computing the non-zero terms
|
||||
temp.z = -1.0f * _omega_integ_corr.y * veloc.x; // After looking at the compiler issue lets remove _veloc and simlify
|
||||
|
||||
_accel_vector -= _temp;
|
||||
_accel_vector -= temp;
|
||||
|
||||
}
|
||||
|
||||
@ -285,9 +240,9 @@ AP_DCM_FW::normalize(void)
|
||||
|
||||
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);
|
||||
_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;
|
||||
@ -299,24 +254,12 @@ AP_DCM_FW::normalize(void)
|
||||
_dcm_matrix.c.x = 0.0f;
|
||||
_dcm_matrix.c.y = 0.0f;
|
||||
_dcm_matrix.c.z = 1.0f;
|
||||
Serial.println("Solution blew up");
|
||||
/*
|
||||
Serial.println(_dcm_matrix.a.x);
|
||||
Serial.println(_dcm_matrix.a.y);
|
||||
Serial.println(_dcm_matrix.a.z);
|
||||
Serial.println(_dcm_matrix.b.x);
|
||||
Serial.println(_dcm_matrix.b.y);
|
||||
Serial.println(_dcm_matrix.b.z);
|
||||
Serial.println(_dcm_matrix.c.x);
|
||||
Serial.println(_dcm_matrix.c.y);
|
||||
Serial.println(_dcm_matrix.c.z);
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
Vector3f
|
||||
AP_DCM_FW::_renorm(Vector3f const &a, int &problem)
|
||||
AP_DCM_FW::renorm(Vector3f const &a, int &problem)
|
||||
{
|
||||
float renorm;
|
||||
|
||||
@ -349,7 +292,7 @@ AP_DCM_FW::drift_correction(void)
|
||||
float accel_weight;
|
||||
float integrator_magnitude;
|
||||
static bool in_motion = FALSE;
|
||||
Matrix3f _rot_mat;
|
||||
Matrix3f rot_mat;
|
||||
|
||||
//*****Roll and Pitch***************
|
||||
|
||||
@ -384,48 +327,39 @@ AP_DCM_FW::drift_correction(void)
|
||||
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));
|
||||
Serial.println(_dcm_matrix.a.x);
|
||||
Serial.println(_dcm_matrix.a.y);
|
||||
Serial.println(_dcm_matrix.a.z);
|
||||
Serial.println(_dcm_matrix.b.x);
|
||||
Serial.println(_dcm_matrix.b.y);
|
||||
Serial.println(_dcm_matrix.b.z);
|
||||
Serial.println(_dcm_matrix.c.x);
|
||||
Serial.println(_dcm_matrix.c.y);
|
||||
Serial.println(_dcm_matrix.c.z);
|
||||
_course_over_ground_x = 1;
|
||||
_course_over_ground_y = 0;
|
||||
Serial.print("in motion = ");
|
||||
Serial.print(in_motion);
|
||||
Serial.print("\t");
|
||||
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));
|
||||
if(in_motion) {
|
||||
error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x); // Equation 23, Calculating YAW error
|
||||
} else {
|
||||
Serial.println("in yaw reset");
|
||||
float cos_psi_err, sin_psi_err;
|
||||
|
||||
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
|
||||
cos_psi_err = cos(yaw - ToRad(_gps->ground_course/100.0));
|
||||
sin_psi_err = sin(yaw - ToRad(_gps->ground_course/100.0));
|
||||
_yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
|
||||
cos_psi_err = cos(_yaw - ToRad(_gps->ground_course/100.0));
|
||||
sin_psi_err = sin(_yaw - ToRad(_gps->ground_course/100.0));
|
||||
// Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
|
||||
// Ryx = sin psi err, Ryy = cos psi err, Ryz = 0
|
||||
// Rzx = Rzy = Rzz = 0
|
||||
_rot_mat.a.x = cos_psi_err;
|
||||
_rot_mat.a.y = - sin_psi_err;
|
||||
_rot_mat.b.x = sin_psi_err;
|
||||
_rot_mat.b.y = cos_psi_err;
|
||||
_dcm_matrix = _rot_mat * _dcm_matrix;
|
||||
rot_mat.a.x = cos_psi_err;
|
||||
rot_mat.a.y = - sin_psi_err;
|
||||
rot_mat.b.x = sin_psi_err;
|
||||
rot_mat.b.y = cos_psi_err;
|
||||
rot_mat.a.z = 0;
|
||||
rot_mat.b.z = 0;
|
||||
rot_mat.c.x = 0;
|
||||
rot_mat.c.y = 0;
|
||||
rot_mat.c.z = 0;
|
||||
|
||||
_dcm_matrix = rot_mat * _dcm_matrix;
|
||||
in_motion = TRUE;
|
||||
error_course = 0;
|
||||
|
||||
}
|
||||
/* } else {
|
||||
} else {
|
||||
error_course = 0;
|
||||
in_motion = FALSE;
|
||||
} */
|
||||
}
|
||||
}
|
||||
|
||||
_error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
@ -447,13 +381,13 @@ 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)9.81); // asin(acc_x)
|
||||
yaw = 0;
|
||||
_roll = atan2(_accel_vector.y, _accel_vector.z); // atan2(acc_y, acc_z)
|
||||
_pitch = -asin((_accel_vector.x) / (double)9.81); // 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);
|
||||
_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
|
||||
|
||||
}
|
||||
|
||||
@ -27,10 +27,11 @@ public:
|
||||
float get_yaw(void); // Radians
|
||||
Vector3f get_gyros(void);
|
||||
Vector3f get_accels(void);
|
||||
Matrix3f get_dcm_matrix(void);
|
||||
|
||||
// Methods
|
||||
void quick_init(void);
|
||||
void init(void);
|
||||
void quick_init(uint16_t *_offset_address);
|
||||
void init(uint16_t *_offset_address);
|
||||
void update_DCM(float _G_Dt);
|
||||
|
||||
float imu_health; //Metric based on accel gain deweighting
|
||||
@ -42,12 +43,11 @@ public:
|
||||
private:
|
||||
// Methods
|
||||
void read_adc_raw(void);
|
||||
void _accel_adjust(void);
|
||||
float _gyro_temp_comp(int i, int temp) const;
|
||||
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);
|
||||
Vector3f renorm(Vector3f const &a, int &problem);
|
||||
void drift_correction(void);
|
||||
void euler_angles(void);
|
||||
|
||||
@ -56,13 +56,9 @@ private:
|
||||
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
|
||||
float _roll; // radians
|
||||
float _pitch; // radians
|
||||
float _yaw; // radians
|
||||
|
||||
Matrix3f _dcm_matrix;
|
||||
|
||||
|
||||
@ -64,16 +64,16 @@ AP_IMU::AP_IMU(void)
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_IMU::quick_init(void)
|
||||
AP_IMU::quick_init(uint16_t *_offset_address)
|
||||
{
|
||||
|
||||
// NOTE *** Need to addd code to retrieve values from EEPROM
|
||||
// We read the imu offsets from EEPROM for a quick air restart
|
||||
eeprom_read_block((void*)&_adc_offset, _offset_address, sizeof(_adc_offset));
|
||||
}
|
||||
|
||||
|
||||
/**************************************************/
|
||||
void
|
||||
AP_IMU::init(void)
|
||||
AP_IMU::init(uint16_t *_offset_address)
|
||||
{
|
||||
|
||||
float temp;
|
||||
@ -97,7 +97,7 @@ AP_IMU::init(void)
|
||||
for (int j = 0; j < 6; j++) {
|
||||
_adc_in[j] = APM_ADC.Ch(_sensors[j]);
|
||||
if (j < 3) {
|
||||
_adc_in[j] -= _gyro_temp_comp(j, tc_temp); // Subtract temp compensated typical gyro bias
|
||||
_adc_in[j] -= gyro_temp_comp(j, tc_temp); // Subtract temp compensated typical gyro bias
|
||||
} else {
|
||||
_adc_in[j] -= 2025;
|
||||
}
|
||||
@ -121,7 +121,9 @@ AP_IMU::init(void)
|
||||
|
||||
_adc_offset[5] += GRAVITY * _sensor_signs[5];
|
||||
|
||||
// NOTE *** Need to addd code to save values to EEPROM
|
||||
// Save offset values to EEPROM for use in an air restart
|
||||
eeprom_write_block((const void *)&_adc_offset, _offset_address, sizeof(_adc_offset));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -129,7 +131,7 @@ AP_IMU::init(void)
|
||||
// Returns the temperature compensated raw gyro value
|
||||
//---------------------------------------------------
|
||||
float
|
||||
AP_IMU::_gyro_temp_comp(int i, int temp) const
|
||||
AP_IMU::gyro_temp_comp(int i, int temp) const
|
||||
{
|
||||
// We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2
|
||||
//------------------------------------------------------------------------
|
||||
@ -143,7 +145,7 @@ AP_IMU::get_gyro(void)
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_adc_in[i] = APM_ADC.Ch(_sensors[i]);
|
||||
_adc_in[i] -= _gyro_temp_comp(i,tc_temp); // Subtract temp compensated typical gyro bias
|
||||
_adc_in[i] -= gyro_temp_comp(i,tc_temp); // Subtract temp compensated typical gyro bias
|
||||
if (_sensor_signs[i] < 0)
|
||||
_adc_in[i] = (_adc_offset[i] - _adc_in[i]);
|
||||
else
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#include <inttypes.h>
|
||||
#include "WProgram.h"
|
||||
#include <APM_ADC.h>
|
||||
#include <avr/eeprom.h>
|
||||
|
||||
|
||||
class AP_IMU
|
||||
@ -15,8 +16,8 @@ public:
|
||||
AP_IMU(); // Default Constructor
|
||||
|
||||
// Methods
|
||||
void quick_init(void); // For air start
|
||||
void init(void); // For ground start
|
||||
void quick_init(uint16_t *_offset_address); // For air start
|
||||
void init(uint16_t *_offset_address); // For ground start
|
||||
Vector3f get_gyro(void); // Radians/second
|
||||
Vector3f get_accel(void); // meters/seconds squared
|
||||
|
||||
@ -27,10 +28,11 @@ public:
|
||||
private:
|
||||
// Methods
|
||||
void read_adc_raw(void);
|
||||
float _gyro_temp_comp(int i, int temp) const;
|
||||
float gyro_temp_comp(int i, int temp) const;
|
||||
float read_adc(int select);
|
||||
|
||||
// members
|
||||
//uint16_t _offset_address; // EEPROM start address for saving/retrieving offsets
|
||||
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user