#ifndef FreeIMU_h

#define FreeIMU_h

#define I2C_SDA p28

#define I2C_SCL p27

#define FREEIMU_v04

#define GEN_MPU6050 // Generic MPU6050 breakout board. Compatible with GY-521, SEN-11028 and other MPU6050 wich have the MPU6050 AD0 pin connected to GND.

#define FREEIMU_LIB_VERSION "20121122"

#define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"

#if F_CPU == 16000000L

#define FREEIMU_FREQ "16 MHz"

#elif F_CPU == 8000000L

#define FREEIMU_FREQ "8 MHz"

#endif

#if defined(FREEIMU_v04)

#define FREEIMU_ID "FreeIMU v0.4"

#endif

#define HAS_MPU6050() (defined(FREEIMU_v04) || defined(GEN_MPU6050))

#define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))

#define HAS_AXIS_ALIGNED() (defined(FREEIMU_v01) || defined(FREEIMU_v02) || defined(FREEIMU_v03) || defined(FREEIMU_v035) || defined(FREEIMU_v035_MS) || defined(FREEIMU_v035_BMP) || defined(FREEIMU_v04) || defined(SEN_10121) || defined(SEN_10736))

#include "mbed.h"

#include "calibration.h"

#if HAS_MPU6050()

// #include <Wire.h>

#include "I2Cdev.h"

#include "MPU6050.h"

#define FIMU_ACCGYRO_ADDR MPU6050_DEFAULT_ADDRESS

#endif

#include <HMC58X3.h>

#include <MS561101BA.h>

#define FIMU_BARO_ADDR MS561101BA_ADDR_CSB_LOW

#define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW

#define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND

#define twoKpDef (2.0f * 1.0f) // 2 * proportional gain

#define twoKiDef (2.0f * 0.6f) // 2 * integral gain

#ifndef cbi

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))

#endif

class FreeIMU

{

public:

FreeIMU();

void init();

void init(bool fastmode);

void init(int accgyro_addr, bool fastmode);

#ifndef CALIBRATION_H

void calLoad();

#endif

void zeroGyro();

void getRawValues(int16_t * raw_values);

void getValues(float * values);

void getQ(float * q);

void getEuler(float * angles);

void getYawPitchRoll(float * ypr);

void getEulerRad(float * angles);

void getYawPitchRollRad(float * ypr);

void gravityCompensateAcc(float * acc, float * q);

float getRawPressure();

float getBaroAlt();

float getBaroAlt(float sea_press);

void getQ_simple(float* q);

// we make them public so that users can interact directly with device classes

MPU6050 *accgyro;

MS561101BA *baro;

HMC58X3 *magn;

int* raw_acc, raw_gyro, raw_magn;

// calibration parameters

int16_t gyro_off_x, gyro_off_y, gyro_off_z;

int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;

float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;

private:

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, bool _magn_valid);

//float q0, q1, q2, q3; // quaternion elements representing the estimated orientation

float iq0, iq1, iq2, iq3;

float exInt, eyInt, ezInt; // scaled integral error

volatile float twoKp; // 2 * proportional gain (Kp)

volatile float twoKi; // 2 * integral gain (Ki)

volatile float twoKiz, twoKpz;

volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame

volatile float integralFBx, integralFBy, integralFBz;

Timer update;

int dt_us;

//unsigned long lastUpdate, now; // sample period expressed in milliseconds

float sampleFreq; // half the sample period expressed in seconds

};

float invSqrt(float number);

void arr3_rad_to_deg(float * arr);

#endif // FreeIMU_h

http://mbed.org/users/tyftyftyf/code/HMC58X3/#8eb12adc8368