EEPW论坛

我们期望得到的是姿态数据，也就是平衡小车的倾角。要得到平衡小车的倾角，就得利用我们的原始数据，进行姿态融合解算，这个比较复杂，知识点比较多，初学者不易掌握。其实， 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[1] / q30;

q2=quat[2] / q30;

q3=quat[3] / 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*q1-q2*q2-q3*q3) * 57.3; //航向角

这里，我们是做平衡小车而不是四轴飞行器，所以，仅仅使用pitch就行了。其他两个角度直接在程序中屏蔽了，因为反三角函数运算在STM32F1中还是需要不少时间的。

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

首先打开我们提供的资料里面的文件夹【DMP移植相关文件和工程】，可以看到如下文件 （最下面有下载链接）

其中库文件包含了模拟IIC的初始化和相关函数，普通工程就是一个简单的工程。比如我们常用的开发板资源里面的流水灯实验工程代码就行，也可以使用你们自己的工程。【移植成功的工程】是移植好了的，大家可以先不看。

首先，我们先把【库文件】里面的三个文件夹都拷贝到【普通工程】里面的【HARDWARE】文件夹。

然后在【普通工程】的【USER】里面找到minibalance打开。

会出现如下界面

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

完成后工程如下图所示

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

然后，把下面的文件包含到SYS.H里面

#include "delay.h"
#include "usart.h"
#include "ioi2c.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "dmpKey.h"
#include "dmpmap.h"
#include  
#include 
#include 
#include 
#include 
#include 

成功后如下图所示

然后回到主函数，调用IIC和MPU6050的初始化函数

至此，程序移植就完成了，然后我们来到void Read_DMP(void)函数里面，添加串口打印语句

printf("%f\r\n",Pitch);，成功后代码如下所示：

void Read_DMP(void)

{ 

 unsigned long sensor_timestamp;

 unsigned char more;

 long quat[4];

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

   if (sensors & INV_WXYZ_QUAT )

   {    

     q0=quat[0] / q30;

     q1=quat[1] / q30;

     q2=quat[2] / q30;

     q3=quat[3] / q30;

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

     printf("%f\r\n",Pitch);

   }

}

还有，请大家注意MPU6050连接STM32的IO,因为是模拟IIC，所以可以随便接，只要IO初始化合适就行了

#define IIC_SCL    PAout(11) //SCL
#define IIC_SDA    PAout(8) //SDA
#define READ_SDA   PAin(8)  //输入SDA
在本实验中，我们SDA接PA8 ,SCL接PA11

至此，我们就可以把程序下载进去验证了。串口调试助手会显示MPU6050的俯仰角Pitch。

——回复可见内容——