以前、各種モーションセンサを比較してみたのですが

スマート靴占い『 IしたoTんきになぁ〜れ』

9軸センサMPU9250のDMP動作が確認できず断念しておりました。
DMP(Digital Motion Processor)とはモーションセンサMPUシリーズに内蔵された自身の姿勢角を算出する機能です。

今回はsparkfunのライブラリでMPU9250のDMP動作が確認できましたので記載します。

構成

sparkfunのMPU9250のDMPライブラリがARM Cortex-M0ベースの SAMD ファミリのみ対応だったのでマイコンはAdafruit Itsy Bitsy M0 Expressを使用しました。

部品

マイコン Adafruit Itsy Bitsy M0 Express

　
9軸モーションセンサ MPU9250

MPU9250

6軸センサMPU6500(３軸加速度＋３軸ジャイロ)と３軸コンパスAK8963を搭載した9軸モーションセンサです。

DMPライブラリとして以下を使用しました。
　https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library

ARM Cortex-M0ベースの SAMDマイコンのみに対応しており、Arduino UNOなどには容量不足でコンパイルできません。。。

ライブラリのプログラム例 MPU9250_DMP_Quaternion を動作させてみました。

ただし26行目は以下のように修正しています。

//#define SerialPort SerialUSB
//以下に修正
#define SerialPort Serial

//#define SerialPort SerialUSB

//以下に修正

#define SerialPort Serial

MPU9250 DMPでクォータニオン pic.twitter.com/wdqqVJEA1r

— HomeMadeGarbage (@H0meMadeGarbage) 2018年10月20日

いい感じにクォータニオンが得られています♪

Adafruit Itsy Bitsy M0 Express

Arduino M0シリーズで小さくてかわいいので購入してみました。

スイッチサイエンスのサイトによると、どのピンでもNeoPixel8000個以上を時間消費せずに点灯可能とか記載あるのですごそうですね。。。今度試してみます。

Arduino IDE用設定

以下を参考に実施しました。
　https://learn.adafruit.com/introducing-itsy-bitsy-m0/using-with-arduino-ide

Arduino IDEのボードマネージャで以下のパッケージをインストールします。

Arduino SAMD Boards (32-bits ARM Cortex-Mo+)

　
Adafruit SAMD Boards

　
インストール後に以下でボードを設定してコンパイルが可能になります。Win7と8はドライバインストールが必要です。

DMP動作確認

以下のMPU6050用のクォータニオンでグラフィック動かすProcessingプログラムを使用して、MPU9250のDMP動作を確認しました。
　https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde

Arduinoコード

MPU9250_DMP_Quaternion.inoを参考にプログラムしました。

クォータニオンをProcessing用に配列にして送信します。

#include <SparkFunMPU9250-DMP.h>

#define SerialPort Serial

MPU9250_DMP imu;

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
unsigned int qw, qx, qy, qz;

void setup() 
{
  SerialPort.begin(115200);

  // Call imu.begin() to verify communication and initialize
  if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      SerialPort.println("Unable to communicate with MPU-9250");
      SerialPort.println("Check connections, and try again.");
      SerialPort.println();
      delay(5000);
    }
  }
  
  imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
               DMP_FEATURE_GYRO_CAL, // Use gyro calibration
              10); // Set DMP FIFO rate to 10 Hz
  // DMP_FEATURE_LP_QUAT can also be used. It uses the 
  // accelerometer in low-power mode to estimate quat's.
  // DMP_FEATURE_LP_QUAT and 6X_LP_QUAT are mutually exclusive
}

void loop() 
{
  // Check for new data in the FIFO
  if ( imu.fifoAvailable() )
  {
    // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
    if ( imu.dmpUpdateFifo() == INV_SUCCESS)
    {
      // computeEulerAngles can be used -- after updating the
      // quaternion values -- to estimate roll, pitch, and yaw
      imu.computeEulerAngles();
      printIMUData();
    }
  }
}

void printIMUData(void)
{  
  // After calling dmpUpdateFifo() the ax, gx, mx, etc. values
  // are all updated.
  // Quaternion values are, by default, stored in Q30 long
  // format. calcQuat turns them into a float between -1 and 1
  float q0 = imu.calcQuat(imu.qw);
  float q1 = imu.calcQuat(imu.qx);
  float q2 = imu.calcQuat(imu.qy);
  float q3 = imu.calcQuat(imu.qz);

  /*
  SerialPort.println("Q: " + String(q0, 4) + ", " +
                    String(q1, 4) + ", " + String(q2, 4) + 
                    ", " + String(q3, 4));
  SerialPort.println("R/P/Y: " + String(imu.roll) + ", "
            + String(imu.pitch) + ", " + String(imu.yaw));
  SerialPort.println("Time: " + String(imu.time) + " ms");
  SerialPort.println();
  */

  qw =int(q0 * 16384.0f);
  qx =int(q1 * 16384.0f);
  qy =int(q2 * 16384.0f);
  qz =int(q3 * 16384.0f);
 
  // display quaternion values in InvenSense Teapot demo format:
  teapotPacket[2] = highByte(qw);
  teapotPacket[3] = lowByte(qw);
  teapotPacket[4] = highByte(qx);
  teapotPacket[5] = lowByte(qx);
  teapotPacket[6] = highByte(qy);
  teapotPacket[7] = lowByte(qy);
  teapotPacket[8] = highByte(qz);
  teapotPacket[9] = lowByte(qz);
  Serial.write(teapotPacket, 14);
  teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
}

