chen ada / IMU

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

imu.h@1:7cf70724bdb0, 2018-06-21 (annotated)

Committer:
adaphoto
Date:
Thu Jun 21 08:46:30 2018 +0000
Revision:
1:7cf70724bdb0
Parent:
0:35bba382318b 
???BMP180????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adaphoto 0:35bba382318b 1 #ifndef __IMU_H_ADA
adaphoto 0:35bba382318b 2 #define __IMU_H_ADA
adaphoto 0:35bba382318b 3
adaphoto 0:35bba382318b 4 #include "mbed.h"
adaphoto 0:35bba382318b 5 #include "rtos.h"
adaphoto 0:35bba382318b 6
adaphoto 0:35bba382318b 7 #include "math.h"
adaphoto 0:35bba382318b 8
adaphoto 0:35bba382318b 9 #define M_PI (float)3.1415926535
adaphoto 0:35bba382318b 10 #define Kp 4.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
adaphoto 0:35bba382318b 11 #define Ki 1.0f // integral gain governs rate of convergence of gyroscope biases
adaphoto 0:35bba382318b 12
adaphoto 0:35bba382318b 13 // define MPU9250 register address
adaphoto 0:35bba382318b 14 //****************************************
adaphoto 0:35bba382318b 15 #define SMPLRT_DIV 0x19 //Sample Rate Divider. Typical values:0x07(125Hz) 1KHz internal sample rate
adaphoto 0:35bba382318b 16 #define CONFIG 0x1A //Low Pass Filter.Typical values:0x06(5Hz)
adaphoto 0:35bba382318b 17 #define GYRO_CONFIG 0x1B //Gyro Full Scale Select. Typical values:0x10(1000dps)
adaphoto 0:35bba382318b 18 #define ACCEL_CONFIG 0x1C //Accel Full Scale Select. Typical values:0x01(2g)
adaphoto 0:35bba382318b 19
adaphoto 0:35bba382318b 20 #define ACCEL_XOUT_H 0x3B
adaphoto 0:35bba382318b 21 #define ACCEL_XOUT_L 0x3C
adaphoto 0:35bba382318b 22 #define ACCEL_YOUT_H 0x3D
adaphoto 0:35bba382318b 23 #define ACCEL_YOUT_L 0x3E
adaphoto 0:35bba382318b 24 #define ACCEL_ZOUT_H 0x3F
adaphoto 0:35bba382318b 25 #define ACCEL_ZOUT_L 0x40
adaphoto 0:35bba382318b 26
adaphoto 0:35bba382318b 27 #define TEMP_OUT_H 0x41
adaphoto 0:35bba382318b 28 #define TEMP_OUT_L 0x42
adaphoto 0:35bba382318b 29
adaphoto 0:35bba382318b 30 #define GYRO_XOUT_H 0x43
adaphoto 0:35bba382318b 31 #define GYRO_XOUT_L 0x44
adaphoto 0:35bba382318b 32 #define GYRO_YOUT_H 0x45
adaphoto 0:35bba382318b 33 #define GYRO_YOUT_L 0x46
adaphoto 0:35bba382318b 34 #define GYRO_ZOUT_H 0x47
adaphoto 0:35bba382318b 35 #define GYRO_ZOUT_L 0x48
adaphoto 0:35bba382318b 36
adaphoto 0:35bba382318b 37
adaphoto 0:35bba382318b 38 #define MAG_XOUT_L 0x03
adaphoto 0:35bba382318b 39 #define MAG_XOUT_H 0x04
adaphoto 0:35bba382318b 40 #define MAG_YOUT_L 0x05
adaphoto 0:35bba382318b 41 #define MAG_YOUT_H 0x06
adaphoto 0:35bba382318b 42 #define MAG_ZOUT_L 0x07
adaphoto 0:35bba382318b 43 #define MAG_ZOUT_H 0x08
adaphoto 0:35bba382318b 44
adaphoto 0:35bba382318b 45
adaphoto 0:35bba382318b 46 #define PWR_MGMT_1 0x6B //Power Management. Typical values:0x00(run mode)
adaphoto 0:35bba382318b 47 #define WHO_AM_I 0x75 //identity of the device
adaphoto 0:35bba382318b 48
adaphoto 0:35bba382318b 49
adaphoto 0:35bba382318b 50 #define GYRO_ADDRESS 0xD0 //Gyro and Accel device address
adaphoto 0:35bba382318b 51 #define MAG_ADDRESS 0x18 //compass device address
adaphoto 0:35bba382318b 52 #define ACCEL_ADDRESS 0xD0
adaphoto 0:35bba382318b 53
adaphoto 0:35bba382318b 54 #define ADDRESS_AD0_LOW 0xD0 //address pin low (GND), default for InvenSense evaluation board
adaphoto 0:35bba382318b 55 #define ADDRESS_AD0_HIGH 0xD1 //address pin high (VCC)
adaphoto 0:35bba382318b 56 #define DEFAULT_ADDRESS GYRO_ADDRESS
adaphoto 0:35bba382318b 57 #define WHO_AM_I_VAL 0x73 //identity of MPU9250 is 0x71. identity of MPU9255 is 0x73.
adaphoto 0:35bba382318b 58
adaphoto 1:7cf70724bdb0 59
adaphoto 1:7cf70724bdb0 60 // define BMP180 register address
adaphoto 1:7cf70724bdb0 61 //****************************************
adaphoto 1:7cf70724bdb0 62 #define BMP180_ADDR 0xEE // default I2C address
adaphoto 1:7cf70724bdb0 63
adaphoto 1:7cf70724bdb0 64 /* ---- Registers ---- */
adaphoto 1:7cf70724bdb0 65 #define CAL_AC1 0xAA // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 66 #define CAL_AC2 0xAC // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 67 #define CAL_AC3 0xAE // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 68 #define CAL_AC4 0xB0 // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 69 #define CAL_AC5 0xB2 // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 70 #define CAL_AC6 0xB4 // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 71 #define CAL_B1 0xB6 // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 72 #define CAL_B2 0xB8 // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 73 #define CAL_MB 0xBA // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 74 #define CAL_MC 0xBC // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 75 #define CAL_MD 0xBE // R Calibration data (16 bits)
adaphoto 1:7cf70724bdb0 76 #define CONTROL 0xF4 // W Control register
adaphoto 1:7cf70724bdb0 77 #define CONTROL_OUTPUT 0xF6 // R Output registers 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB
adaphoto 1:7cf70724bdb0 78
adaphoto 1:7cf70724bdb0 79 // BMP180 Modes
adaphoto 1:7cf70724bdb0 80 #define MODE_ULTRA_LOW_POWER 0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5
adaphoto 1:7cf70724bdb0 81 #define MODE_STANDARD 1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4
adaphoto 1:7cf70724bdb0 82 #define MODE_HIGHRES 2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3
adaphoto 1:7cf70724bdb0 83 #define MODE_ULTRA_HIGHRES 3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25
adaphoto 1:7cf70724bdb0 84
adaphoto 1:7cf70724bdb0 85 // Control register
adaphoto 1:7cf70724bdb0 86 #define READ_TEMPERATURE 0x2E
adaphoto 1:7cf70724bdb0 87 #define READ_PRESSURE 0x34
adaphoto 1:7cf70724bdb0 88 //Other
adaphoto 1:7cf70724bdb0 89 #define MSLP 102025 // Mean Sea Level Pressure = 1013.25 hPA (1hPa = 100Pa = 1mbar)
adaphoto 1:7cf70724bdb0 90 #define LOCAL_ADS_ALTITUDE 2500 //mm altitude of your position now
adaphoto 1:7cf70724bdb0 91 #define PRESSURE_OFFSET 0 //Pa Offset
adaphoto 0:35bba382318b 92
adaphoto 1:7cf70724bdb0 93 /*********************************************/
adaphoto 1:7cf70724bdb0 94 // struct
adaphoto 1:7cf70724bdb0 95 typedef struct
adaphoto 1:7cf70724bdb0 96 {
adaphoto 1:7cf70724bdb0 97 int16_t X;
adaphoto 1:7cf70724bdb0 98 int16_t Y;
adaphoto 1:7cf70724bdb0 99 int16_t Z;
adaphoto 1:7cf70724bdb0 100 }MPU9250_TypeDef;
adaphoto 1:7cf70724bdb0 101
adaphoto 1:7cf70724bdb0 102 typedef struct
adaphoto 1:7cf70724bdb0 103 {
adaphoto 1:7cf70724bdb0 104 int16_t X_Off_Err;
adaphoto 1:7cf70724bdb0 105 int16_t Y_Off_Err;
adaphoto 1:7cf70724bdb0 106 int16_t Z_Off_Err;
adaphoto 1:7cf70724bdb0 107 }MPU9250_TypeDef_Off;
adaphoto 1:7cf70724bdb0 108
adaphoto 1:7cf70724bdb0 109 typedef struct
adaphoto 1:7cf70724bdb0 110 {
adaphoto 1:7cf70724bdb0 111 uint8_t Index;
adaphoto 1:7cf70724bdb0 112 int16_t AvgBuffer[8];
adaphoto 1:7cf70724bdb0 113 }MPU9250_AvgTypeDef;
adaphoto 1:7cf70724bdb0 114
adaphoto 1:7cf70724bdb0 115 typedef struct
adaphoto 1:7cf70724bdb0 116 {
adaphoto 1:7cf70724bdb0 117 uint8_t Index;
adaphoto 1:7cf70724bdb0 118 int32_t AvgBuffer[8];
adaphoto 1:7cf70724bdb0 119 }BMP180_AvgTypeDef;
adaphoto 1:7cf70724bdb0 120
adaphoto 1:7cf70724bdb0 121 enum
adaphoto 1:7cf70724bdb0 122 {
adaphoto 1:7cf70724bdb0 123 START_TEMPERATURE_MEASUREMENT = 0,
adaphoto 1:7cf70724bdb0 124 READ_UT_AND_START_PRESSURE_MEASUREMENT,
adaphoto 1:7cf70724bdb0 125 READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE,
adaphoto 1:7cf70724bdb0 126 };
adaphoto 1:7cf70724bdb0 127 /*******************************************************/
adaphoto 0:35bba382318b 128 class IMU
adaphoto 0:35bba382318b 129 {
adaphoto 0:35bba382318b 130
adaphoto 0:35bba382318b 131 public:
adaphoto 1:7cf70724bdb0 132 //IMU
adaphoto 0:35bba382318b 133 void IMU_Init();
adaphoto 0:35bba382318b 134 void IMU_GetYawPitchRoll(float*Angles);
adaphoto 1:7cf70724bdb0 135
adaphoto 0:35bba382318b 136 //MPU9255
adaphoto 0:35bba382318b 137 void MPU9250_Init();
adaphoto 0:35bba382318b 138 void MPU9250_READ_ACCEL();
adaphoto 0:35bba382318b 139 void MPU9250_READ_GYRO();
adaphoto 0:35bba382318b 140 void MPU9250_READ_MAG();
adaphoto 0:35bba382318b 141 bool MPU9250_Check();
adaphoto 0:35bba382318b 142 void MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
adaphoto 0:35bba382318b 143 void MPU9250_InitGyrOffset();
adaphoto 0:35bba382318b 144
adaphoto 1:7cf70724bdb0 145 //BMP180
adaphoto 1:7cf70724bdb0 146 void CalTemperatureAndPressureAndAltitude(void);
adaphoto 1:7cf70724bdb0 147 void BMP180_Init(void);
adaphoto 1:7cf70724bdb0 148 void BMP180_StartTemperatureMeasurement(void);
adaphoto 1:7cf70724bdb0 149 void BMP180_StartPressureMeasurement(void);
adaphoto 1:7cf70724bdb0 150 void BMP180_ReadUncompensatedTemperature(void);
adaphoto 1:7cf70724bdb0 151 void BMP180_ReadUncompensatedPressure(void);
adaphoto 1:7cf70724bdb0 152 void BMP180_CalculateTrueTemperature(int32_t *pTrueTemperature);
adaphoto 1:7cf70724bdb0 153 void BMP180_CalculateTruePressure(int32_t *pTruePressure);
adaphoto 1:7cf70724bdb0 154 void BMP180_LocalpressureAvg(int32_t *pVal);
adaphoto 1:7cf70724bdb0 155 void BMP180_PressureAtSeaLevel(void);
adaphoto 1:7cf70724bdb0 156 void BMP180_CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal);
adaphoto 1:7cf70724bdb0 157 void BMP180_ReadCalibrationData(void);
adaphoto 1:7cf70724bdb0 158 void BMP180_SetOversample(void);
adaphoto 1:7cf70724bdb0 159
adaphoto 1:7cf70724bdb0 160 int32_t PressureVal, TemperatureVal, AltitudeVal;
adaphoto 0:35bba382318b 161
adaphoto 0:35bba382318b 162 private:
adaphoto 1:7cf70724bdb0 163 //IMU
adaphoto 1:7cf70724bdb0 164 float q0, q1, q2, q3;
adaphoto 0:35bba382318b 165
adaphoto 1:7cf70724bdb0 166 //MPU9250
adaphoto 0:35bba382318b 167 char BUF[10];
adaphoto 1:7cf70724bdb0 168 int16_t accel[3], gyro[3], magn[3];
adaphoto 0:35bba382318b 169 float angles[3];
adaphoto 1:7cf70724bdb0 170
adaphoto 0:35bba382318b 171 MPU9250_TypeDef MPU9250_Offset;//={0};
adaphoto 0:35bba382318b 172 MPU9250_TypeDef_Off MPU9250_Magn_Offset;//={0};
adaphoto 0:35bba382318b 173
adaphoto 0:35bba382318b 174 float invSqrt(float x);
adaphoto 0:35bba382318b 175 void IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
adaphoto 0:35bba382318b 176 void IMU_GetQuater(void);
adaphoto 0:35bba382318b 177
adaphoto 1:7cf70724bdb0 178 //BMP180
adaphoto 1:7cf70724bdb0 179 //int32_t PressureVal, TemperatureVal, AltitudeVal;
adaphoto 1:7cf70724bdb0 180 int16_t AC1, AC2, AC3, B1, B2, MB, MC, MD, _oss;
adaphoto 1:7cf70724bdb0 181 uint16_t AC4, AC5, AC6;
adaphoto 1:7cf70724bdb0 182 int32_t B5, UT, UP, Pressure0;
adaphoto 0:35bba382318b 183
adaphoto 1:7cf70724bdb0 184 void BMP180_CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_t InVal, int32_t *pOutVal);
adaphoto 1:7cf70724bdb0 185
adaphoto 1:7cf70724bdb0 186 //I2C
adaphoto 0:35bba382318b 187 void I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data);
adaphoto 0:35bba382318b 188 uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr);
adaphoto 1:7cf70724bdb0 189 void BMP180_ReadReg(uint8_t RegAddr, uint8_t Num, char *pBuffer);
adaphoto 0:35bba382318b 190 };
adaphoto 0:35bba382318b 191
adaphoto 0:35bba382318b 192 #endif