STM32F103上移植MPU6050的DMP与上位机

在我们使用MPU6050芯片时期望得到的是姿态数据，也就是姿态的倾角。我们通常利用的原始数据，进行姿态融合解算，此方法比较复杂，知识点比较多，需要进行数学的四元数推运算，初学者不易掌握。其实，MPU6050 本身自带了数字运动处理器，即DMP（Digital Motion Processing），并且InvenSense提供了一个 MPU6050的嵌入式运动驱动库，结合MPU6050的DMP，可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到yaw（航向角）、roll（翻滚角）和 pitch（俯仰角）。

     使用内置的DMP，简化了姿态融合解析代码设计，不用进行姿态解算过程，在一定程度上降低了MCU的负担，从而有更多的时间去处理其他事件，提高系统实时性（其实卡尔曼滤波和互补滤波在STM32里面也没花多少时间）。更重要的是，让大家可以不需要接触复杂的滤波算法就可以直接得到姿态的倾角。

    使用MPU6050的DMP输出的四元数是q30格式的，也就是浮点数放大了2的30次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以2的30次方，然后再进行计算，

   计算公式为：

   q0=quat[0] / q30; //q30格式转换为浮点数

   q1=quat / q30;

   q2=quat / q30;

   q3=quat / q30;

   //下面计算得到俯仰角、横滚角、航向角

   pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;//俯仰角

   roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 *q1 - 2 * q2* q2 + 1)* 57.3; //横滚角

   yaw=atan2(2 * (q1 * q2 + q0 * q3),q0 * q0 + q1 *q 1- q2 * q2 - q3 * q3) * 57.3; //航向角

     到此我们即可直接得到姿态的三个参数。

      移植官方 DMP驱动库并不难，下面我们来讲解一下如何移植。

     首先打开我提供的资料里面的文件夹【MPU6050+上位机】，此文件夹为我移植成功的工程文件夹。移植的平台是正点原子miniSTM32开发板V3.0，并增加3.2寸LCD显示屏。

这个是360云盘地址：https://yunpan.cn/OcPnF2zNFdbzvU 访问密码 d4ba。下载并解压，如下图.

USER：里面有keilu5的工程文件，最好用keil5打开。

HARDWARE：里面是外设驱动文件，里面就是我们放的MPU6050库文件。

打开HARDWARE文件夹，看到MPU6050文件，把整个文件夹复制到你的工程里，里面包含了DMP库和模拟IIC文件。

打开USER文件夹中的keil工程文件。可以看到工程为；

然后，我们在MDK里面双击上图中的HARDWARE,在弹出的对话框中把【HARDWARE】里面的所有C文件ADD进去：

 

然后加入文件包含路径，操作如下图所示：

到此DMP的移植工作已经完成。下面简单分析程序的组成：

main函数中与普通的工程一样，一开始进行引脚配置的初始化。下面主要分析MPU6050的函数：MPU6050_initialize()和MPU6050_Pose();

在MPU6050.c文件中我们找到MPU6050_initialize()函数：

 

初始化时先配置模拟IIC，然后对DMP进行初始化，判断初始化函数返回值result，并把初始化的各个阶段串口打印。具体的模拟IIC配置请看STM32_IIC.c文件，由于是模拟IIC所以只要是普通IO引脚就可以。IIC中的读写时序请参考MPU6050的参考手册。

 

 

 

初始化操作完成后，直接使用while(1)循环读取MPU6050数据：MPU6050_Pose(Pose,Gyro,Accel);//Pose;姿态Gyro;陀螺仪Accel;加速度

可以在MPU6050.c中看到函数源代码：

#define q30 1073741824.0f

floatq0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;

floatPitch,Roll,Yaw;

floattemp_p,temp_r,temp_y;

unsigned longsensor_timestamp;

shortsensors;

unsigned charmore;

longquat;

voidMPU6050_Pose(float *pose ,short *gyro,short *accel)

{

dmp_read_fifo(gyro, accel,quat, &sensor_timestamp, &sensors,&more);

if (sensors& INV_WXYZ_QUAT )//姿态量

{

//PrintChar("in Calculating quaternion steps.....\n");

q0=quat[0] /q30;

q1=quat /q30;

q2=quat /q30;

q3=quat /q30;

Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //pitch

Roll = atan2(2* q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //roll

Yaw =atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) *57.3;//yaw

pose[0] =Pitch;//俯仰 绕Y轴

pose =Roll;//横滚 绕X轴

pose =Yaw;//航向 绕Z轴

UART1_ReportIMU((int)(Yaw*10),(int)(Pitch*10),(int)(Roll*10),1,1,1,100);//上位机数据发送函数

}

 

}

上位机发送函数的协议也在usart.c中：

 

voidUART1_ReportIMU(int16_t yaw,int16_t pitch,int16_troll

,int16_talt,int16_t tempr,int16_t press,int16_t IMUpersec)

{

 unsigned int temp=0xaF+2;

charctemp;

UART1_Put_Char(0xa5);

UART1_Put_Char(0x5a);

UART1_Put_Char(14+2);

UART1_Put_Char(0xA1);

if(yaw<0)yaw=32768-yaw;

ctemp=yaw>>8;

UART1_Put_Char(ctemp);

temp+=ctemp;

ctemp=yaw;

UART1_Put_Char(ctemp);

temp+=ctemp;

if(pitch<0)pitch=32768-pitch;

ctemp=pitch>>8;

UART1_Put_Char(ctemp);

temp+=ctemp;

ctemp=pitch;

UART1_Put_Char(ctemp);

temp+=ctemp;

if(roll<0)roll=32768-roll;

ctemp=roll>>8;

UART1_Put_Char(ctemp);

temp+=ctemp;

ctemp=roll;

UART1_Put_Char(ctemp);

temp+=ctemp;

if(alt<0)alt=32768-alt;

ctemp=alt>>8;

UART1_Put_Char(ctemp);

temp+=ctemp;

ctemp=alt;

UART1_Put_Char(ctemp);

temp+=ctemp;

if(tempr<0)tempr=32768-tempr;

ctemp=tempr>>8;

UART1_Put_Char(ctemp);

temp+=ctemp;

ctemp=tempr;

UART1_Put_Char(ctemp);

temp+=ctemp;

if(press<0)press=32768-press;

ctemp=press>>8;

UART1_Put_Char(ctemp);

temp+=ctemp;

ctemp=press;

UART1_Put_Char(ctemp);

temp+=ctemp;

UART1_Put_Char(temp%6);

UART1_Put_Char(0xaa);

 

}

到此可以对你的移植工程进行编译，修改好引脚配置后基本上是没有什么问题的。把编译好的工程下载到板子上，连接MPU6050模块。下面我们来看上位机效果：

打开文件夹中的上位机文件夹，打开AHRS_Captain_Flight.exe（如果是第一次使用需要进行一些配置才能使用3D效果的功能，请网上搜索）

点击文件操作-->端口设置-->选择串口号和波特率等，再打开即可，实时显示数据曲线：

点击3D显示按钮，显示3D效果：

到此整个MPU6050在STM32F103上的移植全部完成。

具体程序的执行和姿态DMP如果运算，请看官方MPU6050资料。也可参考我的下一篇博文四元数的姿态解析。