使用 MPU-6050 的 DMP 來取得更精準的運動感測資料

這裡介紹如何在樹莓派 Raspberry Pi 中使用 MPU-6050 的 DMP 來取得更精準的運動感測資料。

MPU-6050 加速度計與陀螺儀六軸感測器內建的 Digital Motion Processor(DMP)可以負責一些運動處理演算法(motion processing algorithm)的計算,DMP 可從加速度計(accelerometers)、陀螺儀(gyroscopes)或第三方的感測器上讀取資料,透過 DMP 的暫存器來讓使用者讀取運算的結果,或是將運算結果放進 FIFO 中。

DMP 主要的用途在於處理即時性的需求,並且分擔一些運算的工作,一般來說 DMP 會以很高的計算速度(大約是 200Hz)來進行運動處理演算法的運算,降低延遲以提供精準的數據,甚至在低取樣速度(如 5Hz)的應用上,DMP 還是會保持這樣的運算速度,以確保資料的精準性。


前一篇文章中,我們使用了 PiBits 這個專案的 demo_raw.cpp 讀取原始的感測資料,而接下來我們繼續來看第二個使用 DMP 的範例程式 demo_dmp.cpp,因為這個程式碼比較長,所以就不全部貼上來了,我只說明比較重要的部分。

程式一開始先呼叫 setup() 進行初始化:

// 初始化 I2C 設備
printf("Initializing I2C devices...\n");
mpu.initialize();

// 測試一下連線是否正常
printf("Testing device connections...\n");
printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");

// 載入與設定 DMP
printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();

接著啟用 DMP:

// 開啟 DMP
printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);

在讀取資料之前,必須先確認 DMP 的封包大小:

// 取得 DMP 封包大小
packetSize = mpu.dmpGetFIFOPacketSize();

這樣就完成初始化的動作了,接著就開始進入主要的無窮迴圈,重複呼叫 loop() 讀取資料。在 loop() 中,先取得 FIFO 目前的大小:

fifoCount = mpu.getFIFOCount();

然後檢查看看 FIFO 是否有溢位的狀況,如果 FIFO 沒有溢位,再檢查 FIFO 資料大小是否超過一個 DMP 封包大小,如果超過的話,就可以讀取一個封包的資料進來:

if (fifoCount == 1024) { // FIFO 溢位
  // 重設 FIFO
    mpu.resetFIFO();
  printf("FIFO overflow!\n");

// 檢查 FIFO 中 DMP 的資料是否已經可以讀取了
} else if (fifoCount >= packetSize) {
  // 從 FIFO 中讀取一個 DMP 封包資料
  mpu.getFIFOBytes(fifoBuffer, packetSize);

  // 解析 DMP 封包,並輸出資料 ...[略]
}

由於 MPU-6050 的 FIFO 緩衝區的大小是 1024 bytes(請參考 MPU-6050 的官方說明文件),所以這裡我們依據 fifoCount 的值是否為 1024 來判斷 FIFO 是否有溢位。

將資料從 MPU-6050 的 FIFO 讀出來之後,會儲存在 fifoBuffer 這個陣列中,而接下來就是要解析這個 DMP 封包,輸出自己想要的數值資料,在 demo_dmp.cpp 中,有很多寫好範例,例如輸出去除重力(gravity)的加速度:

#ifdef OUTPUT_READABLE_REALACCEL
  // 實際的加速度(去除重力)
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetAccel(&aa, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  printf("areal %6d %6d %6d    ", aaReal.x, aaReal.y, aaReal.z);
#endif

如果要改變 DMP FIFO 的更新速度,可以在 Makefile 中透過 DMP_FIFO_RATE 調整:

CXXFLAGS = -DDMP_FIFO_RATE=9 -Wall -g -O2 `pkg-config gtkmm-3.0 --cflags --libs`

這裡 FIFO rate 的計算公式為

FIFO Rate = (200Hz / (1 + DMP_FIFO_RATE))

如果 DMP_FIFO_RATE 設為 9,則計算出來的 FIFO rate 就是 20Hz,以此類推。

這裡只是簡要的敘述程式的重點,如果想要了解整個程式的細節,還是需要從原始碼一行一行來看才有辦法,另外最好也先看過 InvenSense 官方的文件

有了擷取資料的程式之後,我打算在樹莓派上面用 Node.js 架設一個間單的網頁伺服器,將 MPU-6050 的資料即時轉送到網頁上,讓使用者只要打開瀏覽器就可以立即看到目前所收集到的資料,實作過程請參考樹莓派 Raspberry Pi 使用 Node.js 與 WebSocket 呈現即時性的 MPU-6050 感測器資料

如果您對於樹莓派的應用有興趣,建議您可以繼續閱讀物聯網的相關文章。

參考資料:Geek Mom Projectsi2cdevlib

樹莓派, 物聯網

2 Comments

  1. anonymouse

    您好 想請教一下,
    我看了6050的register map,但是我找不到有關於packetsize的資料,在mpu6050的library是預設42bytes,請問這個Packetsize是什麼?為什麼設42bytes?

    謝謝

  2. 好文章,写的很详细

Leave a Reply