MPU6050使用dmp时，Pitch范围只有-90-90的问题

最近在用MPU6050的DMP输出姿态，基本上就是照搬网上的代码，然后略微修改。不过有两个地方可能需要特别注意一下：

1.自检。

自检就是在MPU初始化函数中的下面这句话：

run_self_test();

查看函数定义如下：

void run_self_test(void){    int result;//    char test_packet[4] = {0};    long gyro[3], accel[3];    result = mpu_run_self_test(gyro, accel);    if (result == 0x07) {//if(1){        /* Test passed. We can trust the gyro data here, so let's push it down         * to the DMP.         */        float sens;        unsigned short accel_sens;        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);PrintChar("setting bias succesfully ......\n");    }else{PrintChar("bias has not been modified ......\n");}}

总之很多人在使用DMP时的那句"bias has not been modified"出处就在这里了。

仔细看一下代码，这个自检不成功的原因是result不等于0x07，再经过一层又一层第剖析函数定义后，发现result意义如下：

0x07即0b00000111，前面5个固定是0，后面的3个1从左往右分别代表磁力计，加速度计，陀螺仪校准是否成功，1为成功。

很显然MPU6050没有磁力计所以绝不可能是0x07，加速度计貌似在绝对水平时才能为1，有参考价值的只有陀螺仪的校准。所以改成“result == 0x01”即可。当然不作判断也可以，反正都能用。

我在自检中还碰到一个问题：DMP初始化稳定时间太长，尤其是在“底朝天”姿态时。对此我强制把accel_sens设为0（大概就是不考虑当前姿态，以绝对姿态作参考吧），不过Pitch稳定下来还是要30s左右。。。

2.姿态换算。

原来的Pitch范围是-90~90，Roll范围-180~180，Yaw范围-180~180，四元素换算代码如下：

Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;// rollPitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitchYaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw

Pitch范围扩展为-180~180，四元素换算代码如下：

Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;// rollif(fabs(Roll) < 90.0) Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;elsePitch = (fabs(q0* q2 - q1 * q3)/(q0* q2 - q1 * q3)) *(180.0 - fabs(asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3));Yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw

好处是强迫症看起来比较爽，坏处是在-90和90度时，Pitch会突变（其实Roll的变化也很剧烈了）