陀螺仪mpu6050的使用附带HAL的使用

我们日常见的陀螺仪模块的使用就是在平衡小车和控制小车的移动上，那么陀螺仪是怎么使用的呢，首先就是能很好的使用I2C,而看到这里，说的一切都是虚的，首先陀螺仪的配置和数据手册大家也是没少看的，但是还是跟我再了解一遍.

MPU6050内部整合了三轴MEMS陀螺仪、三轴MEMS加速度计以及一个可扩展的数字运动处理器DMP(Digital Motion Processor)，而且还可以连接一个第三方数字传感器(如磁力计)，这样的话，就可以通过IIC接口输出一个9轴信号(链接第三方数字传感器才可以输出九轴信号，否则只有六轴信号)。更加方便的是，有了DMP（同时这个可以在官方库去移植），可以结合InvenSense公司提供的运动处理资料库，实现姿态解算。通过自带的DMP，可以通过IIC接口输出9轴融合演算的数据，大大降低了运动处理运算对操作系统的负荷，同时也降低了开发难度。其实，简单一句话说，陀螺仪就是测角速度的，加速度传感器就是测角加速度的，二者数据通过算法就可以得到PITCH、YAW、ROLL角了。

而对陀螺仪的算法精确就是低通滤波和卡尔曼滤波（在低通滤波上我已学会，在卡尔曼滤波我已学废，但是我还是鼓励大家学卡尔曼滤波，毕竟对数据可以很精准）

 这张图片大家也看得很多了吧，但是我们还是应该要知道这个图片就对应这mpu6050的方向，wo我也希望在使用这个模块的时候可以看清它的方向，毕竟这种事我也经常干。

 这个图咋们也能看出来这个就是mpu6050的内部框图，其中就是测温度加速度和姿态的。

这个大家可以参考一下，HAL库的玩家更要看掌握知识：链接

那么简单的来了，上实践操作部分，而且不要怕不会用，我会慢慢讲解的。

1.  
   /*
2.  
   *mpu6050.c
3.  
   *
4.  
   *Creation date: September 11th, 2021
5.  
   *Author: Xiaodong
6.  
   *
7.  
   *Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter
8.  
   */
9.  
    
10.  
     
11.  
    #include <math.h>
12.  
    #include "mpu6050.h"
13.  
     
14.  
    #define RAD_TO_DEG 57.3
15.  
     
16.  
    #define WHO_AM_I_REG 0x75
17.  
    #define PWR_MGMT_1_REG 0x6B
18.  
    #define SMPLRT_DIV_REG 0x19
19.  
    #define ACCEL_CONFIG_REG 0x1C
20.  
    #define ACCEL_XOUT_H_REG 0x3B
21.  
    #define TEMP_OUT_H_REG 0x41
22.  
    #define GYRO_CONFIG_REG 0x1B
23.  
    #define GYRO_XOUT_H_REG 0x43
24.  
     
25.  
    // Setup MPU6050
26.  
    #define MPU6050_ADDR 0xD0
27.  
    const uint16_t i2c_timeout = 100;
28.  
    const double Accel_Z_corrector = 14418.0;
29.  
     
30.  
    uint32_t timer;
31.  
     
32.  
    Kalman_t KalmanX = {
33.  
    .Q_angle = 0.001f,
34.  
    .Q_bias = 0.003f,
35.  
    .R_measure = 0.03f};
36.  
     
37.  
    Kalman_t KalmanY = {
38.  
    .Q_angle = 0.001f,
39.  
    .Q_bias = 0.003f,
40.  
    .R_measure = 0.03f,
41.  
    };
42.  
     
43.  
    uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
44.  
    {
45.  
    uint8_t check;
46.  
    uint8_t Data; // data buffer
47.  
     
48.  
    // check device ID WHO_AM_I
49.  
     
50.  
    HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, i2c_timeout);
51.  
     
52.  
    if (check == 104) // 0x68 will be returned by the sensor if everything goes well
53.  
    {
54.  
    // power management register 0X6B we should write all 0's to wake the sensor up
55.  
    Data = 0;
56.  
    HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout);
57.  
     
58.  
    // Set DATA RATE of 1KHz by writing SMPLRT_DIV register
59.  
    // Sample Rate = Gyroscope Output Rate / (1 SMPLRT_DIV)
60.  
    // Gyroscope Output Rate = 8kHz when the DLPF is disabled, and 1kHz when the DLPF is enabled.
