DCM Code ported from Arduino for FRDM-KL25Z
Dependents: minimu_data_capture minimu_data_capture
Fork of DCM_AHRS by
minimu9.h
- Committer:
- krmreynolds
- Date:
- 2012-04-23
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
File content as of revision 1:3272ece36ce1:
/* MinIMU-9-Arduino-AHRS Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) Copyright (c) 2011 Pololu Corporation. http://www.pololu.com/ MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: http://code.google.com/p/sf9domahrs/ sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel: http://code.google.com/p/ardu-imu/ MinIMU-9-Arduino-AHRS 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 3 of the License, or (at your option) any later version. MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. */ #ifndef MINIMU9_h #define MINIMU9_h #include "mbed.h" #include "L3G4200D.h" #include "LSM303.h" // LSM303 accelerometer: 8 g sensitivity // 3.8 mg/digit; 1 g = 256 #define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer #define ToRad(x) ((x)*0.01745329252) // *pi/180 #define ToDeg(x) ((x)*57.2957795131) // *180/pi // L3G4200D gyro: 2000 dps full scale // 70 mdps/digit; 1 dps = 0.07 #define Gyro_Gain_X 0.07 //X axis Gyro gain #define Gyro_Gain_Y 0.07 //Y axis Gyro gain #define Gyro_Gain_Z 0.07 //Z axis Gyro gain #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second // LSM303 magnetometer calibration constants; use the Calibrate example from // the Pololu LSM303 library to find the right values for your board #define M_X_MIN -796 #define M_Y_MIN -457 #define M_Z_MIN -424 #define M_X_MAX 197 #define M_Y_MAX 535 #define M_Z_MAX 397 #define Kp_ROLLPITCH 0.02 #define Ki_ROLLPITCH 0.00002 #define Kp_YAW 1.2 #define Ki_YAW 0.00002 /*For debugging purposes*/ //OUTPUTMODE=1 will print the corrected data, //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) #define OUTPUTMODE 1 //#define PRINT_DCM 0 //Will print the whole direction cosine matrix #define PRINT_ANALOGS 0 //Will print the analog raw data #define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw typedef unsigned char byte; class minimu9 { public: minimu9( void ); bool update( void ); float get_roll(); float get_pitch(); float get_yaw(); private: void initialize_values( void ); void read_gyro( void ); void read_accel( void ); void read_compass( void ); void compass_heading( void ); void matrix_update( void ); void normalize( void ); void drift_correction( void ); void euler_angles( void ); void print_data( void ); L3G4200D *gyro; LSM303 *compass; Timer t; float G_Dt; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible long timer; //general purpuse timer long timer_old; long timer24; //Second timer used to print values int AN[6]; //array that stores the gyro and accelerometer data int AN_OFFSET[6]; //Array that stores the Offset of the sensors int SENSOR_SIGN[9]; // Orientation int gyro_x; int gyro_y; int gyro_z; int accel_x; int accel_y; int accel_z; int magnetom_x; int magnetom_y; int magnetom_z; float c_magnetom_x; float c_magnetom_y; float c_magnetom_z; float MAG_Heading; float Accel_Vector[3]; //Store the acceleration in a vector float Gyro_Vector[3]; //Store the gyros turn rate in a vector float Omega_Vector[3]; //Corrected Gyro_Vector data float Omega_P[3]; //Omega Proportional correction float Omega_I[3]; //Omega Integrator float Omega[3]; // Euler angles float roll; float pitch; float yaw; float errorRollPitch[3]; float errorYaw[3]; unsigned int counter; byte gyro_sat; float DCM_Matrix[3][3]; float Update_Matrix[3][3]; //Gyros here float Temporary_Matrix[3][3]; }; #endif