AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
minimu9.h
- Committer:
- krmreynolds
- Date:
- 2012-04-12
- Revision:
- 0:dc35364e2291
- Child:
- 1:3272ece36ce1
File content as of revision 0:dc35364e2291:
/* mbed L3G4200D Library version 0.1 * Copyright (c) 2012 Prediluted * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ #pragma once #include "L3G4200D.h" #include "LSM303.h" #include "mbed.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 #define Kp_ROLLPITCH 0.02 #define Ki_ROLLPITCH 0.00002 #define Kp_YAW 1.2 #define Ki_YAW 0.00002 #define STATUS_LED 13 typedef unsigned char byte; class minimu9 { public: minimu9 ( void ); void get_data ( void ); void print_data ( void ); void Gyro_Init(); void Read_Gyro(); void Accel_Init(); void Read_Accel(); void Compass_Init(); void Compass_Heading(); void Read_Compass(); void Normalize( void ); void Drift_correction( void ); void Matrix_update( void) ; void Euler_angles( void ); void Calibrate_compass( void ); void set_calibration_values( int, int, int, int, int, int ); void set_print_settings( int, int, int, int, int ); float get_pitch( void ); float get_roll( void ); float get_yaw( void ); private: 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 unsigned int counter; 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]; 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]; byte gyro_sat; // Gyros here float DCM_Matrix[3][3]; float Update_Matrix[3][3]; float Temporary_Matrix[3][3]; // Calibration values int M_X_MIN; int M_Y_MIN; int M_Z_MIN; int M_X_MAX; int M_Y_MAX; int M_Z_MAX; int OUTPUTMODE; int PRINT_ANALOGS; int PRINT_EULER; int PRINT_DCM; int PRINT_SERIAL; };