fc2ブログ

MPU-6050用ライブラリのプログラム

Jeff Rowberg氏のMPU-6050用ライブラリをインストールできたので、「MPU6050」ライブラリの「Examples」→「MPU6050_DMP6」フォルダ内にある、サンプルスケッチ「MPU6050_DMP6.ino」の処理をもとに、「MPU6050」ライブラリの基本的な使い方を紹介します。
「MPU6050」クラスのドキュメント
「MPU6050」クラスのドキュメント
http://www.i2cdevlib.com/docs/html/class_m_p_u6050.html
ただし、DMP関連のメソッドについては、ライブラリの「MPU6050_6Axis_MotionApps20.h」ファイルの記述を直接参照する必要があるようです。

グローバル
■インクルードファイル
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
「MPU6050」は「I2Cdev」を使うので、「I2Cdev.h」をインクルードします。
DMP(MotionApps)を使う場合は「MPU6050_6Axis_MotionApps20.h」をインクルードします。

「I2Cdev」がArduino SDK標準の「Wire」ライブラリを使う場合は、「Wire.h」をインクルードします（デフォルト）。

■「MPU6050」クラスのオブジェクト作成
MPU6050 mpu;
//MPU6050 mpu(0x69);
「MPU6050」クラスのオブジェクトを作ります。MPU-6050とのやり取りにはこのクラスを使います。
コンストラクタの引数は、デバイスのI2Cアドレスです。
デフォルトは「0x68」なので、「GY-521」のデフォルト状態の場合は省略できます。
「AD0」ピンをプルアップ(HIGH)した場合は、「0x69」を指定します。

■MPU6050処理用変数
bool dmpReady = false;  // DMPの初期化に成功したらtrueに設定
uint8_t mpuIntStatus;   // MPUからの割り込みステータス
uint8_t devStatus;      // デバイス処理時の戻り値(0：成功、0以外：失敗)
uint16_t packetSize;    // expected DMPパケットのサイズ(デフォルトは42バイト)
uint16_t fifoCount;     // FIFO内に現在ある全バイト数
uint8_t fifoBuffer[64]; // FIFOストレージバッファ

Quaternion q;           // [w, x, y, z]         クォータニオン
VectorInt16 aa;         // [x, y, z]            加速度センサの測定値
VectorInt16 aaReal;     // [x, y, z]            重力を除いた加速度センサの測定値
VectorInt16 aaWorld;    // [x, y, z]            重力を除いた加速度センサの測定値(クォータニオンで回転)
VectorFloat gravity;    // [x, y, z]            重力ベクトル
VectorInt16 gyro;       // [x, y, z]            角速度センサの測定値
float euler[3];         // [psi, theta, phi]    オイラー角
float ypr[3];           // [yaw, pitch, roll]   ヨー/ピッチ/ローと重力ベクトル
MPU-6050からの処理結果を格納する変数です。

■割り込み処理
volatile bool mpuInterrupt = false;
void dmpDataReady() {
    mpuInterrupt = true;
}
MPU-6050 (GY-521)からの割り込み処理用の変数と関数です。
割り込みがあると「mpuInterrupt」を「true」にします。

setup関数内の処理
■I2C処理開始
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
#endif
I2Cの処理を開始します。
「I2Cdev」ライブラリは、デフォルトでArduino SDKの「Wire」ライブラリを使いますが、オプションでGene Knight氏の「NBWire」ライブラリや、Francesco Ferrara氏の「Fastwire」コード、DSSCircuits氏の「I2C-Master」ライブラリをサポートしているようです。
使うライブラリは「I2CDEV_IMPLEMENTATION」の設定で切り替えます。（「Wire」ライブラリ以外はどう使うのかよく分かりませんが……）

■シリアル通信を開始
Serial.begin(9600);
while (!Serial);
MPU-6050とは直接関係ありませんが、処理結果をシリアル出力でArduino IDEのシリアルモニタに表示するため、シリアル通信を開始します。ここでは通信速度として、「9600 bps」を指定しています。

■MPU-6050デバイスの初期化
mpu.initialize();
MPU-6050デバイスを初期化します。戻り値はありません。

■MPU-6050デバイスの接続を確認
if(mpu.testConnection() != true) {
    return; // 接続失敗
}
I2Cデバイスに接続できたかどうか、確認します。成功しているとtrueが返ります。

■MPU-6050のDMPにファームウェアをロードし、初期化
devStatus = mpu.dmpInitialize();
if (devStatus != 0) {
    return; // 初期化失敗
}
MPU-6050のDMPを初期化するメソッドです。
このメソッドはドキュメントに記載がありませんが、戻り値は、
　0:成功
　1:main binary block loading failed
　2:configuration block loading failed
です。

