【FireFly试用体验】012-DMP姿态解算实验

一、简介

在上一讲中，已经可以读出MPU6050的加速度传感器和角速度传感器的原始数据。不过对于很多时候这些原始数据并不是那么的有用，特别是对于搞四轴，需要的是姿态数据，也就是欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。要得到欧拉角数据，就得利用原始数据，进行姿态融合解算，不过这个过程比较复杂。而MPU6050自带了数字运动处理器，即DMP，并且，InvenSense提供了一个MPU6050的嵌入式运动驱动库，结合 MPU6050的DMP，可以将原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到yaw、roll和 pitch。。这一讲将介绍如何使用内置的DMP进行姿态解算。

二、硬件电路

硬件电路和上一讲是一样的，这里就不再多说了。

三、软件工具

选用不同的上位机要使用不同的协议，关于协议这里不进行深入讲解。这里将用到匿名四轴上位机，要了解协议的可以上网查找，或者打开软甲后按下F12键，可以看到基本的介绍。

要使用DMP进行姿态解算，还需要DMP固件库，固件库中包含以下文件：

四、软件设计

打开上一讲的工程，把固件库文件夹复制到工程目录下：

打开工程，添加固件库文件到工程中，添加路径：

在inv_mpu.c文件中更改头文件信息：

在inv_mpu_dmp_motion_driver.c文件中更改头文件信息：

接下来需要在inv_mpu.c中添加以下几个函数：

该函数用于DMP库的初始化，可以看到该函数中调用了许多函数来设置mpu6050，这些函数也要一一实现，对于这部分代码就不多说了，在原来的代码上稍作修改即可，可以查看源代码。

接下来实现取得欧拉角的函数：

该函数根据DMP输出的四元数转换为欧拉角，欧拉角就是最终需要的数据。

到这里，DMP库的移植就基本完成了，但有一个很重要的细节不要忘记，DMP库默认mpu6050的IIC地址是0x68，但FireBLE上的mpu6050IIC地址为0x69，所以需要更改DMP中默认的IIC地址，找到下面的代码，更改地址：

接下来打开main.c文件，全部代码如下：

在main.c文件中首先实现和上位机通信的几个函数，然后在main函数中初始化MPU6050和DMP库，然后在死循环中得到欧拉角和原始数据，上传到上位机中。在这里为了提高传输速率，把和LCD有关的函数全部删除了。

五、实验结果

程序下载到开发板后，打开匿名四轴上位机，设置波特率，选择串口，然后点击打开串口：

可以看到上位机已经接收到串口传来的数据。

打开高级收码，然后选择左边栏的波形显示，可以看到六轴原始数据的波形：

选择左侧栏的飞控状态可以看到开发板的姿态，随着开发板的翻动，上位机上的模型相应的翻动。

    上位机：

    DMP库：

    源代码：

    不知道为啥文件上传后下载总会变成php，下载后把后缀改成zip貌似也能解压。