Ameba Arduino: [RTL8195] [RTL8710] MPU6050 – 使用 MPU6050 六軸慣性感測元件

材料準備

  • Ameba x 1
  • MPU6050 x 1

範例說明

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官方網站的一樣)

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

RTL8710接線圖如下:
1

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

這六個值分別是:
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)

 

使用Digital Motion Processor


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關係,在接線上需要多接一根線:
3
編譯並上傳至Ameba之後,按下Reset鍵。打開Serial Monitor,會提示鍵入字元之後啟動DMP
4

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

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

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

程式碼說明


除了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