EEPW论坛

在上一节中，我们初始化时约定了Z轴向上，也即知道了初始的角度；再加上后续获取的实时的角速度，利用角速度进行积分，就可以获取姿态角信息。在实际编程实现时，积分就是累加和，随着时间的增加，积分（累加）的数据越来越多，误差也会越来越大。因此，单独使用陀螺仪的角速度去计算得到姿态角是不行的，误差会越来越大，需要使用加速度计的值来修正。为什么加速度计的值可以来修正角度呢？我们知道，加速度计能测到重力加速度的方向，只要飞行器的运动不算太剧烈，就能近似地认为在一段时间里，加速度计的测量值只有重力，而且姿态角的变化只转过了很小的一个角。
我们仍以下图为例，假定初始时Z轴向上，绕X轴旋转的角度为滚转角；绕Y轴旋转的角度为俯仰角，绕Z轴旋转的角度为偏航角。那么，滚转角（绕X轴转）和俯仰角（绕Y轴转）发生变化时，MPU6050测出来的重力加速度g的方向也会发送变化。而加速度计测出来的重力加速度g的方向是实时的，不会受长时间计算累加的影响。所以，我们可以用加速度计测到的g的方向，来修正陀螺仪积分得到的滚转角和俯仰角。而偏航角（绕Z轴转）变化时，MPU6050测到的重力加速度g方向是不会变化的，所以g的方向不能用来修正偏航角。要想修正偏航角，需要测量地磁偏角的传感器信息。这个用加速度计或磁力计来修正姿态角的过程，一般称为数据融合，有多种方法实现，如果想要自己写算法实现，有互补滤波、卡尔曼滤波等方法，或者直接使用MPU6050官方提供的DMP库。DMP全称Digital Motion Processor，是Invensense公司针对其产品推出的软件包，能直接输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合，适用于MPU6050、MPU6500、MPU9150、MPU9250等传感器。本文就是重点介绍DMP库的移植和使用。这里再提一点浅显的四元数的知识，四元数是一种表示姿态角的方法，可以认为四元数等价为滚转角、俯仰角、偏航角。在姿态解算时，之所以要使用四元数，是因为它可以简化姿态更新的计算过程。MPU6050的DMP库能直接输出四元数，是将陀螺仪的角速度、加速度计的加速度融合之后的结果，由该输出的四元数表示的姿态角是经过加速度计修正过的。

MPU6050官方的DMP库是适用于MSP430的，我们使用时，需要简单地移植一下。这里使用的是5.1版本的库，官方库一共只有下面这几个文件：另外官方库中有一个适用于MSP430单片机的app程序，就是motion_driver_test.c文件，但是这个文件里有比较多的和上位机通信用的测试代码，实际需要的很少，我们可以参考它写自己的应用程序。下面，我们就开始移植DMP库。首先，将上图的文件都拷贝到已有的MPU6050工程目录下，添加到keil工程中。打开inv_mpu.c文件，可以看到文件起始有一串宏定义，这是我们需要移植的主要部分。文件中可以看到定义了几种处理器类型，以MSP430为例，定义了I2C读写函数i2c_write、i2c_read，延时函数delay_ms、get_ms，打印log函数、取绝对值函数、比较大小函数等等。我们仿照它编写适用于STM32的函数如下：I2C的读写适用HAL库的读写函数实现，延时函数也使用HAL已有的HAL_Delay和HAL_GetTick实现。找到inv_mpu.c文件中的mpu_init函数，将其中下面几行注释掉：在inv_mpu.h文件中找到struct int_param_s{}结构体定义的地方，在里面增加一个成员void *temp，否则编译会报错，因为我们没有定义结构体中的宏，会导致结构体内是空的（使用其他的修改方式也行，只要没有语法错误，能编译通过就行）：在inv_mpu_dmp_motion_driver.c这个文件中，开始处添加宏定义，与inv_mpu.c中的实现是一样的：做完上面这些，编译可以通过，MPU6050官方的DMP库就移植好了。

移植完以后，我们可以参照官方的例子motion_driver_test.c，来编写我们自己的应用程序。主要就是两步：初始化和循环获取数据。首先我们看初始化，就是mpu_dmp_init这个函数，具体的实现如下：具体每个步骤的含义注释都有，函数实现也是官方例子里都有的，就不一一解释了。需要注意的是，执行run_self_test()这一步时，调用了mpu_run_self_test这个函数，官方的函数说明中，要求自行自检时，Z轴竖直向上或者竖直向下：获取数据的函数如下：就是调用dmp_read_fifo函数，获取融合后的四元数，放在quat数组中；然后将四元数，除以1073741824.0f（q30）的系数，再计算反三角函数得到俯仰角、滚转角、偏航角：整个测试函数如下所示，先调用mpu_dmp_init初始化，然后主循环中周期性地调用mpu_dmp_get_data获取姿态角：将DMP_test函数放到main函数硬件初始化之后。编译下载运行。可以看到，初始时输出的姿态角：运动中输出的姿态角：长时间运行，也能保持较高的准确度，姿态角不会产生误差累积，说明DMP库起到了融合陀螺仪和加速度计数据的作用。好了，本节关于MPU6050的DMP库的移植和使用就讲到这里了，希望这篇文章能对大家有所帮助。