Skip to main content

HAL 库开发笔记 - I2C 通信(MPU6050)

本篇基于自研 RobotCtrl 开发套件,单片机内核为 STM32F407ZET6,使用 MPU6050 模组讲解 HAL 库 I2C 通信的方式,开发套件原理图及详细介绍请见 RobotCtrl - STM32 通用开发套件

基本原理

I2C 通信

I2C 通信的基本原理可跳转文章 通信协议 - I2C

MPU6050 模组

模组的引脚定义:

  • VCC:3.3V~5V
  • GND:地
  • SCL:I2C 时钟 / SPI 时钟
  • SDA:I2C 数据 / SPI 数据输入
  • XDA:给 I2C 设备提供主时钟
  • AD0:I2C 器件地址选择位 / SPI 数据输出
  • INT:中断引脚

带卡尔曼滤波的 MPU6050 库

这里我们调用带卡尔曼滤波的 MPU6050 库:leech001/MPU6050,将下载的 mpu6050.cmpu6050.h 拷贝至项目文件夹下,并在 STM32CubeIDE/Keil 内将其添加到项目中:

mpu6050.h

#ifndef INC_GY521_H_
#define INC_GY521_H_

#endif /* INC_GY521_H_ */

#include <stdint.h>
#include "i2c.h"

// MPU6050 structure
typedef struct
{

int16_t Accel_X_RAW;
int16_t Accel_Y_RAW;
int16_t Accel_Z_RAW;
double Ax;
double Ay;
double Az;

int16_t Gyro_X_RAW;
int16_t Gyro_Y_RAW;
int16_t Gyro_Z_RAW;
double Gx;
double Gy;
double Gz;

float Temperature;

double KalmanAngleX;
double KalmanAngleY;
} MPU6050_t;

// Kalman structure
typedef struct
{
double Q_angle;
double Q_bias;
double R_measure;
double angle;
double bias;
double P[2][2];
} Kalman_t;

uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx);

void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);

void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);

void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);

void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct);

double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt);
mpu6050.c
#include <math.h>
#include "mpu6050.h"

#define RAD_TO_DEG 57.295779513082320876798154814105

#define WHO_AM_I_REG 0x75
#define PWR_MGMT_1_REG 0x6B
#define SMPLRT_DIV_REG 0x19
#define ACCEL_CONFIG_REG 0x1C
#define ACCEL_XOUT_H_REG 0x3B
#define TEMP_OUT_H_REG 0x41
#define GYRO_CONFIG_REG 0x1B
#define GYRO_XOUT_H_REG 0x43

// Setup MPU6050
#define MPU6050_ADDR 0xD0
const uint16_t i2c_timeout = 100;
const double Accel_Z_corrector = 14418.0;

uint32_t timer;

Kalman_t KalmanX = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.03f};

Kalman_t KalmanY = {
.Q_angle = 0.001f,
.Q_bias = 0.003f,
.R_measure = 0.03f,
};

uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
{
uint8_t check;
uint8_t Data;

// check device ID WHO_AM_I

HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, i2c_timeout);

if (check == 104) // 0x68 will be returned by the sensor if everything goes well
{
// power management register 0X6B we should write all 0's to wake the sensor up
Data = 0;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout);

// Set DATA RATE of 1KHz by writing SMPLRT_DIV register
Data = 0x07;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, i2c_timeout);

// Set accelerometer configuration in ACCEL_CONFIG Register
// XA_ST=0,YA_ST=0,ZA_ST=0, FS_SEL=0 -> � 2g
Data = 0x00;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, ACCEL_CONFIG_REG, 1, &Data, 1, i2c_timeout);

// Set Gyroscopic configuration in GYRO_CONFIG Register
// XG_ST=0,YG_ST=0,ZG_ST=0, FS_SEL=0 -> � 250 �/s
Data = 0x00;
HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, i2c_timeout);
return 0;
}
return 1;
}

void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[6];

// Read 6 BYTES of data starting from ACCEL_XOUT_H register

HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);

DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);

/*** convert the RAW values into acceleration in 'g'
we have to divide according to the Full scale value set in FS_SEL
I have configured FS_SEL = 0. So I am dividing by 16384.0
for more details check ACCEL_CONFIG Register ****/

DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
}

void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[6];

// Read 6 BYTES of data starting from GYRO_XOUT_H register

HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);

DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);

/*** convert the RAW values into dps (�/s)
we have to divide according to the Full scale value set in FS_SEL
I have configured FS_SEL = 0. So I am dividing by 131.0
for more details check GYRO_CONFIG Register ****/

DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;
}

void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[2];
int16_t temp;

// Read 2 BYTES of data starting from TEMP_OUT_H_REG register

HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout);

temp = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);
}

void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
uint8_t Rec_Data[14];
int16_t temp;

// Read 14 BYTES of data starting from ACCEL_XOUT_H register

HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);