61.  
    Data = 0x07;
62.  
    HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, i2c_timeout);
63.  
     
64.  
    // Set accelerometer configuration in ACCEL_CONFIG Register
65.  
    // XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> - 2g
66.  
    Data = 0x00;
67.  
    HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, i2c_timeout);
68.  
     
69.  
    // Set Gyroscopic configuration in GYRO_CONFIG Register
70.  
    // XG_ST=0,YG_ST=0,ZG_ST=0, FS_SEL=0 -> - 250 degree/s
71.  
    Data = 0x00;
72.  
    HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, i2c_timeout);
73.  
    return 0;
74.  
    }
75.  
    return 1;
76.  
    }
77.  
     
78.  
    void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
79.  
    {
80.  
    uint8_t Rec_Data[6];
81.  
     
82.  
    // Read 6 BYTES of data starting from ACCEL_XOUT_H register
83.  
     
84.  
    HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
85.  
     
86.  
    DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
87.  
    DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
88.  
    DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
89.  
     
90.  
    /*** convert the RAW values into acceleration in 'g'
91.  
    we have to divide according to the Full scale value set in FS_SEL
92.  
    I have configured FS_SEL = 0. So I am dividing by 16384.0
93.  
    for more details check ACCEL_CONFIG Register ****/
94.  
     
95.  
    DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;
96.  
    DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
97.  
    DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
98.  
    // DataStruct->Az = DataStruct->Accel_Z_RAW / 16384.0;
99.  
    }
100.  
      
101.  
     void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
102.  
     {
103.  
     uint8_t Rec_Data[6];
104.  
      
105.  
     // Read 6 BYTES of data starting from GYRO_XOUT_H register
106.  
      
107.  
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
108.  
      
109.  
     DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
110.  
     DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
111.  
     DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
112.  
      
113.  
     /*** convert the RAW values into dps (�/s)
114.  
     we have to divide according to the Full scale value set in FS_SEL
115.  
     I have configured FS_SEL = 0. So I am dividing by 131.0
116.  
     for more details check GYRO_CONFIG Register ****/
117.  
      
118.  
     DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
119.  
     DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
120.  
     DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
121.  
     }
122.  
      
123.  
     void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
124.  
     {
125.  
     uint8_t Rec_Data[2];
126.  
     int16_t temp;
127.  
      
128.  
     // Read 2 BYTES of data starting from TEMP_OUT_H_REG register
129.  
      
130.  
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout);
131.  
      
132.  
     temp = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
133.  
     DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 (float)36.53);
134.  
     }
135.  
      
136.  
     void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
137.  
     {
138.  
     uint8_t Rec_Data[14];
139.  
     int16_t temp;
140.  
      
141.  
     // Read 14 BYTES of data starting from ACCEL_XOUT_H register
142.  
     // Accel and gyro's x, y and z data is seriate, so just read 14 bytes for the first register
143.  
      
144.  
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);
145.  
      
146.  
     DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
147.  
     DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
148.  
     DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
149.  
     temp = (int16_t)(Rec_Data[6] << 8 | Rec_Data[7]);
150.  
     DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data[9]);
151.  
     DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data[11]);
152.  
     DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data[13]);
153.  
      
154.  
     DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0; // unit: g
155.  
     DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
156.  
     DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
157.  
     DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 (float)36.53);
158.  
     DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
159.  
     DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
160.  
     DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
161.  
      
162.  
     // Kalman angle solve
163.  
     double dt = (double)(HAL_GetTick() - timer) / 1000;
164.  
     timer = HAL_GetTick();
165.  
     double roll;
166.  
     double roll_sqrt = sqrt(
167.  
     DataStruct->Accel_X_RAW * DataStruct->Accel_X_RAW DataStruct->Accel_Z_RAW * DataStruct->Accel_Z_RAW);
168.  
     if (roll_sqrt != 0.0)
169.  
     {
170.  
     roll = atan(DataStruct->Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG;
171.  
     }
172.  
     else
173.  
     {
174.  
     roll = 0.0;
175.  
     }
176.  
     double pitch = atan2(-DataStruct->Accel_X_RAW, DataStruct->Accel_Z_RAW) * RAD_TO_DEG;
177.  
     if ((pitch < -90 && DataStruct->KalmanAngleY > 90) || (pitch > 90 && DataStruct->KalmanAngleY < -90))
178.  
     {
179.  
     KalmanY.angle = pitch;
180.  
     DataStruct->KalmanAngleY = pitch;
181.  
     }
182.  
     else
183.  
     {
184.  
     DataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt);
185.  
     }
