AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
Diff: minimu9.h
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
--- a/minimu9.h Thu Apr 12 13:47:23 2012 +0000 +++ b/minimu9.h Mon Apr 23 14:31:08 2012 +0000 @@ -1,30 +1,39 @@ -/* 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 +/* + +MinIMU-9-Arduino-AHRS +Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) + +Copyright (c) 2011 Pololu Corporation. +http://www.pololu.com/ + +MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: +http://code.google.com/p/sf9domahrs/ + +sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose +Julio and Doug Weibel: +http://code.google.com/p/ardu-imu/ +MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it +under the terms of the GNU Lesser General Public License as published by the +Free Software Foundation, either version 3 of the License, or (at your option) +any later version. + +MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but +WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for +more details. + +You should have received a copy of the GNU Lesser General Public License along +with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. + +*/ +#ifndef MINIMU9_h +#define MINIMU9_h + +#include "mbed.h" #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 @@ -41,55 +50,64 @@ #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 - +// LSM303 magnetometer calibration constants; use the Calibrate example from +// the Pololu LSM303 library to find the right values for your board +#define M_X_MIN -796 +#define M_Y_MIN -457 +#define M_Z_MIN -424 +#define M_X_MAX 197 +#define M_Y_MAX 535 +#define M_Z_MAX 397 + #define Kp_ROLLPITCH 0.02 #define Ki_ROLLPITCH 0.00002 #define Kp_YAW 1.2 #define Ki_YAW 0.00002 - -#define STATUS_LED 13 +/*For debugging purposes*/ +//OUTPUTMODE=1 will print the corrected data, +//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift) +#define OUTPUTMODE 1 + +//#define PRINT_DCM 0 //Will print the whole direction cosine matrix +#define PRINT_ANALOGS 0 //Will print the analog raw data +#define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw + 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 ); + minimu9( void ); + bool update( void ); + float get_roll(); + float get_pitch(); + float get_yaw(); private: + void initialize_values( void ); + void read_gyro( void ); + void read_accel( void ); + void read_compass( void ); + void compass_heading( void ); + void matrix_update( void ); + void normalize( void ); + void drift_correction( void ); + void euler_angles( void ); + void print_data( void ); + L3G4200D *gyro; - LSM303 *compass; + 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 + 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]; - + long timer24; //Second timer used to print values + 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]; // Orientation int gyro_x; int gyro_y; int gyro_z; @@ -119,25 +137,12 @@ float errorRollPitch[3]; float errorYaw[3]; + unsigned int counter; byte gyro_sat; - // Gyros here float DCM_Matrix[3][3]; - float Update_Matrix[3][3]; + float Update_Matrix[3][3]; //Gyros here 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; - -}; \ No newline at end of file +#endif \ No newline at end of file