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;	// roll
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;					// pitch
Yaw   = 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;	// roll
if(fabs(Roll) < 90.0) 	Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	
else			Pitch = (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的變化也很劇烈了）