186.  
     if (fabs(DataStruct->KalmanAngleY) > 90)
187.  
     DataStruct->Gx = -DataStruct->Gx;
188.  
     DataStruct->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, DataStruct->Gx, dt);
189.  
     }
190.  
      
191.  
     double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
192.  
     {
193.  
     double rate = newRate - Kalman->bias;
194.  
     Kalman->angle = dt * rate;
195.  
      
196.  
     Kalman->P[0][0] = dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] Kalman->Q_angle);
197.  
     Kalman->P[0][1] -= dt * Kalman->P[1][1];
198.  
     Kalman->P[1][0] -= dt * Kalman->P[1][1];
199.  
     Kalman->P[1][1] = Kalman->Q_bias * dt;
200.  
      
201.  
     double S = Kalman->P[0][0] Kalman->R_measure;
202.  
     double K[2];
203.  
     K[0] = Kalman->P[0][0] / S;
204.  
     K[1] = Kalman->P[1][0] / S;
205.  
      
206.  
     double y = newAngle - Kalman->angle;
207.  
     Kalman->angle = K[0] * y;
208.  
     Kalman->bias = K[1] * y;
209.  
      
210.  
     double P00_temp = Kalman->P[0][0];
211.  
     double P01_temp = Kalman->P[0][1];
212.  
      
213.  
     Kalman->P[0][0] -= K[0] * P00_temp;
214.  
     Kalman->P[0][1] -= K[0] * P01_temp;
215.  
     Kalman->P[1][0] -= K[1] * P00_temp;
216.  
     Kalman->P[1][1] -= K[1] * P01_temp;
217.  
      
218.  
     return Kalman->angle;
219.  
     };

不要管这个代码有多长就是不要怕，这个可以直接使用，DataStruct->KalmanAngleX 就是结构指针变量，指针大家都学过吧，这也是一个意思，就是个变量其他的就是算法而已，其中，我也做了卡尔曼滤波的算法，将数据做到更精准，这个就是福利了。

1.  
   /*
2.  
   *mpu6050.h
3.  
   *
4.  
   *Creation date: September 11th, 2021
5.  
   *Author: Xiaodong
6.  
   *
7.  
   *Kalman filter algorithm used from https://github.com/TKJElectronics/KalmanFilter
8.  
   */
9.  
    
10.  
    #ifndef INC_GY521_H_
11.  
    #define INC_GY521_H_
12.  
     
13.  
    #endif /* INC_GY521_H_ */
14.  
     
15.  
    #include <stdint.h>
16.  
    #include "i2c.h"
17.  
     
18.  
    // MPU6050 structure
19.  
    typedef struct
20.  
    {
21.  
     
22.  
    int16_t Accel_X_RAW;
23.  
    int16_t Accel_Y_RAW;
24.  
    int16_t Accel_Z_RAW;
25.  
    double Ax;
26.  
    double Ay;
27.  
    double Az;
28.  
     
29.  
    int16_t Gyro_X_RAW;
30.  
    int16_t Gyro_Y_RAW;
31.  
    int16_t Gyro_Z_RAW;
32.  
    double Gx;
33.  
    double Gy;
34.  
    double Gz;
35.  
     
36.  
    float Temperature;
37.  
     
38.  
    double KalmanAngleX;
39.  
    double KalmanAngleY;
40.  
    } MPU6050_t;
41.  
     
42.  
    // Kalman structure
43.  
    typedef struct
44.  
    {
45.  
    double Q_angle;
46.  
    double Q_bias;
47.  
    double R_measure;
48.  
    double angle;
49.  
    double bias;
50.  
    double P[2][2];
51.  
    } Kalman_t;
52.  
     
53.  
    uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx);
54.  
     
55.  
    void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
56.  
     
57.  
    void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
58.  
     
59.  
    void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
60.  
     
61.  
    void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);
62.  
     
63.  
    double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);

这估计大家都会看懵，并且使用上会有困难，毕竟我这都是指针和域的规划，mpu6050_t，写HAL库的大家应该不少见了吧，那么给他给个定义应该也是不难。

其中用freertos操作系统也是一样的使用

这篇好文章是转载于：学新通技术网