基于MPU9250的IMU库,可以测量pitch,roll,yaw,compass

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers imu.h Source File

imu.h

00001 #ifndef __IMU_H_ADA
00002 #define __IMU_H_ADA
00003 
00004 #include "mbed.h"
00005 #include "rtos.h"
00006 
00007 #include "math.h"
00008 
00009 #define M_PI  (float)3.1415926535
00010 #define Kp     4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
00011 #define Ki     1.0f    // integral gain governs rate of convergence of gyroscope biases
00012 
00013 // define MPU9250 register address
00014 //****************************************
00015 #define SMPLRT_DIV      0x19    //Sample Rate Divider. Typical values:0x07(125Hz) 1KHz internal sample rate
00016 #define CONFIG          0x1A    //Low Pass Filter.Typical values:0x06(5Hz)
00017 #define GYRO_CONFIG     0x1B    //Gyro Full Scale Select. Typical values:0x10(1000dps)
00018 #define ACCEL_CONFIG    0x1C    //Accel Full Scale Select. Typical values:0x01(2g)
00019 
00020 #define ACCEL_XOUT_H    0x3B
00021 #define ACCEL_XOUT_L    0x3C
00022 #define ACCEL_YOUT_H    0x3D
00023 #define ACCEL_YOUT_L    0x3E
00024 #define ACCEL_ZOUT_H    0x3F
00025 #define ACCEL_ZOUT_L    0x40
00026 
00027 #define TEMP_OUT_H      0x41
00028 #define TEMP_OUT_L      0x42
00029 
00030 #define GYRO_XOUT_H     0x43
00031 #define GYRO_XOUT_L     0x44    
00032 #define GYRO_YOUT_H     0x45
00033 #define GYRO_YOUT_L     0x46
00034 #define GYRO_ZOUT_H     0x47
00035 #define GYRO_ZOUT_L     0x48
00036 
00037         
00038 #define MAG_XOUT_L      0x03
00039 #define MAG_XOUT_H      0x04
00040 #define MAG_YOUT_L      0x05
00041 #define MAG_YOUT_H      0x06
00042 #define MAG_ZOUT_L      0x07
00043 #define MAG_ZOUT_H      0x08
00044 
00045 
00046 #define PWR_MGMT_1      0x6B    //Power Management. Typical values:0x00(run mode)
00047 #define WHO_AM_I        0x75  //identity of the device
00048 
00049 
00050 #define GYRO_ADDRESS   0xD0   //Gyro and Accel device address
00051 #define MAG_ADDRESS    0x18   //compass device address
00052 #define ACCEL_ADDRESS  0xD0 
00053 
00054 #define ADDRESS_AD0_LOW     0xD0 //address pin low (GND), default for InvenSense evaluation board
00055 #define ADDRESS_AD0_HIGH    0xD1 //address pin high (VCC)
00056 #define DEFAULT_ADDRESS     GYRO_ADDRESS
00057 #define WHO_AM_I_VAL        0x73 //identity of MPU9250 is 0x71. identity of MPU9255 is 0x73.
00058 
00059 
00060 // define BMP180 register address
00061 //****************************************
00062 #define BMP180_ADDR       0xEE     // default I2C address  
00063 
00064 /* ---- Registers ---- */
00065 #define CAL_AC1           0xAA  // R   Calibration data (16 bits)
00066 #define CAL_AC2           0xAC  // R   Calibration data (16 bits)
00067 #define CAL_AC3           0xAE  // R   Calibration data (16 bits)    
00068 #define CAL_AC4           0xB0  // R   Calibration data (16 bits)
00069 #define CAL_AC5           0xB2  // R   Calibration data (16 bits)
00070 #define CAL_AC6           0xB4  // R   Calibration data (16 bits)
00071 #define CAL_B1            0xB6  // R   Calibration data (16 bits)
00072 #define CAL_B2            0xB8  // R   Calibration data (16 bits)
00073 #define CAL_MB            0xBA  // R   Calibration data (16 bits)
00074 #define CAL_MC            0xBC  // R   Calibration data (16 bits)
00075 #define CAL_MD            0xBE  // R   Calibration data (16 bits)
00076 #define CONTROL           0xF4  // W   Control register 
00077 #define CONTROL_OUTPUT    0xF6  // R   Output registers 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB
00078 
00079 // BMP180 Modes
00080 #define MODE_ULTRA_LOW_POWER    0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5
00081 #define MODE_STANDARD           1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4
00082 #define MODE_HIGHRES            2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3
00083 #define MODE_ULTRA_HIGHRES      3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25
00084 
00085 // Control register
00086 #define READ_TEMPERATURE        0x2E 
00087 #define READ_PRESSURE           0x34 
00088 //Other
00089 #define MSLP                    102025          // Mean Sea Level Pressure = 1013.25 hPA (1hPa = 100Pa = 1mbar)
00090 #define LOCAL_ADS_ALTITUDE      2500            //mm     altitude of your position now
00091 #define PRESSURE_OFFSET         0               //Pa    Offset
00092 
00093 /*********************************************/
00094 // struct
00095 typedef struct
00096 {
00097     int16_t X;
00098     int16_t Y;
00099     int16_t Z;
00100 }MPU9250_TypeDef;
00101 
00102 typedef struct
00103 {
00104     int16_t X_Off_Err;
00105     int16_t Y_Off_Err;
00106     int16_t Z_Off_Err;
00107 }MPU9250_TypeDef_Off;
00108 
00109 typedef struct
00110 {
00111     uint8_t Index;
00112     int16_t AvgBuffer[8];
00113 }MPU9250_AvgTypeDef;
00114 
00115 typedef struct
00116 {
00117     uint8_t Index;
00118     int32_t AvgBuffer[8];
00119 }BMP180_AvgTypeDef;
00120 
00121 enum
00122 {
00123     START_TEMPERATURE_MEASUREMENT = 0,
00124     READ_UT_AND_START_PRESSURE_MEASUREMENT,
00125     READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE,
00126 };
00127 /*******************************************************/
00128 class IMU 
00129 {
00130     
00131     public:
00132     //IMU
00133     void IMU_Init();
00134     void IMU_GetYawPitchRoll(float*Angles);
00135     
00136     //MPU9255
00137     void MPU9250_Init();
00138     void MPU9250_READ_ACCEL();
00139     void MPU9250_READ_GYRO();
00140     void MPU9250_READ_MAG();
00141     bool MPU9250_Check();
00142     void MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
00143     void MPU9250_InitGyrOffset();
00144     
00145     //BMP180
00146     void CalTemperatureAndPressureAndAltitude(void);
00147     void BMP180_Init(void);
00148     void BMP180_StartTemperatureMeasurement(void);
00149     void BMP180_StartPressureMeasurement(void);
00150     void BMP180_ReadUncompensatedTemperature(void);
00151     void BMP180_ReadUncompensatedPressure(void);
00152     void BMP180_CalculateTrueTemperature(int32_t *pTrueTemperature);
00153     void BMP180_CalculateTruePressure(int32_t *pTruePressure);
00154     void BMP180_LocalpressureAvg(int32_t *pVal);
00155     void BMP180_PressureAtSeaLevel(void);
00156     void BMP180_CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal);
00157     void BMP180_ReadCalibrationData(void);
00158     void BMP180_SetOversample(void);
00159     
00160     int32_t PressureVal, TemperatureVal, AltitudeVal;
00161     
00162     private:
00163     //IMU
00164     float   q0, q1, q2, q3;
00165     
00166     //MPU9250
00167     char    BUF[10];
00168     int16_t accel[3], gyro[3], magn[3];
00169     float   angles[3];
00170 
00171     MPU9250_TypeDef     MPU9250_Offset;//={0};
00172     MPU9250_TypeDef_Off MPU9250_Magn_Offset;//={0};
00173     
00174     float   invSqrt(float x);
00175     void    IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
00176     void    IMU_GetQuater(void);
00177     
00178     //BMP180
00179     //int32_t PressureVal, TemperatureVal, AltitudeVal;
00180     int16_t AC1, AC2, AC3, B1, B2, MB, MC, MD, _oss;  
00181     uint16_t AC4, AC5, AC6;
00182     int32_t B5, UT, UP, Pressure0; 
00183     
00184     void BMP180_CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_t InVal, int32_t *pOutVal);
00185 
00186     //I2C
00187     void    I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data);
00188     uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr);
00189     void    BMP180_ReadReg(uint8_t RegAddr, uint8_t Num, char *pBuffer);
00190 };
00191 
00192 #endif