MPU6050 的研究筆記

1.  不使用DMP -> Ref 1

首先用 git 將 GitHub 上的程式碼都下載下來：

git clone https://github.com/richardghirst/PiBits.git

進入 PiBits/MPU6050-Pi-Demo 目錄：

cd PiBits/MPU6050-Pi-Demo

這個目錄包含了三個範例程式以及 I2Cdev 與 MPU6050 兩個類別，基本上整個程式架構都使用物件導向的方式規劃的很清楚，所以只要稍微懂一點 C++ 的人，應該都可以立即上手。

我們來看最簡單的 demo_raw.cpp，這一個程式是單純讀取 MPU-6050 的感測資料，然後輸出：

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;  // 預設 I2C 位址為 0x68
//MPU6050 accelgyro(0x69);  // 設定 I2C 位址為 0x68

int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  // 初始化 I2C 設備
  printf("Initializing I2C devices...\n");
  accelgyro.initialize();

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

void loop() {
  // 從 MPU-6050 讀取加速度計與陀螺儀資料
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 其他的讀取方式
  //accelgyro.getAcceleration(&ax, &ay, &az);
  //accelgyro.getRotation(&gx, &gy, &gz);

  // 輸出
  printf("a/g: %6hd %6hd %6hd   %6hd %6hd %6hd\n",ax,ay,az,gx,gy,gz);
}

int main() {
  setup();
  for (;;)
    loop();
}

這個程式碼非常簡單，我就不多作解釋了。由於 PiBits 中的專案都已經寫好 Makefile 了，所以直接執行 make 就以自動編譯：

make

然後執行

sudo ./demo_raw

正常的話，應該就可以看到類似這樣的輸出：

Initializing I2C devices...
Testing device connections...
MPU6050 connection successful
a/g:  16804     24  -3420     -113   -154    -68
a/g:  16664    -72  -3544      -89   -177    -78
a/g:  16800    -20  -3376     -100   -144    -48
a/g:  16676    -56  -3468      -87   -174    -84
a/g:  16712    -64  -3544     -116   -162    -41
a/g:  16776     28  -3528     -107   -157    -95

如果您執行之後出現這樣的錯誤訊息：

Failed to open device: No such file or directory

那有可能是因為 I2C 的設備位址指定錯誤，可以檢查一下自己的 I2C 設備：

ls /dev/i2c*

如果您的輸出是這樣：

/dev/i2c-1

那麼請將 I2Cdev.cpp 中所有的

open("/dev/i2c-0", O_RDWR);

改為

open("/dev/i2c-1", O_RDWR);

然後再執行一次

make

這樣就可以讓一般使者直接存取了。


2.  使用DMP (減輕RPI負擔) -> REF 2

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）的加速度：

如果要改變 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 官方的文件。



Reference  1: http://blog.gtwang.org/iot/raspberry-pi-read-data-from-mpu6050-using-cpp/

Reference  2: http://blog.gtwang.org/iot/raspberry-pi-mpu-6050-read-data-using-dmp/

Reference : http://gogoprivateryan.blogspot.tw/2014/07/mpu-6050-google.html