MPU6050中DMP参考面使用绝对水平面的方法

之前做四轴的时候调试过姿态传感器，由于是穷逼学生，没钱买好的陀螺仪，买的是GY86模块，上面集成了一块六轴传感器MPU6050和一块HMC5883L磁力计。mpu6050计算出pitch和roll，用HMC5883L去结合pitch和roll修正yaw，完成姿态角的输出。

当时第一次接触姿态解算的时候使用的是原子中的例程，里面使用mpu6050自带的DMP进行解算，直接输出四元数，然后通过四元数转化转换为对应的姿态角，但是没有Z轴上的参考量，所以Yaw是不准的。讲真，个人感觉算出的姿态角还是有点稳定的，除了动态性能不好，恢复较慢，有时会挂之外。但是有一个很大缺点就是DMP库是不开源的，也没有参考资料，不知道里面怎么写的，解算出的姿态角是以上电时的平面为基准面的，又不能自己修改。这就很蛋疼，难道我每次上电都要把四轴摆放到绝对水平的位置吗？那岂不是稍微有点不平，起飞就炸机了= =。

后来就尝试使用了各种解算算法，互补滤波，一二阶，梯度向下等等，虽然解决了DMP解算的很多问题，但这个问题还是让我膈应了很久。

这段时间做比赛又需要调试陀螺仪了，所以想着之前没解决这个问题，索性就研究了一下午。大胆的拆开DMP库源码看了之后发现了问题。

在DMP初始化后面有一个函数run_self_test()，进去查看之后发现了这么一段代码

        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);

        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);

研究了一下发现这应该是DMP修正偏置量（就是我们说的零飘）的过程，只不过把读出来的AD值转化了一下存到了寄存器中。

了解了这个，那么把基准坐标系转化到水平面上就很简单了（这个不清楚的话请查阅一下 “载体坐标系” 和 “世界坐标系”），测量出置于水平面上的偏置量，然后传进去就可以了
比如我是这样写的：

        mpu_get_gyro_sens(&sens);   
        gyro[0] = (long)(GX_OFFSET * sens);
        gyro[1] = (long)(GY_OFFSET * sens);
        gyro[2] = (long)(GZ_OFFSET * sens);
        dmp_set_gyro_bias(gyro);

        mpu_get_accel_sens(&accel_sens);
        accel[0] = AX_OFFSET * accel_sens;
        accel[1] = AY_OFFSET * accel_sens;
        accel[2] = AZ_OFFSET * accel_sens;
        dmp_set_accel_bias(accel);

大概思路就是这样，这样不管上电位置是怎样的，算出的角度仍旧是相对于水平面的坐标系。

至于怎么测量水平的偏置量之前没有说，在自检函数里有个获取上电水平偏置的函数，大家可以研究一下，很容易就可以获取到DMP寄存器中的水平偏置量，也可以参考下我的代码。

最后贴上自己的渣渣代码供大家参考一下（使用的战舰开发板，LCD屏幕显示，在git上自己下载~）。
Gyro_DMP.git