■MPU-6050のオフセットを設定（例）
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1236：私のチップのデフォルト値
加速度と角速度のオフセットを設定して、値のズレを修正できます。
「setXAccelOffset」「setYAccelOffset」メソッドもあります。
また、get系メソッドで現在のオフセットを取得できます。
私の手元にあるチップのデフォルト値は、GyroOffsetが[0 0 0]で、AccelOffsetが[0 0 1236]でした。

■MPU-6050のDMPを有効にする
mpu.setDMPEnabled(true);
MPU-6050のDMPの有効/無効を切り替えるメソッドのようです。
このメソッドはドキュメントに名前以外の具体的な記載がありませんが、trueを渡せば有効になるようです。

■MPU-6050からの割り込み（Arduino NanoのD2ピン）を有効にする
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Arduino Nanoの割り込みピンはD2かD3ですが、ここでは「GY-521」の「INT」ピンを「Arduino Nano」の「D2」ピン（0）に接続しています。
割り込み時に呼び出される「dmpDataReady」は、すでに作成ずみの関数です。
「getIntStatus」メソッドで現在の割り込み状態を取得します。

■MPU-6050の初期化完了
dmpReady = true;
以上で、DMPの初期化が完了しました。「dmpReady」を「true」に設定しておきます。

■MPU-6050のDMPのFIFOパケットのサイズを取得
packetSize = mpu.dmpGetFIFOPacketSize();
このメソッドもドキュメントに記載がありません。
DMPの演算結果は、このパケットサイズ単位でFIFOに格納されるようです。

loop関数内の処理
■DMPからのデータを待つ
if (!dmpReady) return;

while (!mpuInterrupt && fifoCount < packetSize) {
    // 処理するデータを待ちながら、他の処理をする
}
DMPの初期化に失敗していれば、処理を終了します。

MPU-6050から割り込み処理があるか、FIFOに格納されているデータのサイズがパケットサイズを上回るのを待ちます。

■割り込み状態を取得
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
割り込みフラグをリセットして、現在の割り込み状態を取得します。

■FIFOバッファのデータ数を取得
fifoCount = mpu.getFIFOCount();
現在のFIFOバッファに格納されているデータ量を取得します。

■FIFOバッファのオーバーフローチェック
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    mpu.resetFIFO();
    return; // 処理終了
}
FIFOバッファがオーバーフローしていたら、FIFOバッファをリセットして、処理をやり直します。

■FIFOバッファからパケットデータを取得
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
FIFOに格納されているデータのサイズがパケットサイズを上回るのを待ちます。
その後、FIFOからパケットサイズ分のデータを取得します。

以下、取得したパケットデータから、必要な情報を取り出します。

■加速度をシリアルに出力
mpu.dmpGetAccel(&aa, fifoBuffer);
Serial.print("accel\t");
Serial.print(aa.x);
Serial.print("\t");
Serial.print(aa.y);
Serial.print("\t");
Serial.print(aa.z);
パケットデータから加速度を取得します。
【メソッド定義】
uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet);
uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet);
uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet);

■クォータニオンをシリアルに出力[w x y z]
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
パケットデータからクォータニオンを取得します。
【メソッド定義】
uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet);
uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet);
uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet);

■角速度をシリアルに出力
mpu.dmpGetGyro(&gyro, fifoBuffer);
Serial.print("gyro\t");
Serial.print(gyro.x);
Serial.print("\t");
Serial.print(gyro.y);
Serial.print("\t");
Serial.print(gyro.z);
パケットデータから角速度を取得します。
【メソッド定義】
uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet);
uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet);
uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet);

■重力をシリアルに出力
mpu.dmpGetGravity(&gravity, &q);
Serial.print("gravity\t");
Serial.print(gravity.x);
Serial.print("\t");
Serial.print(gravity.y);
Serial.print("\t");
Serial.print(gravity.z);
クォータニオンから重力を取得します。
【メソッド定義】
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q);

■オイラー角（度）をシリアルに出力
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
FIFOバッファからオイラー角を取得します。
【メソッド定義】
uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q);

■ヨー/ピッチ/ロール（度）をシリアルに出力
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
クォータニオンと重力からヨー/ピッチ/ロールを取得します。
【メソッド定義】
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);

■重力を除外した加速度をシリアルに出力
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
加速度から重力を除外した加速度を取得します。
【メソッド定義】
uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);

■重力を除外した加速度（クォータニオンで回転）をシリアルに出力
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
重力を除外した加速度ベクトルを、クォータニオンで回転させる処理です。
【メソッド定義】
uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
2016/02/23
1 Category : FIGROBO TYPE1
Prev Page Next Page

コメント

非公開コメント

承認待ちコメント

このコメントは管理者の承認待ちです

2019-09-10 08:00      編集