File content as of revision 2:85214374e094:

/*

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"

// 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

// 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

/*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 );
    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;

    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
    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;
    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];

    unsigned int counter;
    byte gyro_sat;

    float DCM_Matrix[3][3];
    float Update_Matrix[3][3]; //Gyros here
    float Temporary_Matrix[3][3];
};

#endif