基于MPU9250的IMU库,可以测量pitch,roll,yaw,compass
imu.h@0:35bba382318b, 2018-06-21 (annotated)
- Committer:
- adaphoto
- Date:
- Thu Jun 21 07:00:40 2018 +0000
- Revision:
- 0:35bba382318b
- Child:
- 1:7cf70724bdb0
??MPU9250?IMU??????pith?roll?yaw?compass
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:35bba382318b | 59 | typedef struct |
adaphoto | 0:35bba382318b | 60 | { |
adaphoto | 0:35bba382318b | 61 | int16_t X; |
adaphoto | 0:35bba382318b | 62 | int16_t Y; |
adaphoto | 0:35bba382318b | 63 | int16_t Z; |
adaphoto | 0:35bba382318b | 64 | }MPU9250_TypeDef; |
adaphoto | 0:35bba382318b | 65 | |
adaphoto | 0:35bba382318b | 66 | typedef struct |
adaphoto | 0:35bba382318b | 67 | { |
adaphoto | 0:35bba382318b | 68 | int16_t X_Off_Err; |
adaphoto | 0:35bba382318b | 69 | int16_t Y_Off_Err; |
adaphoto | 0:35bba382318b | 70 | int16_t Z_Off_Err; |
adaphoto | 0:35bba382318b | 71 | }MPU9250_TypeDef_Off; |
adaphoto | 0:35bba382318b | 72 | |
adaphoto | 0:35bba382318b | 73 | typedef struct |
adaphoto | 0:35bba382318b | 74 | { |
adaphoto | 0:35bba382318b | 75 | uint8_t Index; |
adaphoto | 0:35bba382318b | 76 | int16_t AvgBuffer[8]; |
adaphoto | 0:35bba382318b | 77 | }MPU9250_AvgTypeDef; |
adaphoto | 0:35bba382318b | 78 | |
adaphoto | 0:35bba382318b | 79 | class IMU |
adaphoto | 0:35bba382318b | 80 | { |
adaphoto | 0:35bba382318b | 81 | |
adaphoto | 0:35bba382318b | 82 | public: |
adaphoto | 0:35bba382318b | 83 | void IMU_Init(); |
adaphoto | 0:35bba382318b | 84 | void IMU_GetYawPitchRoll(float*Angles); |
adaphoto | 0:35bba382318b | 85 | //MPU9255 |
adaphoto | 0:35bba382318b | 86 | void MPU9250_Init(); |
adaphoto | 0:35bba382318b | 87 | void MPU9250_READ_ACCEL(); |
adaphoto | 0:35bba382318b | 88 | void MPU9250_READ_GYRO(); |
adaphoto | 0:35bba382318b | 89 | void MPU9250_READ_MAG(); |
adaphoto | 0:35bba382318b | 90 | bool MPU9250_Check(); |
adaphoto | 0:35bba382318b | 91 | void MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal); |
adaphoto | 0:35bba382318b | 92 | void MPU9250_InitGyrOffset(); |
adaphoto | 0:35bba382318b | 93 | |
adaphoto | 0:35bba382318b | 94 | int16_t accel[3], gyro[3], magn[3]; |
adaphoto | 0:35bba382318b | 95 | |
adaphoto | 0:35bba382318b | 96 | private: |
adaphoto | 0:35bba382318b | 97 | |
adaphoto | 0:35bba382318b | 98 | char BUF[10]; |
adaphoto | 0:35bba382318b | 99 | //int16_t accel[3], gyro[3], magn[3]; |
adaphoto | 0:35bba382318b | 100 | float angles[3]; |
adaphoto | 0:35bba382318b | 101 | float q0, q1, q2, q3; |
adaphoto | 0:35bba382318b | 102 | MPU9250_TypeDef MPU9250_Offset;//={0}; |
adaphoto | 0:35bba382318b | 103 | MPU9250_TypeDef_Off MPU9250_Magn_Offset;//={0}; |
adaphoto | 0:35bba382318b | 104 | |
adaphoto | 0:35bba382318b | 105 | float invSqrt(float x); |
adaphoto | 0:35bba382318b | 106 | void IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); |
adaphoto | 0:35bba382318b | 107 | void IMU_GetQuater(void); |
adaphoto | 0:35bba382318b | 108 | |
adaphoto | 0:35bba382318b | 109 | //void MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal); |
adaphoto | 0:35bba382318b | 110 | |
adaphoto | 0:35bba382318b | 111 | void I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data); |
adaphoto | 0:35bba382318b | 112 | uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr); |
adaphoto | 0:35bba382318b | 113 | }; |
adaphoto | 0:35bba382318b | 114 | |
adaphoto | 0:35bba382318b | 115 | #endif |