STM32F103C8T6读取陀螺仪MPU6050的角度数据，使用6050自带DMP库姿态解算出各个方向的角度，并使用OLED实时刷新显示，同时可以将数据通过串口发送到计算机，每一组数据50ms。本操作过程简单，方便移植，显示屏接PA5/7，陀螺仪接PB6/7，串口为PA9/10
改变俯仰角Pitch
改变横滚角Roll
改变航偏角Yaw
上面是姿态角的示例，下面是重点：
IIC驱动
STM32的硬件IIC是存在一定问题的，初始化MPU6050时可能导致初始化失败，所以推荐使用软件IIC。
MPU6050的摆放
默认情况下相对于载体坐标系而言，绕x轴旋转为横滚（roll），绕y轴旋转是俯仰角（pitch），绕z轴旋转是偏航（yaw）。注意是绕。
如果把MPU6050看成一架飞机，摆动机翼是横滚角，上下摆动机头是俯仰角，左右摆动机头是偏航角。
下图摆放方式是错误的
下图的摆放方式才是正确的，如上文描述可知。
注意图2.2（上方第二张图），是指默认情况下。在官方的DMP驱动中有这样一段代码
static signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1};
这个是MPU6050的坐标矩阵，可根据实际摆放的位置修改此矩阵。如果不修改这个矩阵，需要按照图2.2摆放，不然数值肯定是错误的。
MPU6050初始化
MPU6050初始化有可能会卡死。可参考正点原子的例程，实测没什么问题。
MPU6050 DMP 是带有自检功能的
run_self_test();run_self_test 这个函数用于自检，MPU6050会以初始状态作为欧拉角的0度，不管有没有放平MPU6050都会以初始状态作为欧拉角的0度，初始化之后MPU6050会稍微校准一下，那么前几秒的数据是可以适当舍弃的。
虽然MPU6050会稍微校准，但是初始化时一定要放平。如果不放平大概率报错。当然也有可能成功，但是MPU6050校准的数据会有偏差。
MPU6050偏航角（yaw）零飘
MPU6050的偏航角是不正常的，即使静止不动也会跳动。这个纯粹是硬件问题，就算算法怎么牛逼也解决不了（不可避免的）。只能外加磁力计解决，也可以直接使用MPU9250，这个是自带磁力计的。使用时注意不要和电机靠太近，磁场很容易受到干扰。
MPU6050万向节锁
默认Z-Y-X顺序下，不知道大家有没有发现这个问题。当MPU6050只转动俯仰角，俯仰角接近90度或者等于90度时，偏航角的角度会发生很大的偏差。那么这个就是万向节锁，此时如果比作一架飞机，当飞机垂直90度向上时，按照Z-Y-X顺序无论怎么改变方向飞机都只能向上飞。
所以不要让俯仰角垂直，这样可以减少bug。
还是要再细讲
本文章主要讲述mpu6050在应用中遇到的问题、问题的本质分析和解决办法。在此之前我们需要知道姿态（x，y，z轴角度）解算是通过mpu6050返回的各轴的“加速度”和“角速度”来解算的。
mpu6050存在的问题：
1. mpu6050零漂问题
2.mpu6050由于离心力的问题
3.mpu6050温漂问题
一个个来讲
1、零漂问题是指传感器在没有输入信号时输出的偏差，这是MEMS传感器常见的问题，因为它们的制造工艺和材料特性可能导致的。（不可避免的）
2、小车在直行过程只有支持力与重力的受力。而转弯时有向心力
小车在急弯行驶时，受到自身的重力和地面的支持力和理论上完全水平向圆心方向的“向心力”。不过在事实中小车由于自身的车身或者自己的机械结构的差异，导致此时的向心力不是完全水平向里的了，就会导致向心力会有一些分力分解到垂直方向上，那么此时的z轴加速度数据就会受到影响。
那我们回来，还记得我们说过姿态解算是通过mpu6050传回来的加速度和角速度进行解算的嘛。那么此时的mpu6050传感器z轴数据就变得不是那么准确了，那我们用不准确的数据解算出来的数据自然也是有问题的。
3.温度变化会影响MEMS传感器的物理性质，如材料的膨胀系数、弹性模量、粘附力等，这些变化会导致传感器的零点漂移和灵敏度变化，进而影响传感器的测量准确性。（不可避免的）
解决办法
1.零漂可以通过漂移的差值来补偿。原理就是一开始mpu6050先测出1秒内的六轴数据的漂移平均值（此过程mpu6050是静止状态）并记录下来，然后此后再读取六轴数据时，把每个轴的数据减去漂移平均值就是约等于此时的六轴数据了。
#include "ti_msp_dl_config.h"
#include "drv_oled.h"
#include "delay.h"
#include "mympu6050.h"
//mpu6050读取出来的原始数据
int16_t ax_Primitve,ay_Primitve,az_Primitve,gx_Primitve,gy_Primitve,gz_Primitve;
//1秒内得到的误差偏移量平均值//取双精度浮点型小数
float MPU6050ERROR[6]={0.00f,0.00f,0.00f,0.00f,0.00f,0.00f};
#define DELAY1 (8000000)
#define MPU6050_ADDRESS 0x68
//指定地址写
//EquiAddr从机未移位的原7位地址
//RegAddr从机寄存器的地址
//Byte要写入的字节
//static
void IIC_WriteByte(uint8_t EquiAddr,uint8_t RegAddr,uint8_t Byte)
{
uint8_t buff[2]={RegAddr,Byte};
DL_I2C_fillControllerTXFIFO(I2C1,buff,2);
while(!(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_IDLE));
DL_I2C_startControllerTransfer(I2C1,EquiAddr,DL_I2C_CONTROLLER_DIRECTION_TX,2);
while(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
}
//指定地址连续写
//EquiAddr从机未移位的原7位地址
//RegAddr从机寄存器的地址
//GetArray要发送数据存放的数组
//len数据长度
void IIC_ScanWrite(uint8_t EquiAddr,uint8_t RegAddr,uint8_t *GetArray,uint16_t len)
{
uint8_t array[256];
array[0]=RegAddr;
for(uint16_t i=0;i<len;i++)
{array[i+1]=GetArray[i];}
//填充主机发送缓存区
DL_I2C_fillControllerTXFIFO(I2C1,array,len+1);
while(!(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_IDLE));
DL_I2C_startControllerTransfer(I2C1,EquiAddr,DL_I2C_CONTROLLER_DIRECTION_TX,len+1);
//主机开始发送，指定IIC外设，从机地址，读写方向（写方向），数据长度（包含寄存器地址）
while(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
}
//指定地址读(读取一个字节)
//EquiAddr从机未移位的原7位地址
//RegAddr从机寄存器的地址
uint8_t IIC_ReadByte(uint8_t EquiAddr,uint8_t RegAddr)
{
//填充主机发送缓存区
DL_I2C_fillControllerTXFIFO(I2C1,&RegAddr,1);
// 确保I2C控制器处于空闲状态
while(!(DL_I2C_getControllerStatus(I2C1) & DL_I2C_CONTROLLER_STATUS_IDLE));
//主机开始发送，指定IIC外设，从机地址，读写方向（写方向），数据长度（包含寄存器地址）
DL_I2C_startControllerTransfer(I2C1,EquiAddr,DL_I2C_CONTROLLER_DIRECTION_TX,1);
while(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
while(!(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_IDLE));
//主机开始接收，指定IIC外设，从机地址，读写方向（读方向），要读取的数据长度
DL_I2C_startControllerTransfer(I2C1,EquiAddr,DL_I2C_CONTROLLER_DIRECTION_RX,1);
while(DL_I2C_isControllerRXFIFOEmpty(I2C1));
return DL_I2C_receiveControllerData(I2C1);
}
//指定地址连续读
//EquiAddr从机未移位的原7位地址
//RegAddr从机寄存器的地址
//*GetArray接收数据的数组
//len接收数据的数组长度
void IIC_ScanRead(uint8_t EquiAddr,uint8_t RegAddr,uint8_t *GetArray,uint8_t len)
{
// if(len==0)return;
// //填充主机发送缓存区
DL_I2C_fillControllerTXFIFO(I2C1,&RegAddr,1);
while(!(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_IDLE));
//主机开始发送，指定IIC外设，从机地址，读写方向（写方向），数据长度（包含寄存器地址）
DL_I2C_startControllerTransfer(I2C1,EquiAddr,DL_I2C_CONTROLLER_DIRECTION_TX,1);
while(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_BUSY_BUS);
while(!(DL_I2C_getControllerStatus(I2C1)&DL_I2C_CONTROLLER_STATUS_IDLE));
DL_I2C_startControllerTransfer(I2C1,EquiAddr,DL_I2C_CONTROLLER_DIRECTION_RX,len);
//主机开始接收，指定IIC外设，从机地址，读写方向（读方向），要读取的数据长度
while(len--)
{
while(DL_I2C_isControllerRXFIFOEmpty(I2C1));
*(GetArray++)=DL_I2C_receiveControllerData(I2C1);
}
}
//往MPU6050指定寄存器写入数据
void MPU6050_WriteByte(uint8_t RegAddr,uint8_t Data)
{
IIC_WriteByte(MPU6050_ADDRESS,RegAddr,Data);
}
//往MPU6050指定寄存器连续写入数据
void MPU6050_scanWrite(uint8_t RegAddr,uint8_t *Data,uint8_t len)
{
IIC_ScanWrite(MPU6050_ADDRESS,RegAddr,Data,len);
}
//从寄存器读取一个字节
uint8_t MPU6050_ReadByte(uint8_t RegAddr)
{
return IIC_ReadByte(MPU6050_ADDRESS,RegAddr);
}
//从MPU6050指定起始寄存器读取数据
void MPU6050_ScanRead(uint8_t RegAddr,uint8_t *Data,uint8_t len)
{
IIC_ScanRead(MPU6050_ADDRESS,RegAddr,Data,len);
}
//MPU6050初始化
void MPU6050_Init(void)
{
// //mpu6050EN
// DL_GPIO_setPins(GPIOB,DL_GPIO_PIN_1);
MPU6050_WriteByte(MPU6050_PWR_MGMT_1,0x01);
delay_cycles(DELAY1);
MPU6050_WriteByte(MPU6050_PWR_MGMT_2, 0x00);
MPU6050_WriteByte(MPU6050_SMPLRT_DIV, 0x09);
MPU6050_WriteByte(MPU6050_CONFIG, 0x06);
MPU6050_WriteByte(MPU6050_GYRO_CONFIG, 0x18);
MPU6050_WriteByte(MPU6050_ACCEL_CONFIG, 0x18);
// uint8_t wake[6]={0x09,0x06,0x18,0x18,0x01,0x00};
// MPU6050_scanWrite(MPU6050_PWR_MGMT_1,&wake[4],2);
// MPU6050_scanWrite(MPU6050_SMPLRT_DIV,&wake[0],4);
delay_cycles(DELAY1);
}
//获取MPU6050的ID
uint8_t MPU6050_GetID(void)
{
return MPU6050_ReadByte(MPU6050_WHO_AM_I);
}
//获取MPU6050的六轴数据
void MPU6050_GetData(int16_t *ACCX,int16_t *ACCY,int16_t *ACCZ,int16_t *GYROX,int16_t *GYROY,int16_t *GYROZ)
{
uint8_t GetArray[12];
MPU6050_ScanRead(MPU6050_ACCEL_XOUT_H,&GetArray[0],6);
MPU6050_ScanRead(MPU6050_GYRO_XOUT_H,&GetArray[6],6);
*ACCX=(GetArray[0]<<8)+GetArray[1];
*ACCY=(GetArray[2]<<8)+GetArray[3];
*ACCZ=(GetArray[4]<<8)+GetArray[5];
*GYROX=(GetArray[6]<<8)+GetArray[7];
*GYROY=(GetArray[8]<<8)+GetArray[9];
*GYROZ=(GetArray[10]<<8)+GetArray[11];
}
//注：在读取mpu6050偏移量的时候，不能让mpu6050晃动
// 1秒内得到静止状态下的测量的100个误差值
void getDataErrorSum(void)
{
for(int i = 0; i < 100; i++)
{
MPU6050_GetData(&ax_Primitve,&ay_Primitve,&az_Primitve,&gx_Primitve,&gy_Primitve,&gz_Primitve);
MPU6050ERROR[0] += ax_Primitve;
MPU6050ERROR[1] += ay_Primitve;
MPU6050ERROR[2] += az_Primitve - 9.8;//重力加速度
MPU6050ERROR[3] += gx_Primitve;
MPU6050ERROR[4] += gy_Primitve;
MPU6050ERROR[5] +=gz_Primitve;
delay_ms(10);//10ms刷新一次
}
}
// 将100个误差值取均值,得到对应的偏移误差
void getDataError(void)
{
for(int i = 0; i < 6; i++)
{
MPU6050ERROR[i] /= 100.0;
}
}
// 得到滤波之后的数值
void dataGetAndFilter(void)
{
MPU6050_GetData(&ax_Primitve,&ay_Primitve,&az_Primitve,&gx_Primitve,&gy_Primitve,&gz_Primitve);
ax_Primitve=ax_Primitve-MPU6050ERROR[0];
ay_Primitve=ax_Primitve-MPU6050ERROR[1];
az_Primitve=ax_Primitve-MPU6050ERROR[2];
gx_Primitve=ax_Primitve-MPU6050ERROR[3];
gy_Primitve=ax_Primitve-MPU6050ERROR[4];
gz_Primitve=ax_Primitve-MPU6050ERROR[5];
}
getDataErrorSum函数
这个函数的目的是在传感器静止时收集数据，以便计算误差的总和。这样做的原因是，在静止状态下，加速度计应该只测量到重力加速度，而陀螺仪应该读到接近于零的值（理想情况下）。
MPU6050_GetData 函数被用来读取原始的传感器数据，然后累加到 MPU6050ERROR 数组的相应元素中。Z轴加速度数据在累加前减去9.8，这是因为在静止状态下，Z轴应该只测量到地球的重力加速度。
getDataError 函数
这个函数通过将 getDataErrorSum 函数收集的误差总和除以数据点的数量（在这个例子中是100），来计算误差的平均值。这个平均误差可以视为传感器在静止状态下的偏移误差，通常用于后续的数据校准。
dataGetAndFilter 函数
这个函数使用前面计算出的平均误差来滤波最新的传感器数据，以消除或减小偏移误差的影响。
在 dataGetAndFilter 函数中，首先调用 MPU6050_GetData 读取最新的传感器数据，然后从每个通道的数据中减去对应的偏移误差，得到滤波后的数据。
这种简单的偏移补偿方法适用于静态误差的校正，但在动态条件下可能不够准确。在实际应用中，可能需要更复杂的滤波算法，如卡尔曼滤波器，来提供更准确的数据融合和噪声抑制。
