chen ada / IMU

基于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?

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 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