[项目]基于FreeRTOS的STM32四轴飞行器: 十一.MPU6050配置与读取

一.芯片介绍

芯片应该放置在PCB中间，X Y轴原点，敏感度131表示范围越小越灵敏。理想状态放置在地面上X，Y，Z轴为0，即使地面极平有可能锡膏导致芯片倾斜。


AD0接地和接电源地址不一样：

寄存器介绍：

二.配置I2C

观察原理图：

CubeMX中配置I2C1，配置Master Features 速度为高速，400KHz，占空比33%。

配置PB3为下拉输入模式：

三.编写驱动

I2C协议中，地址需要左移1位，最低位表示读写方向：
0 表示写操作，1 表示读操作，直接将查好寄存器的代码直接移植。

写入寄存器和读取寄存器函数：

/*** @description: 向指定的寄存器写入一个字节* @param {uint8_t} reg* @param {uint8_t} byte* @return {*}*/
void Inf_MPU6050_WriteReg(uint8_t reg, uint8_t byte)
{HAL_I2C_Mem_Write(&hi2c1,MPU6050_ADDR_W,reg,I2C_MEMADD_SIZE_8BIT,&byte,1,2000);
}/*** @description: 从指定的寄存器startReg写入多个字节* @param {uint8_t} startReg 开始寄存器地址* @param {uint8_t} *bytes 写的字节数据* @param {uint8_t} len 多少个字节* @return {*}*/
void Inf_MPU6050_WriteRegs(uint8_t startReg, uint8_t *bytes, uint8_t len)
{HAL_I2C_Mem_Write(&hi2c1,MPU6050_ADDR_W,startReg,I2C_MEMADD_SIZE_8BIT,bytes,len,2000);
}/*** @description: 从指定的寄存器读取一个字节* @param {uint8_t} reg* @return {*}*/
uint8_t Inf_MPU6050_ReadReg(uint8_t reg)
{uint8_t rByte = 0;HAL_I2C_Mem_Read(&hi2c1,MPU6050_ADDR_R,reg,I2C_MEMADD_SIZE_8BIT,&rByte,1,2000);return rByte;
}/*** @description: 从指定的寄存器读取多个字节* @param {uint8_t} startReg* @param {uint8_t} *bytes* @param {uint8_t} len* @return {*}*/
void Inf_MPU6050_ReadRegs(uint8_t startReg, uint8_t *bytes, uint8_t len)
{HAL_I2C_Mem_Read(&hi2c1,MPU6050_ADDR_R,startReg,I2C_MEMADD_SIZE_8BIT,bytes,len,2000);
}

根据数据手册MPU6050初始化流程配置：

低通滤波器：
数据输出速率1kHz。

采样率：
采样率是多少的时候，才能正确的反应模拟。
香农定律（麦克斯韦定律）：采样率 >= 2 * 最高的频率
这时候可以根据数字信号还原出模拟信号了。
采样频率 = 陀螺仪输出频率 / (1 + SMPLRT_DIV)
500Hz = 1000Hz / (1 + x)
x = 1
因为飞控任务调度周期是4ms，意味着1S执行250次，这时采样频率为500Hz可以1S将数据转换500次，转换多读取少没问题，不能低于250Hz。

加速度和陀螺仪是否进入待机模式：

读取角速度：

读取加速度：

初始化：

/*** @description: mpu6050初始化函数* @return {*}*/
void Inf_MPU6050_Init(void)
{/* 1. 复位MPU6050:   复位->休眠200ms->唤醒 */Inf_MPU6050_WriteReg(MPU_PWR_MGMT1_REG, 1 << 7);HAL_Delay(200);Inf_MPU6050_WriteReg(MPU_PWR_MGMT1_REG, 0);/* 2. 设置量程 *//* 2.1 角速度的量程  +-2000°/s */Inf_MPU6050_WriteReg(MPU_GYRO_CFG_REG, 3 << 3);/* 2.2 加速度的量程  +2g  */Inf_MPU6050_WriteReg(MPU_ACCEL_CFG_REG, 0);/* 3. 关闭中断、关闭 第2 IIC 接口、禁止 FIFO */Inf_MPU6050_WriteReg(MPU_INT_EN_REG, 0);Inf_MPU6050_WriteReg(MPU_USER_CTRL_REG, 0);Inf_MPU6050_WriteReg(MPU_FIFO_EN_REG, 0);/* 4. 设置陀螺仪采样率和低通滤波器低通滤波器:  数据输出速率时 1kHz采样率: 采样率是多少的时候,才能正确的反应模拟信号香浓定律(奈奎斯特定律): 采样率 >= 2 * 信号中最高的频率采样频率 = 陀螺仪输出频率 / (1 + SMPLRT_DIV)500Hz = 1000Hz / (1 + x)x = 1*/Inf_MPU6050_WriteReg(MPU_SAMPLE_RATE_REG, 1); /*  采样率 */Inf_MPU6050_WriteReg(MPU_CFG_REG, 1 << 0);    /* 低通滤波器 *//* 5. 配置系统时钟源 */Inf_MPU6050_WriteReg(MPU_PWR_MGMT1_REG, 1);/* 6. 使能角速度传感器和加速度传感器   进入工作模式 (禁用待机模式)*/Inf_MPU6050_WriteReg(MPU_PWR_MGMT2_REG, 0);/* 校准6轴数据 */Inf_MPU6050_Calibrate();
}

