minimu9.h

00001 /*
00002 
00003 MinIMU-9-Arduino-AHRS
00004 Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
00005 
00006 Copyright (c) 2011 Pololu Corporation.
00007 http://www.pololu.com/
00008 
00009 MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
00010 http://code.google.com/p/sf9domahrs/
00011 
00012 sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
00013 Julio and Doug Weibel:
00014 http://code.google.com/p/ardu-imu/
00015 
00016 MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
00017 under the terms of the GNU Lesser General Public License as published by the
00018 Free Software Foundation, either version 3 of the License, or (at your option)
00019 any later version.
00020 
00021 MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
00022 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00023 FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
00024 more details.
00025 
00026 You should have received a copy of the GNU Lesser General Public License along
00027 with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
00028 
00029 */
00030 #ifndef MINIMU9_h
00031 #define MINIMU9_h
00032 
00033 #include "mbed.h"
00034 #include "L3G4200D.h"
00035 #include "LSM303.h"
00036 
00037 // LSM303 accelerometer: 8 g sensitivity
00038 // 3.8 mg/digit; 1 g = 256
00039 #define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer 
00040 
00041 #define ToRad(x) ((x)*0.01745329252)  // *pi/180
00042 #define ToDeg(x) ((x)*57.2957795131)  // *180/pi
00043 
00044 // L3G4200D gyro: 2000 dps full scale
00045 // 70 mdps/digit; 1 dps = 0.07
00046 #define Gyro_Gain_X 0.07 //X axis Gyro gain
00047 #define Gyro_Gain_Y 0.07 //Y axis Gyro gain
00048 #define Gyro_Gain_Z 0.07 //Z axis Gyro gain
00049 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
00050 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
00051 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
00052 
00053 // LSM303 magnetometer calibration constants; use the Calibrate example from
00054 // the Pololu LSM303 library to find the right values for your board
00055 #define M_X_MIN -796
00056 #define M_Y_MIN -457
00057 #define M_Z_MIN -424
00058 #define M_X_MAX 197
00059 #define M_Y_MAX 535
00060 #define M_Z_MAX 397
00061 
00062 #define Kp_ROLLPITCH 0.02
00063 #define Ki_ROLLPITCH 0.00002
00064 #define Kp_YAW 1.2
00065 #define Ki_YAW 0.00002
00066 
00067 /*For debugging purposes*/
00068 //OUTPUTMODE=1 will print the corrected data,
00069 //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
00070 #define OUTPUTMODE 1
00071 
00072 //#define PRINT_DCM 0     //Will print the whole direction cosine matrix
00073 #define PRINT_ANALOGS 0 //Will print the analog raw data
00074 #define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw
00075 
00076 typedef unsigned char byte;
00077 
00078 class minimu9 {
00079 public:
00080     minimu9( void );
00081     bool update( void );
00082     float get_roll();
00083     float get_pitch();
00084     float get_yaw();
00085 
00086 private:
00087     void initialize_values( void );
00088     void read_gyro( void );
00089     void read_accel( void );
00090     void read_compass( void );
00091     void compass_heading( void );
00092     void matrix_update( void );
00093     void normalize( void );
00094     void drift_correction( void );
00095     void euler_angles( void );
00096     void print_data( void );
00097     
00098     L3G4200D *gyro;
00099     LSM303   *compass;
00100 
00101     Timer t;
00102 
00103     float G_Dt;         // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible
00104 
00105     long timer;         //general purpuse timer
00106     long timer_old;
00107     long timer24;       //Second timer used to print values
00108     int AN[6];          //array that stores the gyro and accelerometer data
00109     int AN_OFFSET[6];   //Array that stores the Offset of the sensors
00110     int SENSOR_SIGN[9]; // Orientation
00111     int gyro_x;
00112     int gyro_y;
00113     int gyro_z;
00114     int accel_x;
00115     int accel_y;
00116     int accel_z;
00117     int magnetom_x;
00118     int magnetom_y;
00119     int magnetom_z;
00120     float c_magnetom_x;
00121     float c_magnetom_y;
00122     float c_magnetom_z;
00123     float MAG_Heading;
00124 
00125     float Accel_Vector[3];  //Store the acceleration in a vector
00126     float Gyro_Vector[3];   //Store the gyros turn rate in a vector
00127     float Omega_Vector[3];  //Corrected Gyro_Vector data
00128     float Omega_P[3];       //Omega Proportional correction
00129     float Omega_I[3];       //Omega Integrator
00130     float Omega[3];
00131 
00132     // Euler angles
00133     float roll;
00134     float pitch;
00135     float yaw;
00136 
00137     float errorRollPitch[3];
00138     float errorYaw[3];
00139 
00140     unsigned int counter;
00141     byte gyro_sat;
00142 
00143     float DCM_Matrix[3][3];
00144     float Update_Matrix[3][3]; //Gyros here
00145     float Temporary_Matrix[3][3];
00146 };
00147 
00148 #endif