A Team / DCM_AHRS

DCM Code ported from Arduino for FRDM-KL25Z

Dependents:   minimu_data_capture minimu_data_capture

Fork of DCM_AHRS by Kris Reynolds

minimu9.h@2:85214374e094, 2015-01-20 (annotated)

Committer:
ogarai
Date:
Tue Jan 20 02:04:07 2015 +0000
Revision:
2:85214374e094
Parent:
1:3272ece36ce1 
First Commit

Who changed what in which revision?

UserRevisionLine numberNew 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