File content as of revision 1:7cf70724bdb0:

#ifndef __IMU_H_ADA
#define __IMU_H_ADA

#include "mbed.h"
#include "rtos.h"

#include "math.h"

#define M_PI  (float)3.1415926535
#define Kp     4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki     1.0f    // integral gain governs rate of convergence of gyroscope biases

// define MPU9250 register address
//****************************************
#define SMPLRT_DIV      0x19    //Sample Rate Divider. Typical values:0x07(125Hz) 1KHz internal sample rate
#define CONFIG          0x1A    //Low Pass Filter.Typical values:0x06(5Hz)
#define GYRO_CONFIG     0x1B    //Gyro Full Scale Select. Typical values:0x10(1000dps)
#define ACCEL_CONFIG    0x1C    //Accel Full Scale Select. Typical values:0x01(2g)

#define ACCEL_XOUT_H    0x3B
#define ACCEL_XOUT_L    0x3C
#define ACCEL_YOUT_H    0x3D
#define ACCEL_YOUT_L    0x3E
#define ACCEL_ZOUT_H    0x3F
#define ACCEL_ZOUT_L    0x40

#define TEMP_OUT_H      0x41
#define TEMP_OUT_L      0x42

#define GYRO_XOUT_H     0x43
#define GYRO_XOUT_L     0x44    
#define GYRO_YOUT_H     0x45
#define GYRO_YOUT_L     0x46
#define GYRO_ZOUT_H     0x47
#define GYRO_ZOUT_L     0x48

        
#define MAG_XOUT_L      0x03
#define MAG_XOUT_H      0x04
#define MAG_YOUT_L      0x05
#define MAG_YOUT_H      0x06
#define MAG_ZOUT_L      0x07
#define MAG_ZOUT_H      0x08


#define PWR_MGMT_1      0x6B    //Power Management. Typical values:0x00(run mode)
#define WHO_AM_I        0x75  //identity of the device


#define GYRO_ADDRESS   0xD0   //Gyro and Accel device address
#define MAG_ADDRESS    0x18   //compass device address
#define ACCEL_ADDRESS  0xD0 

#define ADDRESS_AD0_LOW     0xD0 //address pin low (GND), default for InvenSense evaluation board
#define ADDRESS_AD0_HIGH    0xD1 //address pin high (VCC)
#define DEFAULT_ADDRESS     GYRO_ADDRESS
#define WHO_AM_I_VAL        0x73 //identity of MPU9250 is 0x71. identity of MPU9255 is 0x73.


// define BMP180 register address
//****************************************
#define BMP180_ADDR       0xEE     // default I2C address  

/* ---- Registers ---- */
#define CAL_AC1           0xAA  // R   Calibration data (16 bits)
#define CAL_AC2           0xAC  // R   Calibration data (16 bits)
#define CAL_AC3           0xAE  // R   Calibration data (16 bits)    
#define CAL_AC4           0xB0  // R   Calibration data (16 bits)
#define CAL_AC5           0xB2  // R   Calibration data (16 bits)
#define CAL_AC6           0xB4  // R   Calibration data (16 bits)
#define CAL_B1            0xB6  // R   Calibration data (16 bits)
#define CAL_B2            0xB8  // R   Calibration data (16 bits)
#define CAL_MB            0xBA  // R   Calibration data (16 bits)
#define CAL_MC            0xBC  // R   Calibration data (16 bits)
#define CAL_MD            0xBE  // R   Calibration data (16 bits)
#define CONTROL           0xF4  // W   Control register 
#define CONTROL_OUTPUT    0xF6  // R   Output registers 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB

// BMP180 Modes
#define MODE_ULTRA_LOW_POWER    0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5
#define MODE_STANDARD           1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4
#define MODE_HIGHRES            2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3
#define MODE_ULTRA_HIGHRES      3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25

// Control register
#define READ_TEMPERATURE        0x2E 
#define READ_PRESSURE           0x34 
//Other
#define MSLP                    102025          // Mean Sea Level Pressure = 1013.25 hPA (1hPa = 100Pa = 1mbar)
#define LOCAL_ADS_ALTITUDE      2500            //mm     altitude of your position now
#define PRESSURE_OFFSET         0               //Pa    Offset

/*********************************************/
// struct
typedef struct
{
    int16_t X;
    int16_t Y;
    int16_t Z;
}MPU9250_TypeDef;

typedef struct
{
    int16_t X_Off_Err;
    int16_t Y_Off_Err;
    int16_t Z_Off_Err;
}MPU9250_TypeDef_Off;

typedef struct
{
    uint8_t Index;
    int16_t AvgBuffer[8];
}MPU9250_AvgTypeDef;

typedef struct
{
    uint8_t Index;
    int32_t AvgBuffer[8];
}BMP180_AvgTypeDef;

enum
{
    START_TEMPERATURE_MEASUREMENT = 0,
    READ_UT_AND_START_PRESSURE_MEASUREMENT,
    READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE,
};
/*******************************************************/
class IMU 
{
    
    public:
    //IMU
    void IMU_Init();
    void IMU_GetYawPitchRoll(float*Angles);
    
    //MPU9255
    void MPU9250_Init();
    void MPU9250_READ_ACCEL();
    void MPU9250_READ_GYRO();
    void MPU9250_READ_MAG();
    bool MPU9250_Check();
    void MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
    void MPU9250_InitGyrOffset();
    
    //BMP180
    void CalTemperatureAndPressureAndAltitude(void);
    void BMP180_Init(void);
    void BMP180_StartTemperatureMeasurement(void);
    void BMP180_StartPressureMeasurement(void);
    void BMP180_ReadUncompensatedTemperature(void);
    void BMP180_ReadUncompensatedPressure(void);
    void BMP180_CalculateTrueTemperature(int32_t *pTrueTemperature);
    void BMP180_CalculateTruePressure(int32_t *pTruePressure);
    void BMP180_LocalpressureAvg(int32_t *pVal);
    void BMP180_PressureAtSeaLevel(void);
    void BMP180_CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal);
    void BMP180_ReadCalibrationData(void);
    void BMP180_SetOversample(void);
    
    int32_t PressureVal, TemperatureVal, AltitudeVal;
    
    private:
    //IMU
    float   q0, q1, q2, q3;
    
    //MPU9250
    char    BUF[10];
    int16_t accel[3], gyro[3], magn[3];
    float   angles[3];

    MPU9250_TypeDef     MPU9250_Offset;//={0};
    MPU9250_TypeDef_Off MPU9250_Magn_Offset;//={0};
    
    float   invSqrt(float x);
    void    IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
    void    IMU_GetQuater(void);
    
    //BMP180
    //int32_t PressureVal, TemperatureVal, AltitudeVal;
    int16_t AC1, AC2, AC3, B1, B2, MB, MC, MD, _oss;  
    uint16_t AC4, AC5, AC6;
    int32_t B5, UT, UP, Pressure0; 
    
    void BMP180_CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_t InVal, int32_t *pOutVal);

    //I2C
    void    I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data);
    uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr);
    void    BMP180_ReadReg(uint8_t RegAddr, uint8_t Num, char *pBuffer);
};

#endif