File content as of revision 0:62284d27d75e:

/*
MinIMU-9-mbed-AHRS
Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System)

Modified and ported to mbed environment by Michael Shimniok
http://www.bot-thoughts.com/

Basedd on 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 __DCM_H
#define __DCM_H

#define GRAVITY 1024  //this equivalent to 1G in the raw data coming from the accelerometer 
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square

#define ToRad(x) ((x)*0.01745329252)  // *pi/180
#define ToDeg(x) ((x)*57.2957795131)  // *180/pi

//#define Kp_ROLLPITCH 0.02
//#define Ki_ROLLPITCH 0.00002
//#define Kp_YAW 1.2
//#define Ki_YAW 0.00002
#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002

#define OUTPUTMODE 1 // enable drift correction

// TODO: move elsewhere
// Gyro sensor hardware-specific stuff
#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 per second
#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians per second
#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians per second

/** DCM AHRS algorithm ported to mbed from Pololu MinIMU-9 in turn ported from ArduPilot 1.5
 * revised in a more OO style, though no done yet.
 *
 * Early version. There's more to do here but it's a start, at least.
 *
 * Warning: Interface will most likely change.
 *
 * Expects to see a Sensors.h in your project, as follows, with functions that read sensors and set the appropriate 
 * "input" member variables below. Eventually I'll change this to a better encapsulated OOP approach. 
 *
 * This approach of an external Sensors.* is an abstraction layer that makes it much, much easier to
 * swap in a different sensor suite versus the original code. You can use Serial, I2C, SPI, Analog,
 * whatever you want, whatever it takes as long as you populate the gyro, accel, and mag vectors before
 * calling Update_step()
 *
 * @code
 * #ifndef __SENSORS_H
 * #define __SENSORS_H
 *
 * void Gyro_Init(void);
 * void Read_Gyro(void);
 * void Accel_Init(void);
 * void Read_Accel(void);
 * void Compass_Init(void);
 * void Read_Compass(void);
 * void Calculate_Offsets(void);
 * void Compass_Heading(void);
 * 
 * #endif
 * @endcode
 *
 */
class DCM {
    public:
        /** Output: Euler angle: roll */
        float roll;
        /** Output:  Euler angle: pitch */
        float pitch;
        /** Output:  Euler angle: yaw */
        float yaw;
        
        /** Input gyro sensor reading X-axis */
        int gyro_x;
        /** Input gyro sensor reading Y-axis */
        int gyro_y;
        /** Input gyro sensor reading Z-axis */
        int gyro_z;
        /** Input accelerometer sensor reading X-axis */
        int accel_x;
        /** Input accelerometer sensor reading Y-axis */
        int accel_y;
        /** Input accelerometer sensor reading Z-axis */
        int accel_z;
        /*
        int magnetom_x;
        int magnetom_y;
        int magnetom_z;
        */
        /** Input for the offset corrected & scaled magnetometer reading, X-axis */
        float c_magnetom_x;
        /** Input for the offset corrected & scaled magnetometer reading, Y-axis */
        float c_magnetom_y;
        /** Input for the offset corrected & scaled magnetometer reading, Z-axis */
        float c_magnetom_z;
        /** Input for the calculated magnetic heading */
        float MAG_Heading;
        /** Input for the speed (ie, the magnitude of the 3d velocity vector */
        float speed_3d;
        /** Input for the integration time (DCM algorithm) */
        float G_Dt;

        /** Creates a new DCM AHRS algorithm object
         */
        DCM(void);

        /** A single update cycle for the algorithm; updates the matrix, normalizes it, corrects for gyro
         * drift on 3 axes, (does not yet adjust for acceleration). Magnetometer update is run less often
         * than gyro/accelerometer-based update
         */
        void Update_step(void);
        
    private:        
        float Accel_Vector[3];  // Store the acceleration in a vector
        float Gyro_Vector[3];   // Store the gyros turn rate in a vector
        float dcm[3][3];        // The Direction Cosine Matrix
        float errorRollPitch[3]; 
        float errorYaw[3];
        float Omega_Vector[3];  // Corrected Gyro_Vector data
        float Omega_P[3];       // Omega Proportional correction
        float Omega_I[3];       // Omega Integrator
        float Omega[3];         // Omega
        float Temporary_Matrix[3][3];
        float Update_Matrix[3][3];
        int update_count;   // call Update_mag() every update_count calls to Update_step()
        float constrain(float x, float a, float b);
        void Normalize(void);
        void Drift_correction(void);
        void Accel_adjust(void);
        void Matrix_update(void);
        void Euler_angles(void);
        void Update_mag(void);
        
};

#endif