Kris Reynolds / DCM_AHRS

AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor

minimu9.h@0:dc35364e2291, 2012-04-12 (annotated)

Committer:
krmreynolds
Date:
Thu Apr 12 13:47:23 2012 +0000
Revision:
0:dc35364e2291
Child:
1:3272ece36ce1
Added GNU Agreement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krmreynolds 0:dc35364e2291 1 /* mbed L3G4200D Library version 0.1
krmreynolds 0:dc35364e2291 2 * Copyright (c) 2012 Prediluted
krmreynolds 0:dc35364e2291 3 *
krmreynolds 0:dc35364e2291 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
krmreynolds 0:dc35364e2291 5 * of this software and associated documentation files (the "Software"), to deal
krmreynolds 0:dc35364e2291 6 * in the Software without restriction, including without limitation the rights
krmreynolds 0:dc35364e2291 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
krmreynolds 0:dc35364e2291 8 * copies of the Software, and to permit persons to whom the Software is
krmreynolds 0:dc35364e2291 9 * furnished to do so, subject to the following conditions:
krmreynolds 0:dc35364e2291 10 *
krmreynolds 0:dc35364e2291 11 * The above copyright notice and this permission notice shall be included in
krmreynolds 0:dc35364e2291 12 * all copies or substantial portions of the Software.
krmreynolds 0:dc35364e2291 13 *
krmreynolds 0:dc35364e2291 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
krmreynolds 0:dc35364e2291 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
krmreynolds 0:dc35364e2291 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
krmreynolds 0:dc35364e2291 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
krmreynolds 0:dc35364e2291 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
krmreynolds 0:dc35364e2291 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
krmreynolds 0:dc35364e2291 20 * THE SOFTWARE.
krmreynolds 0:dc35364e2291 21 */
krmreynolds 0:dc35364e2291 22
krmreynolds 0:dc35364e2291 23 #pragma once
krmreynolds 0:dc35364e2291 24
krmreynolds 0:dc35364e2291 25 #include "L3G4200D.h"
krmreynolds 0:dc35364e2291 26 #include "LSM303.h"
krmreynolds 0:dc35364e2291 27 #include "mbed.h"
krmreynolds 0:dc35364e2291 28 // LSM303 accelerometer: 8 g sensitivity
krmreynolds 0:dc35364e2291 29 // 3.8 mg/digit; 1 g = 256
krmreynolds 0:dc35364e2291 30 #define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer
krmreynolds 0:dc35364e2291 31
krmreynolds 0:dc35364e2291 32 #define ToRad(x) ((x)*0.01745329252) // *pi/180
krmreynolds 0:dc35364e2291 33 #define ToDeg(x) ((x)*57.2957795131) // *180/pi
krmreynolds 0:dc35364e2291 34
krmreynolds 0:dc35364e2291 35 // L3G4200D gyro: 2000 dps full scale
krmreynolds 0:dc35364e2291 36 // 70 mdps/digit; 1 dps = 0.07
krmreynolds 0:dc35364e2291 37 #define Gyro_Gain_X 0.07 //X axis Gyro gain
krmreynolds 0:dc35364e2291 38 #define Gyro_Gain_Y 0.07 //Y axis Gyro gain
krmreynolds 0:dc35364e2291 39 #define Gyro_Gain_Z 0.07 //Z axis Gyro gain
krmreynolds 0:dc35364e2291 40 #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 41 #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 42 #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 43
krmreynolds 0:dc35364e2291 44
krmreynolds 0:dc35364e2291 45 #define Kp_ROLLPITCH 0.02
krmreynolds 0:dc35364e2291 46 #define Ki_ROLLPITCH 0.00002
krmreynolds 0:dc35364e2291 47 #define Kp_YAW 1.2
krmreynolds 0:dc35364e2291 48 #define Ki_YAW 0.00002
krmreynolds 0:dc35364e2291 49
krmreynolds 0:dc35364e2291 50
krmreynolds 0:dc35364e2291 51 #define STATUS_LED 13
krmreynolds 0:dc35364e2291 52 typedef unsigned char byte;
krmreynolds 0:dc35364e2291 53
krmreynolds 0:dc35364e2291 54 class minimu9 {
krmreynolds 0:dc35364e2291 55 public:
krmreynolds 0:dc35364e2291 56 minimu9 ( void );
krmreynolds 0:dc35364e2291 57 void get_data ( void );
krmreynolds 0:dc35364e2291 58 void print_data ( void );
krmreynolds 0:dc35364e2291 59 void Gyro_Init();
krmreynolds 0:dc35364e2291 60 void Read_Gyro();
krmreynolds 0:dc35364e2291 61 void Accel_Init();
krmreynolds 0:dc35364e2291 62 void Read_Accel();
krmreynolds 0:dc35364e2291 63 void Compass_Init();
krmreynolds 0:dc35364e2291 64 void Compass_Heading();
krmreynolds 0:dc35364e2291 65 void Read_Compass();
krmreynolds 0:dc35364e2291 66 void Normalize( void );
krmreynolds 0:dc35364e2291 67 void Drift_correction( void );
krmreynolds 0:dc35364e2291 68 void Matrix_update( void) ;
krmreynolds 0:dc35364e2291 69 void Euler_angles( void );
krmreynolds 0:dc35364e2291 70 void Calibrate_compass( void );
krmreynolds 0:dc35364e2291 71 void set_calibration_values( int, int, int, int, int, int );
krmreynolds 0:dc35364e2291 72 void set_print_settings( int, int, int, int, int );
krmreynolds 0:dc35364e2291 73
krmreynolds 0:dc35364e2291 74 float get_pitch( void );
krmreynolds 0:dc35364e2291 75 float get_roll( void );
krmreynolds 0:dc35364e2291 76 float get_yaw( void );
krmreynolds 0:dc35364e2291 77
krmreynolds 0:dc35364e2291 78 private:
krmreynolds 0:dc35364e2291 79 L3G4200D *gyro;
krmreynolds 0:dc35364e2291 80 LSM303 *compass;
krmreynolds 0:dc35364e2291 81
krmreynolds 0:dc35364e2291 82 Timer t;
krmreynolds 0:dc35364e2291 83
krmreynolds 0:dc35364e2291 84 float G_Dt; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible
krmreynolds 0:dc35364e2291 85 long timer; //general purpuse timer
krmreynolds 0:dc35364e2291 86 long timer_old;
krmreynolds 0:dc35364e2291 87 long timer24; //Second timer used to print values
krmreynolds 0:dc35364e2291 88 unsigned int counter;
krmreynolds 0:dc35364e2291 89 int AN[6]; //array that stores the gyro and accelerometer data
krmreynolds 0:dc35364e2291 90 int AN_OFFSET[6]; //Array that stores the Offset of the sensors
krmreynolds 0:dc35364e2291 91 int SENSOR_SIGN[9];
krmreynolds 0:dc35364e2291 92
krmreynolds 0:dc35364e2291 93 int gyro_x;
krmreynolds 0:dc35364e2291 94 int gyro_y;
krmreynolds 0:dc35364e2291 95 int gyro_z;
krmreynolds 0:dc35364e2291 96 int accel_x;
krmreynolds 0:dc35364e2291 97 int accel_y;
krmreynolds 0:dc35364e2291 98 int accel_z;
krmreynolds 0:dc35364e2291 99 int magnetom_x;
krmreynolds 0:dc35364e2291 100 int magnetom_y;
krmreynolds 0:dc35364e2291 101 int magnetom_z;
krmreynolds 0:dc35364e2291 102 float c_magnetom_x;
krmreynolds 0:dc35364e2291 103 float c_magnetom_y;
krmreynolds 0:dc35364e2291 104 float c_magnetom_z;
krmreynolds 0:dc35364e2291 105 float MAG_Heading;
krmreynolds 0:dc35364e2291 106
krmreynolds 0:dc35364e2291 107 float Accel_Vector[3]; //Store the acceleration in a vector
krmreynolds 0:dc35364e2291 108 float Gyro_Vector[3]; //Store the gyros turn rate in a vector
krmreynolds 0:dc35364e2291 109 float Omega_Vector[3]; //Corrected Gyro_Vector data
krmreynolds 0:dc35364e2291 110 float Omega_P[3]; //Omega Proportional correction
krmreynolds 0:dc35364e2291 111 float Omega_I[3]; //Omega Integrator
krmreynolds 0:dc35364e2291 112 float Omega[3];
krmreynolds 0:dc35364e2291 113
krmreynolds 0:dc35364e2291 114 // Euler angles
krmreynolds 0:dc35364e2291 115 float roll;
krmreynolds 0:dc35364e2291 116 float pitch;
krmreynolds 0:dc35364e2291 117 float yaw;
krmreynolds 0:dc35364e2291 118
krmreynolds 0:dc35364e2291 119 float errorRollPitch[3];
krmreynolds 0:dc35364e2291 120 float errorYaw[3];
krmreynolds 0:dc35364e2291 121
krmreynolds 0:dc35364e2291 122 byte gyro_sat;
krmreynolds 0:dc35364e2291 123
krmreynolds 0:dc35364e2291 124 // Gyros here
krmreynolds 0:dc35364e2291 125 float DCM_Matrix[3][3];
krmreynolds 0:dc35364e2291 126 float Update_Matrix[3][3];
krmreynolds 0:dc35364e2291 127 float Temporary_Matrix[3][3];
krmreynolds 0:dc35364e2291 128
krmreynolds 0:dc35364e2291 129 // Calibration values
krmreynolds 0:dc35364e2291 130 int M_X_MIN;
krmreynolds 0:dc35364e2291 131 int M_Y_MIN;
krmreynolds 0:dc35364e2291 132 int M_Z_MIN;
krmreynolds 0:dc35364e2291 133 int M_X_MAX;
krmreynolds 0:dc35364e2291 134 int M_Y_MAX;
krmreynolds 0:dc35364e2291 135 int M_Z_MAX;
krmreynolds 0:dc35364e2291 136
krmreynolds 0:dc35364e2291 137 int OUTPUTMODE;
krmreynolds 0:dc35364e2291 138 int PRINT_ANALOGS;
krmreynolds 0:dc35364e2291 139 int PRINT_EULER;
krmreynolds 0:dc35364e2291 140 int PRINT_DCM;
krmreynolds 0:dc35364e2291 141 int PRINT_SERIAL;
krmreynolds 0:dc35364e2291 142
krmreynolds 0:dc35364e2291 143 };