#include <SparkFunMPU9250-DMP.h>

#define SerialPort Serial

MPU9250_DMP imu;

// packet structure for InvenSense teapot demo

uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

unsigned int qw, qx, qy, qz;

void setup()

{

SerialPort.begin(115200);

// Call imu.begin() to verify communication and initialize

if (imu.begin() != INV_SUCCESS)

{

while (1)

{

SerialPort.println("Unable to communicate with MPU-9250");

SerialPort.println("Check connections, and try again.");

SerialPort.println();

delay(5000);

}

imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat

DMP_FEATURE_GYRO_CAL, // Use gyro calibration

10); // Set DMP FIFO rate to 10 Hz

// DMP_FEATURE_LP_QUAT can also be used. It uses the

// accelerometer in low-power mode to estimate quat's.

// DMP_FEATURE_LP_QUAT and 6X_LP_QUAT are mutually exclusive

}

void loop()

{

// Check for new data in the FIFO

if ( imu.fifoAvailable() )

{

// Use dmpUpdateFifo to update the ax, gx, mx, etc. values

if ( imu.dmpUpdateFifo() == INV_SUCCESS)

{

// computeEulerAngles can be used -- after updating the

// quaternion values -- to estimate roll, pitch, and yaw

imu.computeEulerAngles();

printIMUData();

}

void printIMUData(void)

{

// After calling dmpUpdateFifo() the ax, gx, mx, etc. values

// are all updated.

// Quaternion values are, by default, stored in Q30 long

// format. calcQuat turns them into a float between -1 and 1

float q0 = imu.calcQuat(imu.qw);

float q1 = imu.calcQuat(imu.qx);

float q2 = imu.calcQuat(imu.qy);

float q3 = imu.calcQuat(imu.qz);

SerialPort.println("Q: " + String(q0, 4) + ", " +

String(q1, 4) + ", " + String(q2, 4) +

", " + String(q3, 4));

SerialPort.println("R/P/Y: " + String(imu.roll) + ", "

+ String(imu.pitch) + ", " + String(imu.yaw));

SerialPort.println("Time: " + String(imu.time) + " ms");

SerialPort.println();

qw =int(q0 * 16384.0f);

qx =int(q1 * 16384.0f);

qy =int(q2 * 16384.0f);

qz =int(q3 * 16384.0f);

// display quaternion values in InvenSense Teapot demo format:

teapotPacket[2] = highByte(qw);

teapotPacket[3] = lowByte(qw);

teapotPacket[4] = highByte(qx);

teapotPacket[5] = lowByte(qx);

teapotPacket[6] = highByte(qy);

teapotPacket[7] = lowByte(qy);

teapotPacket[8] = highByte(qz);

teapotPacket[9] = lowByte(qz);

Serial.write(teapotPacket, 14);

teapotPacket[11]++; // packetCount, loops at 0xFF on purpose

}

動作

ProcessingのMPUTeapot.pdeを起動して、実行でMPU9250のDMPによって算出されたクォータニオンで飛行機がうごいきます。

USBポート設定はProcessingコード上でうまいこと実施してください(71行目)。

安定していますね。#MPU9250 pic.twitter.com/naoZlmpnPk

— HomeMadeGarbage (@H0meMadeGarbage) 2018年10月20日

まとめ

やっとMPU9250でDMP動作が確認できました。オフセット調整などを手動でせずとも安定した姿勢角取得が確認できました。

やったーー！MPU9250はBNO055より安価ですので活用方法ひろがりそうですね！いまのところM0マイコンでしか動かせないが。。

「モーションセンサ MPU9250 の DMP 動作確認」への2件のフィードバック

ken より:

2021年9月9日 5:21 PM

たいへん参考になります。他のマイコン（例えばESP32等）への移植は可能でしょうか？

返信
1. お父ちゃん より:
  
  2021年9月9日 5:25 PM
  
  ありがとうございます。
  
  できるかもしれないのですが私は技量がないのでDMPの
  サンプルがないマイコンへの移植は以前失敗に終わっています。
  
  ESP32のMPU9250 DMPサンプルは探せばどこかにあるかもしれません。
  
  返信

コメントはこちらからコメントをキャンセル

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。

構成

部品