之前做四轴的时候调试过姿态传感器，由于是穷逼学生，没钱买好的陀螺仪，买的是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
快三稳赚10大技巧[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