[無人機]姿態擷取問題

如官網上說的，擷取raw值很容易，剩下的很麻煩，
其實，歸功於許多開源的前輩，在ardunio上已經很容易取得raw值了，

至於如何估算姿態？
用MPU6050有個好處，裡面的DMP模組已經可以提供pitch,row,yaw角度了，
如果單純只做角度估測，DMP就夠用。

But....
四軸的控制，我們還有許多工作得處理，像是再加上其他的感測器如超音波，
就會造成時序的衝突，導致DMP或者超音波其中一個不work，

所以，拿掉DMP吧，如果你未來的內容不只是角度估測而已的話。

好家在，之前碩論做過，不過之前只用卡曼估算Pitch、Roll兩軸，很快就可以解決，
但是如果想加上Yaw軸，網路上多用四元數來解決，下面就紀錄一下這過程。

拿到MPU6050，有幾項工作得做，

一、確認raw值的scale範圍
在下面網頁有說明
http://www.i2cdevlib.com/forums/topic/4-understanding-raw-values-of-accelerometer-and-gyrometer/

在MPU6050.cpp初始化函示有提到，預設為+/-2g，+/-250degree/s
/** Power on and prepare for general usage.
 * This will activate the device and take it out of sleep mode (which must be done
 * after start-up). This function also sets both the accelerometer and the gyroscope
 * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
 * the clock source to use the X Gyro for reference, which is slightly better than
 * the default internal clock source.
 */
void MPU6050::initialize() {
    setClockSource(MPU6050_CLOCK_PLL_XGYRO);
    setFullScaleGyroRange(MPU6050_GYRO_FS_250);
    setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}

如果有必要修改，下面是修改方式

mpu.setFullScaleGyroRange(0);
//0 = +/- 250 degrees/sec | 1 = +/- 500 degrees/sec | 2 = +/- 1000 degrees/sec | 3 =  +/- 2000 degrees/sec
mpu.setFullScaleAccelRange(0);
 //0 = +/- 2g | 1 = +/- 4g | 2 = +/- 8g | 3 =  +/- 16g 

二、取得重力與角速度
在這個函式拿到xyz軸的加速度計重力值以及陀螺儀的角速度
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

三、四元數是啥?

幾個背景知識參考
四元數、四元數與旋轉
開源飛行器crazepony的軟體姿態解法，他的參考來源
對岸的參考資料有提到Z方向的需求要看應用
關於六軸與九軸的比較

一個簡單的範例，這個範例簡單好用，不過只有在運動時穩定，靜止時十分糟糕，
應該是沒有濾波的關係。


But....我發現我手邊有個九軸感測器，對於Yaw旋轉的效果會比六軸好

網路資源，網路資源二，資源三感覺資源二可以試試看。
如果還是會造成時序衝突，那就用之前碩論的卡曼，可能還比較好一點。