如官網上說的，擷取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
//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旋轉的效果會比六軸好
網路資源，網路資源二，資源三感覺資源二可以試試看。
如果還是會造成時序衝突，那就用之前碩論的卡曼，可能還比較好一點。
，
可直接積分得到
，末速度
，
