MPU-6050姿态解算和数据融合
本小节对 MPU-6050 传感器的原始数据进行姿态解算，然后再利用数据融合算法得到精准的角度。
在上两小节中，我们已经读到 MPU-6050 传感器的原始数据，学习了加速度传感器、陀螺仪的工作原理、姿态解算，并对互补滤波有所了解。在本小节中，我们结合 STM32CubeMX、MDK-ARM 软件，对 MPU-6050 传感器数据进行姿态解算和数据融合。
具体步骤
进入我们上一小节修改过的 MiaowLabs-Demo 文件夹，再打开里面的 MDK-ARM 文件夹，找到 MiaowLabs-Demo.uvprojx 工程文件，双击，打开工程。在 MDK-ARM 工程界面，按下组合键 Ctrl+N（按住 Ctrl 键再按 N 键），新建一个文件，再按下组合键 Ctrl+S，文件名改为 filter.c，保存到 MiaowLabs-DEMO 的 Src 文件夹里，接着在 MDK-ARM 工程界面左侧 Project 栏目双击 Application/User 文件夹，把 filter.c 加进来。
双击 filter.c 源文件，敲入以下代码：
#include "filter.h"//加入头文件
//互补滤波器
// a = tau / (tau + dt)
// acc = 加速度传感器数据
// gyro = 陀螺仪数据
// dt = 运行周期
float angle;
float a;
float ComplementaryFilter(float acc, float gyro, float dt)
{
a = 0.98;
angle = a * (angle + gyro * dt) + (1 - a) * (acc);
return angle;
}
filter.c 源文件中暂时就只是封装了一个互补滤波的函数。
上面代码添加了头文件，但我们还没创建头文件呢，再新建一个文件 filter.h 头文件，把文件保存到 Inc 文件夹。然后，把下面代码敲进去。
#ifndef __FILTER_H
#define __FILTER_H
float ComplementaryFilter(float acc, float gyro, float dt);
#endif
在 MDK-ARM 工程界面，按下组合键 Ctrl+N（按住 Ctrl 键再按 N 键），新建一个文件，再按下组合键 Ctrl+S，文件名改为 control.c，保存到 MiaowLabs-DEMO 的 Src 文件夹里，接着在 MDK-ARM 工程界面左侧 Project 栏目双击 Application/User 文件夹，把 control.c 加进来。
双击 control.c 源文件，敲入以下代码：
#include "control.h"
#include "filter.h"
#include "mpu6050.h"
#include "math.h"
short x_nAcc,y_nAcc,z_nAcc;//加速度x轴、y轴、z轴数据
short x_nGyro,y_nGyro,z_nGyro;//陀螺仪x轴、y轴、z轴数据
float x_fAcc,y_fAcc,z_fAcc;
float g_fAccAngle;//加速度传感器经过atan2()解算得到的角度
float g_fGyroAngleSpeed;//陀螺仪角速度
float g_fCarAngle;//小车倾角
float dt = 0.005;//互补滤波器控制周期
unsigned char g_ucMainEventCount;//主事件计数，会用在中断中
void GetMpuData(void)//获取MPU-6050数据函数
{
MPU_Get_Accelerometer(&x_nAcc,&y_nAcc,&z_nAcc);//获取MPU-6050加速度数据
MPU_Get_Gyroscope(&x_nGyro,&y_nGyro,&z_nGyro); //获取MPU-6050陀螺仪数据
}
void AngleCalculate(void)//角度计算函数
{
//-------加速度数据处理--------------------------
//量程为±2g时，灵敏度：16384 LSB/g
x_fAcc = x_nAcc / 16384.0;
y_fAcc = y_nAcc / 16384.0;
z_fAcc = z_nAcc / 16384.0;
g_fAccAngle = atan2(y_fAcc,z_fAcc) / 3.14 * 180.0;
//-------陀螺仪数据处理-------------------------
//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
g_fGyroAngleSpeed = x_nGyro / 16.4; //计算角速度值
//-------互补滤波---------------
g_fCarAngle = ComplementaryFilter(g_fAccAngle, g_fGyroAngleSpeed, dt);
}
函数 GetMpuData() 的主要作用，就是分别获取 MPU-6050 传感器的三轴加速度、三轴陀螺仪数据，并将数据存入 x_nAcc、y_nAcc、z_nAcc、x_nGyro、y_nGyro、z_nGyro 变量中。
函数 AngleCalculate() 的主要作用，就是先将加速度数据除于灵敏度，将加速度数据由 LSB 转化成 g，注意这时变量类型为了保持精度为使用了 float 类型，再使用 atan2() 函数计算出角度，这时的角度单位为弧度，先除 3.14 再乘 180°，将单位转换成我们日常用的度。然后，将陀螺仪数据除于灵敏度，得到角速度，单位为 deg/s。最后，将加速度初算的角度和陀螺仪角速度送入互补滤波器进行数据融合，得到最终的稳定的精准的小车倾角。
注意这里我们定义 dt = 0.005，即 5ms，要怎么样才能实现精准的 5ms 运行一次呢？放在主循环中是不可靠的，因为按顺序运行，无法精确地控制时间。唯有靠定时中断，我们可以将关键代码放在我们先前用过的 SysTick_Handler() 滴答定时器中断服务函数中，以期实现精准的运行。
添加 math.h 头文件，才可以正常使用 atan2() 反正切函数。
再新建一个文件 control.h 头文件，把文件保存到 Inc 文件夹。然后，把下面代码敲进去：
#ifndef __CONTROL_H
#define __CONTROL_H
#include "filter.h"
extern unsigned char g_ucMainEventCount;
extern float g_fCarAngle;
void GetMpuData(void);
void AngleCalculate(void);
#endif
打开 main.h 头文件，将 control.h 头文件添加进去。
#include "control.h"
这里不把 filter.h 头文件添加进去，我们已经在 control.h 里面包含了 filter.h 头文件，因此不必在这里再重复添加。
在 MDK-ARM 左侧 Application/User 文件夹中，找到 stm32f1xx_it.c 源文件，双击打开，将以下代码敲入 SysTick_Handler() 函数中：
/* USER CODE BEGIN SysTick_IRQn 0 */
g_ucMainEventCount++;
if(g_ucMainEventCount>=5)
{
g_ucMainEventCount=0;
GetMpuData(); //读取MPU6050数据函数，每5ms执行一次
AngleCalculate(); //角度环计算函数，每5ms执行一次
}
/* USER CODE END SysTick_IRQn 0 */
在 main.c 主循环中添加以下代码：
printf("小车角度 = %f",g_fCarAngle);//打印实时角度
HAL_Delay(500);//延时0.5s，防止打印发送数据太快
HAL_GPIO_TogglePin(LED_GPIO_Port,LED_Pin);//指示灯
点击编译按钮，重新进行编译，这时没有报错。但是烧录代码会发现不仅数据发送不出，甚至连指示灯都不闪烁。
注意中断服务函数里的代码：
/* USER CODE BEGIN SysTick_IRQn 0 */
g_ucMainEventCount++;
if(g_ucMainEventCount>=5)
{
g_ucMainEventCount=0;
GetMpuData(); //读取MPU6050数据函数，每5ms执行一次
AngleCalculate(); //角度环计算函数，每5ms执行一次
}
/* USER CODE END SysTick_IRQn 0 */
因为这段代码是在 1ms 内运行完，所以要确保函数 GetMpuData() 和 AngleCalculate() 能在 1ms 内就运行完毕。
而函数 GetMpuData() 里面的 函数 MPU_Get_Accelerometer() 和 MPU_Get_Gyroscope() 都调用了 MPU_Read_Len() 这个函数，里面有一句延时代码 HAL_Delay(100);。也就是说，运行完这段代码要 100ms+ 的时间，在 1ms 中断里根本运行不完，压根没有时间给到主循环执行其他代码，所以发送不出数据，指示灯也不闪。
我们将延时函数 HAL_Delay(100); 注释掉，加快 I2C 读取速度，并不影响 I2C 正常工作。
点击编译按钮，重新进行编译，这时没有报错。将代码烧录进小车中，会看到指示灯每 0.5s 闪烁一次。
插上 Micro-USB 数据线，打开串口助手，可以看到随着小车倾斜角度变化，发送到串口助手上的数据也随之变化。
使用虚拟示波器
使用 printf() 函数打印输出角度到串口助手上，虽然可以看到大致的角度效果，但不够直观，而且无法看到加速度、陀螺仪、融合后的角度之间的联系。接下来，我们将加速度、陀螺仪、角度等参数，通过虚拟示波器转化成曲线输出，更加直观地观察数据之间的关系。
进入 MiaowLabs-Demo 文件夹，再打开里面的 MDK-ARM 文件夹，找到 MiaowLabs-Demo.uvprojx 工程文件，双击，打开工程。
从光盘中找到虚拟示波器上位机的 API 代码文件： outputdata.c、outputdata.h，这两个文件主要就是将发送到虚拟示波器的协议和校验等等功能都已经封装好了，我们只需要修改最后的发送函数，就可以调用。
将 outputdata.c ，复制到 MiaowLabs-Demo 文件夹的 Src 文件夹里。
将 outputdata.h 复制到 MiaowLabs-Demo 文件夹的 Inc 文件夹里。
打开 MDK-ARM 软件，在左侧目录中双击 Application/User，将 Src 文件夹的 outputdata.c 加入工程。
双击打开 outputdata.c 源文件，对两个地方进行修改。
第一个地方是注释掉原来的头文件 uart.h，把我们用到的头文件 usart.h 添加进来。
#include "outputdata.h"
//#include "uart.h"
#include "usart.h"
第二个地方是修改发送函数，将原句 uart_putchar(databuf[i]); 注释掉，改为 HAL 库的发送函数 HAL_UART_Transmit(&huart1,&databuf[i],1,0xFF);。
void OutPut_Data(void)
{
int temp[4] = {0};
unsigned int temp1[4] = {0};
unsigned char databuf[10] = {0};
unsigned char i;
unsigned short CRC16 = 0;
for(i=0;i<4;i++)
{
temp[i] = (int)OutData[i];
temp1[i] = (unsigned int)temp[i];
}
for(i=0;i<4;i++)
{
databuf[i*2] = (unsigned char)(temp1[i]%256);
databuf[i*2+1] = (unsigned char)(temp1[i]/256);
}
CRC16 = CRC_CHECK(databuf,8);
databuf[8] = CRC16%256;
databuf[9] = CRC16/256;
for(i=0;i<10;i++)
//uart_putchar(databuf[i]);
HAL_UART_Transmit(&huart1,&databuf[i],1,0xFF);
}
下面给出 outputdata.c 源文件修改之后的全部代码，以便萌新们进行参考。
#include "outputdata.h"
//#include "uart.h"
#include "usart.h"
float OutData[4] = { 0 };
unsigned short CRC_CHECK(unsigned char *Buf, unsigned char CRC_CNT)
{
unsigned short CRC_Temp;
unsigned char i,j;
CRC_Temp = 0xffff;
for (i=0;i<CRC_CNT; i++){
CRC_Temp ^= Buf[i];
for (j=0;j<8;j++) {
if (CRC_Temp & 0x01)
CRC_Temp = (CRC_Temp >>1 ) ^ 0xa001;
else
CRC_Temp = CRC_Temp >> 1;
}
}
return(CRC_Temp);
}
void OutPut_Data(void)
{
int temp[4] = {0};
unsigned int temp1[4] = {0};
unsigned char databuf[10] = {0};
unsigned char i;
unsigned short CRC16 = 0;
for(i=0;i<4;i++)
{
temp[i] = (int)OutData[i];
temp1[i] = (unsigned int)temp[i];
}
for(i=0;i<4;i++)
{
databuf[i*2] = (unsigned char)(temp1[i]%256);
databuf[i*2+1] = (unsigned char)(temp1[i]/256);
}
CRC16 = CRC_CHECK(databuf,8);
databuf[8] = CRC16%256;
databuf[9] = CRC16/256;
for(i=0;i<10;i++)
//uart_putchar(databuf[i]);
HAL_UART_Transmit(&huart1,&databuf[i],1,0xFF);
}
分别在 control.c、main.h 文件里面添加 outputdata.h 头文件。
在 control.c 文件里的 AngleCalculate() 函数中，把要发送的数据分别送入输出数组中。
void AngleCalculate(void)
{
//-------加速度数据处理--------------------------
//量程为±2g时，灵敏度：16384 LSB/g
x_fAcc = x_nAcc / 16384.0;
y_fAcc = y_nAcc / 16384.0;
z_fAcc = z_nAcc / 16384.0;
g_fAccAngle = atan2(y_fAcc,z_fAcc) * 180.0 / 3.14;
//-------陀螺仪数据处理-------------------------
//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
g_fGyroAngleSpeed = x_nGyro / 16.4; //计算角速度值
//-------互补滤波---------------
g_fCarAngle = ComplementaryFilter(g_fAccAngle, g_fGyroAngleSpeed, dt);
OutData[0]=g_fAccAngle;//发送加速度初步计算的角度
OutData[1]=g_fGyroAngleSpeed;//发送陀螺仪角速度
OutData[2]=g_fCarAngle;//发送数据融合得到的角度
}
在 main.c 文件的主循环中注释掉或删掉之前实验的代码，添加调用虚拟示波器的发送函数。
while (1)
{
//printf("小车角度 = %f",g_fCarAngle);
//HAL_Delay(500);//延时0.5s，防止打印发送数据太快
//HAL_GPIO_TogglePin(LED_GPIO_Port,LED_Pin);
OutPut_Data();//调用虚拟示波器的发送函数
}
用数据线连接小车，打开虚拟示波器，然后点击左上方菜单栏的 Setup 栏， 选择对应的 COM 口，波特率选择 115200。
设置好之后，电机右下角的 RUN 按钮，就会开始显示波形，由于坐标系默认数值比较大，而我们要显示的数据数值又比较小，可能一开始几乎看不到波形。
别慌，这时候我们点击上面的工具栏，可以分别逐步放大 X 轴和 Y 轴，直至显示清晰的波形。
虚拟示波器的 4 个通道，分别对应代码里的数组 OutData[4] 的四个数据。在这里，刚才我们将加速度数据赋给 OutData[0]，所以加速度数据对应 CH1 通道，即红线。以此类推，黄线代表陀螺仪角速度，蓝线代表数据融合后的角度。因为我们没有重新赋值给 OutData[3]，即通道 4，所以它默认的数值为 0，在上位机中只是一条直线。
通过上面的波形可以看到，经过滤波后的角度值，能非常迅速地跟踪到加速度数据，又没有加速度数据那么多噪音，这就是我们想要的效果。