MPU6050に真価を発揮させる

前回記事

の続きです。
6軸センサのMPU6050は、DMP(Digital Motion Processor)という内部処理機能を利用して、センサーフュージョンを手軽に実現することができます。
今まではセンサーの生の値をチラチラ覗き見していましたが、DMPを使ってこそMPU6050の真価が発揮されようというものです。

やること

センサーを校正した後、DMPを使うデモスクリプトを実行し、結果をProcessingでビジュアライズします。

準備物・実施環境

Arduino系のもの（今回の狙いはTeensy3.2)
processing3
6軸センサ　GY-521(MPU6050)
手元の環境はMac

参考

使用ライブラリおよびサンプルスケッチ

https://github.com/electroniccats/mpu6050

ライブラリやサンプルの使い方の参考

大変参考になりました。ありがとうございました。
※当記事はほぼホームメードガービッジ様の記事をなぞる形になっています。

接続

GY-521	Arduino
VCC	5V
GND	GND
SCL	SCL(Pin19)
SDA	SDA(Pin18)
XDA	none
AD0	none
INT	none

SCL0はArduinoUno,Teensy3.2共にPin19です。
SDA0はArduinoUno,Teensy3.2共にPin18です。
接続は電源2本、信号2本の計4本で動きます。

準備

ArduinoIDEの「ツール」→「ライブラリの管理」より、MPU6050を検索してインストールします。いろいろなタイプがありますが無印の「MPU6050」(by Electronic Cats)を選びます。

センサの校正

ArduinoIDEの「ファイル」→「スケッチ例」→「MPU6050」→「IMU_Zero」を選択しスクリプトを開きます。
センサを水平に置いて、スクリプトを実行します。

校正が完了するまで数分かかります。
下図のような結果が表示されます。-- done -- が出れば完了です。

-- done --の上の一行が反映すべきオフセット値です。
図の例ではXAccelであれば[-2053]が、YAccelでは[-1798]がオフセット値となるようです。この６つの値をコピペなどでキープしておきます。

Arduino系側のスケッチ

サンプルスクリプトの「MPU6050_DMP6」を開きます。
205行目あたりにある設定値を書き換えます。

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXAccelOffset(-2053);
  mpu.setYAccelOffset(-1798);
  mpu.setZAccelOffset(1452);// 1688 factory default for my test chip
  mpu.setXGyroOffset(102);
  mpu.setYGyroOffset(-3);
  mpu.setZGyroOffset(-15);

次に103行目の

#define OUTPUT_READABLE_YAWPITCHROLL

が有効になっていることを確認します。
他の行の#defineのコメントアウトをオンオフすることで、出力される値をクオータニオンやTEAPOT形式に切り替えることもできるようです。

前回作ったProcessing用スクリプトに合わせるため、OUTPUT_READABLE_YAWPITCHROLLの出力内容を下記のように若干変更します。

#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    //Serial.print("ypr\t");
    Serial.print(ypr[2] * 180 / M_PI);
    Serial.print(",");
    Serial.print(ypr[1] * 180 / M_PI);
    Serial.print(",");
    Serial.println(ypr[0] * 180 / M_PI);
#endif

シリアルの出力書式と、値の出力順をrpyの順になるように変更しています。

今回必要そうな部分のみ抜粋して関数にまとめたArduino系（Teensy）用のコード全文はこちらになります（クリックで表示）

//※サンプルスクリプトに含まれていた初期化メッセージやエラーメッセージ表示などは
//省略しています。

#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;

// MPU control/status vars
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [roll, pitch, yaw]   roll/pitch/yaw container and gravity vector

void setupMPU() {
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  mpu.initialize();
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXAccelOffset(-2027);
  mpu.setYAccelOffset(-1794);
  mpu.setZAccelOffset(1446);
  mpu.setXGyroOffset(105);
  mpu.setYGyroOffset(-3);
  mpu.setZGyroOffset(-16);

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.setDMPEnabled(true);
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    Serial.print("DMP Initialization failed.");
  }
}

void getYawPitchRoll() {
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print(ypr[2] * 180 / M_PI);
    Serial.print(",");
    Serial.print(ypr[1] * 180 / M_PI);
    Serial.print(",");
    Serial.println(ypr[0] * 180 / M_PI);
  }
}

void setup() {
  Serial.begin(115200);
  setupMPU();
}

void loop() {
  getYawPitchRoll();
}

実行

Arduino側のプログラムは以上です。
実行してArduinoIDEのシリアルモニタを開きます。
シリアルモニタから何か文字を送信すると、センサの出力が開始します。
ピリオド区切りの3つの値が流れ始めたら成功です。
動作を確認できたらシリアルモニタを一旦閉じておきます。（開いているとProcessing側のシリアルが実行できません）

Processing側のスケッチ

Processing4

import processing.serial.*;
Serial myPort;
printArray(Serial.list());

まずProcessing3の画面で上記のスケッチを実行します。
Processing3のスクリプト画面の下段にあるシリアル通信のコンソール画面がに、シリアルポートのリストが出力されます。
ここで、Arduino系で使っているシリアルポートと同じものの番号を調べて控えておきます。
Arduinoで使用中のポートはArduinoSDKのメニューから「ツール」→「シリアルポート」で確認できます。

Processing4

import processing.serial.*; // シリアルライブラリをインポート

Serial myPort;  //シリアルポートのインスタンス
int available_serialport = 2; // シリアル検索プログラムで調べたシリアルポートの番号に設定数値を変更しておく
String arduinoPort = Serial.list()[available_serialport ]; //シリアルポートの設定
float []data = new float [6]; //シリアルのデータを格納する配列を宣言

void setup() {
  lights();
  size(300, 300, P3D); // キャンバスサイズ
  myPort = new Serial(this, arduinoPort, 115200); // シリアルポートの設定
}

void draw() {
  background(230); //背景グレー
  translate(width / 2, height / 2, 0); // 原点を図形の中心に
  rotateX(radians(data[0])*90); //dataの中身は重力加速度gなので、90を掛けると角度に概算できる
  rotateZ(radians(-data[1])*90); //dataの中身は重力加速度gなので、90を掛けると角度に概算できる

  int size = 10;//図形のサイズ倍率。キャンバスを大きくする時に変更可
  box(20 * size, 1 * size, 15 * size); //GY-521基盤のような直方体を描く
  translate(0, -4 * size, 0);
}

void serialEvent(Serial p) { //シリアルを監視
  String inString = myPort.readStringUntil('\n'); //データがあったら改行のところまで読み込む
  if (inString != null) { //シリアルの文字列データが何か入っていれば
    inString = trim(inString); //シリアル文字列の前後の空白を削除
    data = float(split(inString, ',')); //ピリオドで分割して配列に格納
    println(data);//受信した配列データをprocessingのコンソールに出力
  }
}

実行

Arduinoからシリアル信号が送られている状態で上記のProcessing用スケッチを実行すると、画面に板が現れます。
Arduinoに接続したセンサを傾けると、画面の中の板もシンクロして傾くはずです。
ヨー軸側の回転にもしっかり反応してくれるのが嬉しいです。

手元のセンサと画面の板の回転方向が合わない場合には、Processingの17~19行目のプラスマイナスを変更してみてください。

ただし、今回紹介したスクリプトの場合、ピッチ軸を90度付近まで上げるとセンサからの出力値がちょっと変なことになります。
結果、画面上の板がそこでクルンと反転してしまう現象が発生します。
出力値の変化の様子についてはArduino IDEの「ツール」→「シリアルプロッタ」で確認すると分かりやすいです。