AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
minimu9.h@1:3272ece36ce1, 2012-04-23 (annotated)
- Committer:
- krmreynolds
- Date:
- Mon Apr 23 14:31:08 2012 +0000
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
Ported L3G4200D and LSM303 from the Polulu website, dropped the horrible code that someone else built
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
krmreynolds | 1:3272ece36ce1 | 1 | /* |
krmreynolds | 1:3272ece36ce1 | 2 | |
krmreynolds | 1:3272ece36ce1 | 3 | MinIMU-9-Arduino-AHRS |
krmreynolds | 1:3272ece36ce1 | 4 | Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) |
krmreynolds | 1:3272ece36ce1 | 5 | |
krmreynolds | 1:3272ece36ce1 | 6 | Copyright (c) 2011 Pololu Corporation. |
krmreynolds | 1:3272ece36ce1 | 7 | http://www.pololu.com/ |
krmreynolds | 1:3272ece36ce1 | 8 | |
krmreynolds | 1:3272ece36ce1 | 9 | MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: |
krmreynolds | 1:3272ece36ce1 | 10 | http://code.google.com/p/sf9domahrs/ |
krmreynolds | 1:3272ece36ce1 | 11 | |
krmreynolds | 1:3272ece36ce1 | 12 | sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose |
krmreynolds | 1:3272ece36ce1 | 13 | Julio and Doug Weibel: |
krmreynolds | 1:3272ece36ce1 | 14 | http://code.google.com/p/ardu-imu/ |
krmreynolds | 0:dc35364e2291 | 15 | |
krmreynolds | 1:3272ece36ce1 | 16 | MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it |
krmreynolds | 1:3272ece36ce1 | 17 | under the terms of the GNU Lesser General Public License as published by the |
krmreynolds | 1:3272ece36ce1 | 18 | Free Software Foundation, either version 3 of the License, or (at your option) |
krmreynolds | 1:3272ece36ce1 | 19 | any later version. |
krmreynolds | 1:3272ece36ce1 | 20 | |
krmreynolds | 1:3272ece36ce1 | 21 | MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but |
krmreynolds | 1:3272ece36ce1 | 22 | WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
krmreynolds | 1:3272ece36ce1 | 23 | FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for |
krmreynolds | 1:3272ece36ce1 | 24 | more details. |
krmreynolds | 1:3272ece36ce1 | 25 | |
krmreynolds | 1:3272ece36ce1 | 26 | You should have received a copy of the GNU Lesser General Public License along |
krmreynolds | 1:3272ece36ce1 | 27 | with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. |
krmreynolds | 1:3272ece36ce1 | 28 | |
krmreynolds | 1:3272ece36ce1 | 29 | */ |
krmreynolds | 1:3272ece36ce1 | 30 | #ifndef MINIMU9_h |
krmreynolds | 1:3272ece36ce1 | 31 | #define MINIMU9_h |
krmreynolds | 1:3272ece36ce1 | 32 | |
krmreynolds | 1:3272ece36ce1 | 33 | #include "mbed.h" |
krmreynolds | 0:dc35364e2291 | 34 | #include "L3G4200D.h" |
krmreynolds | 0:dc35364e2291 | 35 | #include "LSM303.h" |
krmreynolds | 1:3272ece36ce1 | 36 | |
krmreynolds | 0:dc35364e2291 | 37 | // LSM303 accelerometer: 8 g sensitivity |
krmreynolds | 0:dc35364e2291 | 38 | // 3.8 mg/digit; 1 g = 256 |
krmreynolds | 0:dc35364e2291 | 39 | #define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer |
krmreynolds | 0:dc35364e2291 | 40 | |
krmreynolds | 0:dc35364e2291 | 41 | #define ToRad(x) ((x)*0.01745329252) // *pi/180 |
krmreynolds | 0:dc35364e2291 | 42 | #define ToDeg(x) ((x)*57.2957795131) // *180/pi |
krmreynolds | 0:dc35364e2291 | 43 | |
krmreynolds | 0:dc35364e2291 | 44 | // L3G4200D gyro: 2000 dps full scale |
krmreynolds | 0:dc35364e2291 | 45 | // 70 mdps/digit; 1 dps = 0.07 |
krmreynolds | 0:dc35364e2291 | 46 | #define Gyro_Gain_X 0.07 //X axis Gyro gain |
krmreynolds | 0:dc35364e2291 | 47 | #define Gyro_Gain_Y 0.07 //Y axis Gyro gain |
krmreynolds | 0:dc35364e2291 | 48 | #define Gyro_Gain_Z 0.07 //Z axis Gyro gain |
krmreynolds | 0:dc35364e2291 | 49 | #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second |
krmreynolds | 0:dc35364e2291 | 50 | #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second |
krmreynolds | 0:dc35364e2291 | 51 | #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second |
krmreynolds | 0:dc35364e2291 | 52 | |
krmreynolds | 1:3272ece36ce1 | 53 | // LSM303 magnetometer calibration constants; use the Calibrate example from |
krmreynolds | 1:3272ece36ce1 | 54 | // the Pololu LSM303 library to find the right values for your board |
krmreynolds | 1:3272ece36ce1 | 55 | #define M_X_MIN -796 |
krmreynolds | 1:3272ece36ce1 | 56 | #define M_Y_MIN -457 |
krmreynolds | 1:3272ece36ce1 | 57 | #define M_Z_MIN -424 |
krmreynolds | 1:3272ece36ce1 | 58 | #define M_X_MAX 197 |
krmreynolds | 1:3272ece36ce1 | 59 | #define M_Y_MAX 535 |
krmreynolds | 1:3272ece36ce1 | 60 | #define M_Z_MAX 397 |
krmreynolds | 1:3272ece36ce1 | 61 | |
krmreynolds | 0:dc35364e2291 | 62 | #define Kp_ROLLPITCH 0.02 |
krmreynolds | 0:dc35364e2291 | 63 | #define Ki_ROLLPITCH 0.00002 |
krmreynolds | 0:dc35364e2291 | 64 | #define Kp_YAW 1.2 |
krmreynolds | 0:dc35364e2291 | 65 | #define Ki_YAW 0.00002 |
krmreynolds | 0:dc35364e2291 | 66 | |
krmreynolds | 1:3272ece36ce1 | 67 | /*For debugging purposes*/ |
krmreynolds | 1:3272ece36ce1 | 68 | //OUTPUTMODE=1 will print the corrected data, |
krmreynolds | 1:3272ece36ce1 | 69 | //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) |
krmreynolds | 1:3272ece36ce1 | 70 | #define OUTPUTMODE 1 |
krmreynolds | 1:3272ece36ce1 | 71 | |
krmreynolds | 1:3272ece36ce1 | 72 | //#define PRINT_DCM 0 //Will print the whole direction cosine matrix |
krmreynolds | 1:3272ece36ce1 | 73 | #define PRINT_ANALOGS 0 //Will print the analog raw data |
krmreynolds | 1:3272ece36ce1 | 74 | #define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw |
krmreynolds | 1:3272ece36ce1 | 75 | |
krmreynolds | 0:dc35364e2291 | 76 | typedef unsigned char byte; |
krmreynolds | 0:dc35364e2291 | 77 | |
krmreynolds | 0:dc35364e2291 | 78 | class minimu9 { |
krmreynolds | 0:dc35364e2291 | 79 | public: |
krmreynolds | 1:3272ece36ce1 | 80 | minimu9( void ); |
krmreynolds | 1:3272ece36ce1 | 81 | bool update( void ); |
krmreynolds | 1:3272ece36ce1 | 82 | float get_roll(); |
krmreynolds | 1:3272ece36ce1 | 83 | float get_pitch(); |
krmreynolds | 1:3272ece36ce1 | 84 | float get_yaw(); |
krmreynolds | 0:dc35364e2291 | 85 | |
krmreynolds | 0:dc35364e2291 | 86 | private: |
krmreynolds | 1:3272ece36ce1 | 87 | void initialize_values( void ); |
krmreynolds | 1:3272ece36ce1 | 88 | void read_gyro( void ); |
krmreynolds | 1:3272ece36ce1 | 89 | void read_accel( void ); |
krmreynolds | 1:3272ece36ce1 | 90 | void read_compass( void ); |
krmreynolds | 1:3272ece36ce1 | 91 | void compass_heading( void ); |
krmreynolds | 1:3272ece36ce1 | 92 | void matrix_update( void ); |
krmreynolds | 1:3272ece36ce1 | 93 | void normalize( void ); |
krmreynolds | 1:3272ece36ce1 | 94 | void drift_correction( void ); |
krmreynolds | 1:3272ece36ce1 | 95 | void euler_angles( void ); |
krmreynolds | 1:3272ece36ce1 | 96 | void print_data( void ); |
krmreynolds | 1:3272ece36ce1 | 97 | |
krmreynolds | 0:dc35364e2291 | 98 | L3G4200D *gyro; |
krmreynolds | 1:3272ece36ce1 | 99 | LSM303 *compass; |
krmreynolds | 0:dc35364e2291 | 100 | |
krmreynolds | 0:dc35364e2291 | 101 | Timer t; |
krmreynolds | 0:dc35364e2291 | 102 | |
krmreynolds | 1:3272ece36ce1 | 103 | float G_Dt; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible |
krmreynolds | 1:3272ece36ce1 | 104 | |
krmreynolds | 1:3272ece36ce1 | 105 | long timer; //general purpuse timer |
krmreynolds | 0:dc35364e2291 | 106 | long timer_old; |
krmreynolds | 1:3272ece36ce1 | 107 | long timer24; //Second timer used to print values |
krmreynolds | 1:3272ece36ce1 | 108 | int AN[6]; //array that stores the gyro and accelerometer data |
krmreynolds | 1:3272ece36ce1 | 109 | int AN_OFFSET[6]; //Array that stores the Offset of the sensors |
krmreynolds | 1:3272ece36ce1 | 110 | int SENSOR_SIGN[9]; // Orientation |
krmreynolds | 0:dc35364e2291 | 111 | int gyro_x; |
krmreynolds | 0:dc35364e2291 | 112 | int gyro_y; |
krmreynolds | 0:dc35364e2291 | 113 | int gyro_z; |
krmreynolds | 0:dc35364e2291 | 114 | int accel_x; |
krmreynolds | 0:dc35364e2291 | 115 | int accel_y; |
krmreynolds | 0:dc35364e2291 | 116 | int accel_z; |
krmreynolds | 0:dc35364e2291 | 117 | int magnetom_x; |
krmreynolds | 0:dc35364e2291 | 118 | int magnetom_y; |
krmreynolds | 0:dc35364e2291 | 119 | int magnetom_z; |
krmreynolds | 0:dc35364e2291 | 120 | float c_magnetom_x; |
krmreynolds | 0:dc35364e2291 | 121 | float c_magnetom_y; |
krmreynolds | 0:dc35364e2291 | 122 | float c_magnetom_z; |
krmreynolds | 0:dc35364e2291 | 123 | float MAG_Heading; |
krmreynolds | 0:dc35364e2291 | 124 | |
krmreynolds | 0:dc35364e2291 | 125 | float Accel_Vector[3]; //Store the acceleration in a vector |
krmreynolds | 0:dc35364e2291 | 126 | float Gyro_Vector[3]; //Store the gyros turn rate in a vector |
krmreynolds | 0:dc35364e2291 | 127 | float Omega_Vector[3]; //Corrected Gyro_Vector data |
krmreynolds | 0:dc35364e2291 | 128 | float Omega_P[3]; //Omega Proportional correction |
krmreynolds | 0:dc35364e2291 | 129 | float Omega_I[3]; //Omega Integrator |
krmreynolds | 0:dc35364e2291 | 130 | float Omega[3]; |
krmreynolds | 0:dc35364e2291 | 131 | |
krmreynolds | 0:dc35364e2291 | 132 | // Euler angles |
krmreynolds | 0:dc35364e2291 | 133 | float roll; |
krmreynolds | 0:dc35364e2291 | 134 | float pitch; |
krmreynolds | 0:dc35364e2291 | 135 | float yaw; |
krmreynolds | 0:dc35364e2291 | 136 | |
krmreynolds | 0:dc35364e2291 | 137 | float errorRollPitch[3]; |
krmreynolds | 0:dc35364e2291 | 138 | float errorYaw[3]; |
krmreynolds | 0:dc35364e2291 | 139 | |
krmreynolds | 1:3272ece36ce1 | 140 | unsigned int counter; |
krmreynolds | 0:dc35364e2291 | 141 | byte gyro_sat; |
krmreynolds | 0:dc35364e2291 | 142 | |
krmreynolds | 0:dc35364e2291 | 143 | float DCM_Matrix[3][3]; |
krmreynolds | 1:3272ece36ce1 | 144 | float Update_Matrix[3][3]; //Gyros here |
krmreynolds | 0:dc35364e2291 | 145 | float Temporary_Matrix[3][3]; |
krmreynolds | 1:3272ece36ce1 | 146 | }; |
krmreynolds | 0:dc35364e2291 | 147 | |
krmreynolds | 1:3272ece36ce1 | 148 | #endif |