From 610609f98daab7ba5b97002379483f734c505abc Mon Sep 17 00:00:00 2001 From: deweibel Date: Mon, 25 Oct 2010 03:02:56 +0000 Subject: [PATCH] Delete obsolete / incomplete DCM library. git-svn-id: https://arducopter.googlecode.com/svn/trunk@730 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/DCM/DCM.cpp | 472 ------------------- libraries/DCM/DCM.h | 147 ------ libraries/DCM/examples/DCM_test/DCM_test.pde | 38 -- 3 files changed, 657 deletions(-) delete mode 100644 libraries/DCM/DCM.cpp delete mode 100644 libraries/DCM/DCM.h delete mode 100644 libraries/DCM/examples/DCM_test/DCM_test.pde diff --git a/libraries/DCM/DCM.cpp b/libraries/DCM/DCM.cpp deleted file mode 100644 index 30bdb6f47a..0000000000 --- a/libraries/DCM/DCM.cpp +++ /dev/null @@ -1,472 +0,0 @@ -#include "DCM.h" - -// XXX HACKS -APM_ADC adc; - -// XXX END HACKS - - -#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer -#define ADC_CONSTRAINT 900 - -#define Kp_ROLLPITCH 0.0014 //0.015 // Pitch&Roll Proportional Gain -#define Ki_ROLLPITCH 0.0000003 // 0.00001 Pitch&Roll Integrator Gain -#define Kp_YAW 1.2 // 1.2 Yaw Porportional Gain -#define Ki_YAW 0.00005 // 0.00005 Yaw Integrator Gain - -// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ -const uint8_t AP_DCM::_sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware -const int AP_DCM::_sensor_signs[] = {1,-1,-1,-1,1,1,-1,-1,-1}; //{-1,1,-1,1,-1,1,-1,-1,-1} !!!! These are probably not right - -// Temp compensation curve constants -// These must be produced by measuring data and curve fitting -// [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants] -const float AP_DCM::_gyro_temp_curve[3][3] = { - {1665,0,0}, - {1665,0,0}, - {1665,0,0} -}; // values may migrate to a Config file - - - -// Constructors //////////////////////////////////////////////////////////////// -AP_DCM::AP_DCM(APM_Compass *withCompass) : - _compass(withCompass), - _dcm_matrix(1, 0, 0, - 0, 1, 0, - 0, 0, 1), - _G_Dt(0.02), - _course_over_ground_x(0), - _course_over_ground_y(1) -{ -} - -void -AP_DCM::update_DCM(void) -{ - read_adc_raw(); // Get current values for IMU sensors - matrix_update(); // Integrate the DCM matrix - normalize(); // Normalize the DCM matrix - drift_correction(); // Perform drift correction - euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation -} - - -// Read the 6 ADC channels needed for the IMU -// ------------------------------------------ -void -AP_DCM::read_adc_raw(void) -{ - int tc_temp = adc.Ch(_gyro_temp_ch); - for (int i = 0; i < 6; i++) { - _adc_in[i] = adc.Ch(_sensors[i]); - if (i < 3) { // XXX magic numbers! - _adc_in[i] -= _gyro_temp_comp(i, tc_temp); // Subtract temp compensated typical gyro bias - } else { - _adc_in[i] -= 2025; // Subtract typical accel bias - } - } -} - -// Returns the temperature compensated raw gyro value -//--------------------------------------------------- -float -AP_DCM::_gyro_temp_comp(int i, int temp) const -{ - // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2 - //------------------------------------------------------------------------ - return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp; -} - -// Returns an analog value with the offset removed -// ----------------- -float -AP_DCM::read_adc(int select) -{ - float temp; - if (_sensor_signs[select] < 0) - temp = (_adc_offset[select] - _adc_in[select]); - else - temp = (_adc_in[select] - _adc_offset[select]); - - if (abs(temp) > ADC_CONSTRAINT) - adc_constraints++; // We keep track of the number of times we constrain the ADC output for performance reporting - -/* -// For checking the pitch/roll drift correction gain time constants -switch (select) { - case 3: - return 0; - break; - case 4: - return 0; - break; - case 5: - return 400; - break; -} -*/ - - -//End of drift correction gain test code - - return constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values -} - -/**************************************************/ -void -AP_DCM::normalize(void) -{ - float error = 0; - DCM_Vector temporary[3]; - - uint8_t problem = 0; - - error = -_dcm_matrix(0).dot_product(_dcm_matrix(1)) * 0.5; // eq.19 - - temporary[0] = _dcm_matrix(1) * error + _dcm_matrix(0); // eq.19 - temporary[1] = _dcm_matrix(0) * error + _dcm_matrix(1); // eq.19 - - temporary[2] = temporary[0] ^ temporary[1]; // c= a x b // eq.20 - - _dcm_matrix(0) = _renorm(temporary[0], problem); - _dcm_matrix(1) = _renorm(temporary[1], problem); - _dcm_matrix(2) = _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(0, 0)= 1.0f; - _dcm_matrix(0, 1)= 0.0f; - _dcm_matrix(0, 2)= 0.0f; - _dcm_matrix(1, 0)= 0.0f; - _dcm_matrix(1, 1)= 1.0f; - _dcm_matrix(1, 2)= 0.0f; - _dcm_matrix(2, 0)= 0.0f; - _dcm_matrix(2, 1)= 0.0f; - _dcm_matrix(2, 2)= 1.0f; - } -} - -DCM_Vector -AP_DCM::_renorm(DCM_Vector const &a, uint8_t &problem) -{ - float renorm; - - renorm = a.dot_product(a); - - if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK with 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::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.magnitude() / GRAVITY; // Scale to gravity. - - // 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 performanc 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 = _accel_vector ^ _dcm_matrix(2); - - // error_roll_pitch are in Accel ADC units - // Limit max error_roll_pitch to limit max omega_P and omega_I - _error_roll_pitch(0) = constrain(_error_roll_pitch(0), -50, 50); - _error_roll_pitch(1) = constrain(_error_roll_pitch(1), -50, 50); - _error_roll_pitch(2) = constrain(_error_roll_pitch(2), -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(0, 0) * _compass->Heading_Y) - (_dcm_matrix(1, 0) * _compass->Heading_X); // Calculating YAW error - } else { - // Use GPS Ground course to correct yaw gyro drift - if (ground_speed >= SPEEDFILT) { - // Optimization: We have precalculated course_over_ground_x and course_over_ground_y (Course over Ground X and Y) from GPS info - error_course = (_dcm_matrix(0, 0) * _course_over_ground_y) - (_dcm_matrix(1, 0) * _course_over_ground_x); // Calculating YAW error - } - } - _error_yaw = _dcm_matrix(2) * error_course; // Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. - - _omega_P += _error_yaw * Kp_YAW; // Adding Proportional. - _omega_I += _error_yaw * Ki_YAW; // adding integrator to the omega_I - - // 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 = sqrt(_omega_I.dot_product(_omega_I)); - if (integrator_magnitude > radians(300)) { - _omega_I *= (0.5f * radians(300) / integrator_magnitude); - } - -} - -/**************************************************/ -void -AP_DCM::_accel_adjust(void) -{ - _accel_vector(1) += accel_scale((ground_speed / 100) * _omega(2)); // Centrifugal force on Acc_y = GPS_speed * GyroZ - _accel_vector(2) -= accel_scale((ground_speed / 100) * _omega(1)); // Centrifugal force on Acc_z = GPS_speed * GyroY -} - - -/**************************************************/ -void -AP_DCM::matrix_update(void) -{ - DCM_Matrix update_matrix; - - _gyro_vector(0) = gyro_scaled_X(read_adc(0)); // gyro x roll - _gyro_vector(1) = gyro_scaled_Y(read_adc(1)); // gyro y pitch - _gyro_vector(2) = gyro_scaled_Z(read_adc(2)); // gyro Z yaw - - //Record when you saturate any of the gyros. - if((abs(_gyro_vector(0)) >= radians(300)) || - (abs(_gyro_vector(1)) >= radians(300)) || - (abs(_gyro_vector(2)) >= radians(300))) - gyro_sat_count++; - -/* -Serial.print (__adc_in[0]); -Serial.print (" "); -Serial.print (_adc_offset[0]); -Serial.print (" "); -Serial.print (_gyro_vector(0)); -Serial.print (" "); -Serial.print (__adc_in[1]); -Serial.print (" "); -Serial.print (_adc_offset[1]); -Serial.print (" "); -Serial.print (_gyro_vector(1)); -Serial.print (" "); -Serial.print (__adc_in[2]); -Serial.print (" "); -Serial.print (_adc_offset[2]); -Serial.print (" "); -Serial.println (_gyro_vector(2)); -*/ - -// _accel_vector(0) = read_adc(3); // acc x -// _accel_vector(1) = read_adc(4); // acc y -// _accel_vector(2) = read_adc(5); // acc z - // Low pass filter on accelerometer data (to filter vibrations) - _accel_vector(0) = _accel_vector(0) * 0.6 + (float)read_adc(3) * 0.4; // acc x - _accel_vector(1) = _accel_vector(1) * 0.6 + (float)read_adc(4) * 0.4; // acc y - _accel_vector(2) = _accel_vector(2) * 0.6 + (float)read_adc(5) * 0.4; // acc z - - _omega = _gyro_vector + _omega_I; // adding proportional term - _omega_vector = _omega + _omega_P; // adding Integrator term - - _accel_adjust(); // Remove centrifugal acceleration. - - #if OUTPUTMODE == 1 - update_matrix(0, 0) = 0; - update_matrix(0, 1) = -_G_Dt * _omega_vector(2); // -z - update_matrix(0, 2) = _G_Dt * _omega_vector(1); // y - update_matrix(1, 0) = _G_Dt * _omega_vector(2); // z - update_matrix(1, 1) = 0; - update_matrix(1, 2) = -_G_Dt * _omega_vector(0); // -x - update_matrix(2, 0) = -_G_Dt * _omega_vector(1); // -y - update_matrix(2, 1) = _G_Dt * _omega_vector(0); // x - update_matrix(2, 2) = 0; - #else // Uncorrected data (no drift correction) - update_matrix(0, 0) = 0; - update_matrix(0, 1) = -_G_Dt * _gyro_vector(2); // -z - update_matrix(0, 2) = _G_Dt * _gyro_vector(1); // y - update_matrix(1, 0) = _G_Dt * _gyro_vector(2); // z - update_matrix(1, 1) = 0; - update_matrix(1, 2) = -_G_Dt * _gyro_vector(0); - update_matrix(2, 0) = -_G_Dt * _gyro_vector(1); - update_matrix(2, 1) = _G_Dt * _gyro_vector(0); - update_matrix(2, 2) = 0; - #endif - - // update - _dcm_matrix += _dcm_matrix * update_matrix; - -/* -Serial.print (_G_Dt * 1000); -Serial.print (" "); -Serial.print (dcm_matrix(0, 0)); -Serial.print (" "); -Serial.print (dcm_matrix(0, 1)); -Serial.print (" "); -Serial.print (dcm_matrix(0, 2)); -Serial.print (" "); -Serial.print (dcm_matrix(1, 0)); -Serial.print (" "); -Serial.print (dcm_matrix(1, 1)); -Serial.print (" "); -Serial.print (dcm_matrix(1, 2)); -Serial.print (" "); -Serial.print (dcm_matrix(2, 0)); -Serial.print (" "); -Serial.print (dcm_matrix(2, 1)); -Serial.print (" "); -Serial.println (dcm_matrix(2, 2)); -*/ -} - -/**************************************************/ -void -AP_DCM::euler_angles(void) -{ - #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes) - roll = atan2(_accel_vector(1), _accel_vector(2)); // atan2(acc_y, acc_z) - roll_sensor = degrees(roll) * 100; - pitch = -asin((_accel_vector(0)) / (double)GRAVITY); // asin(acc_x) - pitch_sensor = degrees(pitch) * 100; - yaw = 0; - #else - pitch = -asin(_dcm_matrix(2, 0)); - pitch_sensor = degrees(pitch) * 100; - roll = atan2(_dcm_matrix(2, 1), _dcm_matrix(2, 2)); - roll_sensor = degrees(roll) * 100; - yaw = atan2(_dcm_matrix(1, 0), _dcm_matrix(0, 0)); - yaw_sensor = degrees(yaw) * 100; - #endif - - /* - Serial.print ("Roll "); - Serial.print (roll_sensor / 100); - Serial.print (", Pitch "); - Serial.print (pitch_sensor / 100); - Serial.print (", Yaw "); - Serial.println (yaw_sensor / 100); - */ -} - -/**************************************************/ -//Computes the dot product of two vectors -float -DCM_Vector::dot_product(DCM_Vector const &vector2) const -{ - float op = 0; - - for(int c = 0; c < 3; c++) - op += _v[c] * vector2(c); - - return op; -} - -// cross-product -DCM_Vector -DCM_Vector::operator^(DCM_Vector const &a) const -{ - DCM_Vector result; - - result(0) = (_v[1] * a(2)) - (_v[2] * a(1)); - result(1) = (_v[2] * a(0)) - (_v[0] * a(2)); - result(2) = (_v[0] * a(1)) - (_v[1] * a(0)); - - return(result); -} - -// scale -DCM_Vector -DCM_Vector::operator*(float scale) const -{ - DCM_Vector result; - - result(0) = _v[0] * scale; - result(1) = _v[1] * scale; - result(2) = _v[2] * scale; - - return(result); -} - -// scale -void -DCM_Vector::operator*=(float scale) -{ - _v[0] *= scale; - _v[1] *= scale; - _v[2] *= scale; -} - -// add -DCM_Vector -DCM_Vector::operator+(DCM_Vector const &a) const -{ - DCM_Vector result; - - result(0) = _v[0] + a(0); - result(1) = _v[1] + a(1); - result(2) = _v[2] + a(2); - - return(result); -} - -// add -void -DCM_Vector::operator+=(DCM_Vector const &a) -{ - _v[0] += a(0); - _v[1] += a(1); - _v[2] += a(2); -} - -// magnitude -float -DCM_Vector::magnitude(void) const -{ - return(sqrt((_v[0] * _v[0]) + - (_v[1] * _v[1]) + - (_v[2] * _v[2]))); -} - -// 3x3 matrix multiply -DCM_Matrix -DCM_Matrix::operator*(DCM_Matrix const &a) const -{ - DCM_Matrix result; - - for (int x = 0; x < 3; x++) { - for (int y = 0; y < 3; y++) { - result(x, y) = - _m[x](0) * a(0, y) + - _m[x](1) * a(1, y) + - _m[x](2) * a(2, y); - } - } - return(result); -} - -// 3x3 matrix add -void -DCM_Matrix::operator+=(DCM_Matrix const &a) -{ - for (int x = 0; x < 3; x++) - for (int y = 0; y < 3; y++) - _m[x](y) += a(x,y); -} - diff --git a/libraries/DCM/DCM.h b/libraries/DCM/DCM.h deleted file mode 100644 index 30b5ddeede..0000000000 --- a/libraries/DCM/DCM.h +++ /dev/null @@ -1,147 +0,0 @@ -#ifndef AP_DCM_h -#define AP_DCM_h - -#include -//#include "WProgram.h" - -//////////////////////////////////////////////////////////////////////////////// -// XXX HACKS -class APM_Compass { -public: - int Heading_X; - int Heading_Y; -}; - -typedef uint8_t byte; - -class APM_ADC { -public: - int Ch(int c) {return ~c;}; -}; - -extern int ground_speed; -extern int pitch; -extern int yaw; -extern int roll; -extern int roll_sensor; -extern int pitch_sensor; -extern int yaw_sensor; -#define SPEEDFILT 100 - -// XXX warning, many of these are nonsense just to make the compiler think -#define abs(_x) (((_x) < 0) ? -(_x) : (_x)) -#define constrain(_x, _min, _max) (((_x) < (_min)) ? (_min) : (((_x) > (_max)) ? (_max) : (_x))) -#define sqrt(_x) ((_x) / 2) // !!! -#define radians(_x) ((_x) / (180 * 3.14)) // !!! shoot me... -#define degrees(_x) ((_x) * (180 / 3.14)) -#define accel_scale(_x) ((_x) * 3) // !!! -#define gyro_scaled_X(_x) ((_x) / 3) -#define gyro_scaled_Y(_x) ((_x) / 3) -#define gyro_scaled_Z(_x) ((_x) / 3) -#define asin(_x) ((_x) * 5) -#define atan2(_x, _y) (((_x) + (_y)) / 5) - -// XXX END HACKS -//////////////////////////////////////////////////////////////////////////////// - -class DCM_Vector { -public: - DCM_Vector(float v0 = 0, float v1 = 0, float v2 = 0); - - // access vector elements with obj(element) - float& operator() (int x) {return _v[x];}; - float operator() (int x) const {return _v[x];}; - - DCM_Vector operator+ (DCM_Vector const &a) const; // add - void operator+= (DCM_Vector const &a); // add - DCM_Vector operator^ (DCM_Vector const &a) const; // cross-product - DCM_Vector operator* (float scale) const; // scale - void operator*= (float scale); // scale - - float dot_product(DCM_Vector const &v2) const; - float magnitude(void) const; - -private: - float _v[3]; -}; - -class DCM_Matrix { -public: - DCM_Matrix(float m00 = 0, float m01 = 0, float m02 = 0, - float m10 = 0, float m11 = 0, float m12 = 0, - float m20 = 0, float m21 = 0, float m22 = 0); - - // access matrix elements with obj(x,y) - float& operator() (int x, int y) {return _m[x](y);}; - float operator() (int x, int y) const {return _m[x](y);}; - - // access matrix columns with obj(x) - DCM_Vector& operator() (int x) {return _m[x];}; - DCM_Vector operator() (int x) const {return _m[x];}; - - // matrix multiply - DCM_Matrix operator* (DCM_Matrix const &a) const; - - // matrix add - void operator+= (DCM_Matrix const &a); - -private: - DCM_Vector _m[3]; -}; - - -class AP_DCM -{ -public: - // Methods - AP_DCM(APM_Compass *withCompass); - void update_DCM(void); //G_Dt - - // XXX these are all private (called by update_DCM only?) - void read_adc_raw(void); - void euler_angles(void); - void matrix_update(void); - void drift_correction(void); - void normalize(void); - float read_adc(int select); - - float imu_health; //Metric based on accel gain deweighting - byte gyro_sat_count; - byte adc_constraints; - byte renorm_sqrt_count; - byte renorm_blowup_count; - -private: - // Methods - void _accel_adjust(void); - float _gyro_temp_comp(int i, int temp) const; - DCM_Vector _renorm(DCM_Vector const &a, uint8_t &problem); - - // members - APM_Compass *_compass; - - DCM_Matrix _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 - float _G_Dt; // Integration time for the gyros (DCM algorithm) - DCM_Vector _accel_vector; // Store the acceleration in a vector - DCM_Vector _gyro_vector; //Store the gyros turn rate in a vector - DCM_Vector _omega_vector; //Corrected Gyro_Vector data - DCM_Vector _omega_P; //Omega Proportional correction - DCM_Vector _omega_I; //Omega Integrator - DCM_Vector _omega; - DCM_Vector _error_roll_pitch; - DCM_Vector _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 diff --git a/libraries/DCM/examples/DCM_test/DCM_test.pde b/libraries/DCM/examples/DCM_test/DCM_test.pde deleted file mode 100644 index c7225d44ae..0000000000 --- a/libraries/DCM/examples/DCM_test/DCM_test.pde +++ /dev/null @@ -1,38 +0,0 @@ -/* - Example of APM_Compass library (HMC5843 sensor). - Code by Jordi MuÒoz and Jose Julio. DIYDrones.com -*/ - -//#include -#include // Compass Library - -unsigned long timer; - -void setup() -{ - DCM.init(); // Initialization - Serial.begin(38400); - Serial.println("Compass library test (HMC5843)"); - delay(1000); - timer = millis(); -} - -void loop() -{ - float tmp; - - if((millis()- timer) > 100){ - timer = millis(); - APM_Compass.Read(); - APM_Compass.Calculate(0, 0); // roll = 0, pitch = 0 for this example - Serial.print("Heading:"); - Serial.print(ToDeg(APM_Compass.Heading)); - Serial.print(" ("); - Serial.print(APM_Compass.Mag_X); - Serial.print(","); - Serial.print(APM_Compass.Mag_Y); - Serial.print(","); - Serial.print(APM_Compass.Mag_Z); - Serial.println(" )"); - } -} \ No newline at end of file