DataStruct->Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
DataStruct->Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
DataStruct->Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);
temp = (int16_t)(Rec_Data[6] << 8 | Rec_Data[7]);
DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data[9]);
DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data[11]);
DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data[13]);

DataStruct->Ax = DataStruct->Accel_X_RAW / 16384.0;
DataStruct->Ay = DataStruct->Accel_Y_RAW / 16384.0;
DataStruct->Az = DataStruct->Accel_Z_RAW / Accel_Z_corrector;
DataStruct->Temperature = (float)((int16_t)temp / (float)340.0 + (float)36.53);
DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;

// Kalman angle solve
double dt = (double)(HAL_GetTick() - timer) / 1000;
timer = HAL_GetTick();
double roll;
double roll_sqrt = sqrt(
DataStruct->Accel_X_RAW * DataStruct->Accel_X_RAW + DataStruct->Accel_Z_RAW * DataStruct->Accel_Z_RAW);
if (roll_sqrt != 0.0)
{
roll = atan(DataStruct->Accel_Y_RAW / roll_sqrt) * RAD_TO_DEG;
}
else
{
roll = 0.0;
}
double pitch = atan2(-DataStruct->Accel_X_RAW, DataStruct->Accel_Z_RAW) * RAD_TO_DEG;
if ((pitch < -90 && DataStruct->KalmanAngleY > 90) || (pitch > 90 && DataStruct->KalmanAngleY < -90))
{
KalmanY.angle = pitch;
DataStruct->KalmanAngleY = pitch;
}
else
{
DataStruct->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, DataStruct->Gy, dt);
}
if (fabs(DataStruct->KalmanAngleY) > 90)
DataStruct->Gx = -DataStruct->Gx;
DataStruct->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, DataStruct->Gx, dt);
}

double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, double dt)
{
double rate = newRate - Kalman->bias;
Kalman->angle += dt * rate;

Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle);
Kalman->P[0][1] -= dt * Kalman->P[1][1];
Kalman->P[1][0] -= dt * Kalman->P[1][1];
Kalman->P[1][1] += Kalman->Q_bias * dt;

double S = Kalman->P[0][0] + Kalman->R_measure;
double K[2];
K[0] = Kalman->P[0][0] / S;
K[1] = Kalman->P[1][0] / S;

double y = newAngle - Kalman->angle;
Kalman->angle += K[0] * y;
Kalman->bias += K[1] * y;

double P00_temp = Kalman->P[0][0];
double P01_temp = Kalman->P[0][1];

Kalman->P[0][0] -= K[0] * P00_temp;
Kalman->P[0][1] -= K[0] * P01_temp;
Kalman->P[1][0] -= K[1] * P00_temp;
Kalman->P[1][1] -= K[1] * P01_temp;

return Kalman->angle;
};

可以看到,在设置了 I2C 的地址后,在 MPU6050_Init 函数内初始化,并在其余的函数中操作读取各个数值。

使用 I2C 读取 MPU6050 返回的信息

在 CubeMX 内配置 I2C 总线

在 CubeMX 左侧功能分类栏选择 通信 - I2Cx,将 I2C 的选项设置从 disable 更改为 I2C,并在弹出的配置界面配置参数(默认即可):

在代码内配置 I2C 读取 MPU6050 返回的信息

首先,在 main.c 中调用 MPU6050 的库:

main.c
/* USER CODE BEGIN Includes */

#include "mpu6050.h"

/* USER CODE END Includes */

接着,实例化对象:

main.c
/* USER CODE BEGIN PV */

MPU6050_t MPU6050;

/* USER CODE END PV */

在主函数里面初始化,完毕时才继续执行程序:

main.c
/* USER CODE BEGIN 2 */

while (MPU6050*Init(&hi2c1) == 1);

/* USER CODE END 2 */

在 while 循环内读取库计算出来的变量,并给一定的延时缓冲:

main.c

/* USER CODE BEGIN 3 */

MPU6050_Read_All(&hi2c1, &MPU6050);
HAL_Delay(100);
}

/* USER CODE END 3 */

执行了这条语句,就能读出 MPU6050 结构体内的变量,比如MPU6050.KalmanAngleX(X 轴滤波后角度)。MPU6050 结构体的元素及类型如下:

typedef struct
{

int16_t Accel_X_RAW;
int16_t Accel_Y_RAW;
int16_t Accel_Z_RAW;
double Ax;
double Ay;
double Az;

int16_t Gyro_X_RAW;
int16_t Gyro_Y_RAW;
int16_t Gyro_Z_RAW;
double Gx;
double Gy;
double Gz;

float Temperature;

double KalmanAngleX;
double KalmanAngleY;
} MPU6050_t;

可以在配置串口后,通过以下语句输出变量:

printf("XAngle: %.2f°\t", MPU6050.KalmanAngleX);

参考与致谢

文章作者:Power Lin
原文地址:https://wiki-power.com
版权声明:文章采用 CC BY-NC-SA 4.0 协议,转载请注明出处。