MPU6050 – 使用 MPU6050 六軸慣性感測元件

MPU6050是款常見的6軸慣性感測元件，它包括了三軸加速度計及三軸陀螺儀。使用與讀取資料的介面為I2C，可以控制的 細節相當多，我們可以找不少開源的library可供使用。
I2CDev是其中知名的library，你可以找到原始的source code。

Ameba使用MPU6050需要2個library，I2CDev與MPU6050, 請到底下的位址下載:

• I2CDev
  Ameba沒有修改I2CDev裡面的程式碼，只是打包好讓使用者下載
• MPU6050
  Ameba沒有修改API的內容，只有根據實際使用狀況修改example。

安裝library的方式請參考Arduino官方網站的教學文章將zip檔的library加入Ameba：
https://www.arduino.cc/en/Guide/Libraries#toc4

接著打開範例 “Files” -> “Example” -> “AmebaMPU6050” -> “MPU6050_raw”。（這個範例與I2CDev官方網站的一樣）

這個範例裡展示讀取三軸加速度計及三軸陀螺儀的值，我們先接線如下:

RTL8710接線圖如下:

編譯並上傳程式碼，按下Reset之後可以在Serial Monitor看到一直印訊息：

這六個值分別是：
1. x軸的加速度
2. y軸的加速度
3. z軸的加速度
4. x軸的角速度
5. y軸的角速度
6. z軸的角速度

值的範圍是 -16383 ~ 16384，其中可以看到z軸因為重力加速度的關係，值會接近16384
這些值會因為干擾而一直變動，可以轉動一下MPU6050觀察一下值的變化。

雖然程式碼很長，但重要的只有幾個。第一個是初始化: initialize()
初始化完就可以取值了: getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)

MPU6050搭載了一顆DMP(Digital Motion Processor)，可以負責一些運動處理演算法，像是轉換成飛機三軸yaw/pitch/roll，轉換四元數(Quaternion)，或是轉換成尤拉角(Euler angle)。
DMP會以高速計算，完成後發出GPIO Interrupt訊號在INT這根Pin上。計算結果會存在FIFO裡，Ameba如果一直都沒將結果取出，會造成FIFO overflow，並且下一次讀出的結果不正確。這種情況將FIFO reset即可。
另一個狀況要注意的是，當MPU6050發出GPIO Interrupt的時候，如果正好在處理I2C資料的送收，那麼MPU6050會停住，並且再也不會有任何動作。MPU6050沒有Reset或Enable Pin，這種情況下只能將MPU6050斷電再插電。因此寫程式的時候要注意，如果要處理要花時間的運算，最好等到下一次GPIO Interrupt發生再對MPU6050送收I2C資料。
我們打開範例 “File” -> “Examples” -> “AmebaMPU6050” -> “MPU6050_DMP6”。（與I2CDev官方網站的範例有些設定上的差異。）
因為打開DMP關係，在接線上需要多接一根線：

編譯並上傳至Ameba之後，按下Reset鍵。打開Serial Monitor，會提示鍵入字元之後啟動DMP

鍵入字元之後，會出現ypr的值

ypr即Yaw/Pitch/Roll，是飛機飛行常用的表示法

範例裡還有一些其它的輸出方式可以參考。

除了MPU6050的Initialize()之外, DMP也需要初始化 mpu.dmpInitialize()
可以用底下這些API對初始值做校正:
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
啟動DMP: mpu.setDMPEnabled(true);
設定Ameba在要哪根GPIO傾聽Interrupt，我們設定為D3。當interrupt發生的時候，呼叫function dmpDataReady()
attachInterrupt(3, dmpDataReady, RISING);
取得DMP每次計算結果的封包大小
packetSize = mpu.dmpGetFIFOPacketSize();
設定DMP的運算速度，這個值預設值是4，也就是 1k/(1+4) = 200Hz，但是會因為印Log很花時間的關係，造成Ameba來不及在下一次Interrupt發生的時候印完log，造成有機會在I2C送收的時候發生Interrupt，隨即MPU6050就停住不動了。所以需要適當選擇rate，如果rate的值太高(sample rate太低)，會讓DMP的反應速度沒那麼快。
mpu.setRate(5); // 1khz / (1 + 5) = 166 Hz
接著在loop()裡面檢查是否有interrupt發生，如果有就將結果取出
fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
範例裡預設使用OUTPUT_READABLE_YAWPITCHROLL，並各種轉換
mpu.dmpGetQuaternion(&q, fifoBuffer); // 四元數
mpu.dmpGetGravity(&gravity, &q); // 取得重力的值
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); // 轉換成yaw/pitch/roll