读取角速度和加速度：

/*** @description: 读取角速度* @param {Gyro_Struct} *gyro* @return {*}*/
void Inf_MPU6050_ReadGyro(Gyro_Struct *gyro)
{uint8_t data[6] = {0};Inf_MPU6050_ReadRegs(MPU_GYRO_XOUTH_REG, data, 6);gyro->gyroX = (int16_t)((data[0] << 8) | data[1]);gyro->gyroY = (int16_t)((data[2] << 8) | data[3]);gyro->gyroZ = (int16_t)((data[4] << 8) | data[5]);
}/*** @description: 读取加速度* @param {Accel_Struct} *accel* @return {*}*/
void Inf_MPU6050_ReadAccel(Accel_Struct *accel)
{uint8_t data[6] = {0};Inf_MPU6050_ReadRegs(MPU_ACCEL_XOUTH_REG, data, 6);accel->accelX = (int16_t)((data[0] << 8) | data[1]);accel->accelY = (int16_t)((data[2] << 8) | data[3]);accel->accelZ = (int16_t)((data[4] << 8) | data[5]);
}/*** @description: 读取角速度和加速度* @param {GyroAccel_Struct} *gyroAccel* @return {*}*/
void Inf_MPU6050_ReadGyroAccel(GyroAccel_Struct *gyroAccel)
{Inf_MPU6050_ReadGyro(&gyroAccel->gyro);Inf_MPU6050_ReadAccel(&gyroAccel->accel);
}

四.读取任务的测试

飞行模块中初始化MPU6050：

编写打印角速度加速度数据的函数：

观察打印数据，角速度和加速度都有误差值，应该尽量让前5个值为0。

五.MPU6050六轴数据的校准

在读取时使用HAL_Delay延时3ms，可以让读取的速度变慢，保证读取的值有意义。

得到需要校准的值：

/*** @description: 在水平平面静止状态下校准6轴数据*  1.确认飞机处于静止*      多次测量3轴数据(角速度),本次与上次对比,如果变化小于某个阈值,则认为飞机处于静止状态*  2.多次测量计算偏移*** @return {*}*/
void Inf_MPU6050_Calibrate(void)
{/* 1.确认静止  */uint8_t          cnt = 30;GyroAccel_Struct current;GyroAccel_Struct last;debug_printfln(" 陀螺仪: 等待静止");while(cnt){Inf_MPU6050_ReadGyroAccel(&current);if(abs(current.gyro.gyroX - last.gyro.gyroX) <= 10 &&abs(current.gyro.gyroY - last.gyro.gyroY) <= 10 &&abs(current.gyro.gyroZ - last.gyro.gyroZ) <= 10){cnt--;}last = current; /* 上次的结构体执行这次 */HAL_Delay(3);}debug_printfln(" 陀螺仪: 已经静止");debug_printfln(" 陀螺仪: 开始校准");int32_t  sumBuff[6] = {0};uint16_t sumCount   = 255;for(uint16_t i = 0; i < sumCount; i++){Inf_MPU6050_ReadGyroAccel(&gyroAccel);sumBuff[0] += gyroAccel.gyro.gyroX - 0;sumBuff[1] += gyroAccel.gyro.gyroY - 0;sumBuff[2] += gyroAccel.gyro.gyroZ - 0;sumBuff[3] += gyroAccel.accel.accelX - 0;sumBuff[4] += gyroAccel.accel.accelY - 0;sumBuff[5] += gyroAccel.accel.accelZ - 16383;HAL_Delay(3);}offsetGyroAccel.gyro.gyroX   = sumBuff[0] / sumCount;offsetGyroAccel.gyro.gyroY   = sumBuff[1] / sumCount;offsetGyroAccel.gyro.gyroZ   = sumBuff[2] / sumCount;offsetGyroAccel.accel.accelX = sumBuff[3] / sumCount;offsetGyroAccel.accel.accelY = sumBuff[4] / sumCount;offsetGyroAccel.accel.accelZ = sumBuff[5] / sumCount;// Com_Config_PrintGyroAccel("cal", &offsetGyroAccel);debug_printfln(" 陀螺仪: 结束校准");
}

需要在读取角速度和角速度后减去需要校准的值。

校准后数据如下：

void Inf_MPU6050_ReadGyroAccelCalibrated(GyroAccel_Struct *gyroAccel)
{/* 获取原始数据 */Inf_MPU6050_ReadGyroAccel(gyroAccel);/* 减去校准值 */gyroAccel->gyro.gyroX -= offsetGyroAccel.gyro.gyroX;gyroAccel->gyro.gyroY -= offsetGyroAccel.gyro.gyroY;gyroAccel->gyro.gyroZ -= offsetGyroAccel.gyro.gyroZ;gyroAccel->accel.accelX -= offsetGyroAccel.accel.accelX;gyroAccel->accel.accelY -= offsetGyroAccel.accel.accelY;gyroAccel->accel.accelZ -= offsetGyroAccel.accel.accelZ;
}

观察发现数据仍然有些许波动，需编写滤波算法进行滤波。