M5StackGrayでMPU-9250のDMPを使う

1. M5Stack

M5Stackはesp32を搭載した開発モジュールです。
カラーディスプレイとボタン3つを備えているのが特徴です。

esp32がBLEとWifiのモジュールを標準搭載しているためか、IoT分野でも開発が盛んなようです。
豊富なArduinoのライブラリとの互換性を持てているのも利点です。

1.1 M5Stack Gray

GrayというモデルがIMU(MPU-9250)を搭載しており、3軸加速度・3軸角速度・3軸地磁気のデータを得ることができます。

1.2 DMP

MPU-9250はDMP(Digital Motion Processer)を備えています。通常は3軸加速度・3軸角速度・3軸地磁気のセンサ値からカルマンフィルタや相補フィルタでセンサフュージョンを行ってクォータニオン(センサの姿勢を表す値)を求めますが、DMPのお陰で自力でこれらを実装せずともクォータニオンを取得することができます。

2. 実装

2.1 PlatformIO

M5Stackにプログラムを書き込むための環境を構築します。マルチプラットフォームであり、vscodeによるIDEの補完が期待できることからPlatformIOを選択しました。
https://platformio.org/

具体的な構築手順はこちらの記事が詳しいです。
M5Stackの開発環境を整える - PlatformIO IDE編

2.2 DMPライブラリ

SparkFunのリポジトリにMPU-9250のDMP用のライブラリがあるのでソースコードを落とします。
sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library

どこに配置するかは任意ですが、私はプロジェクトのlibディレクトリにSparkFun_MPU-9250-DMP_Arduino_Libraryというディレクトリを作り、以下にライブラリのソースファイルやライセンスファイルを配置しました。

root/
　├ lib/
　│　└ SparkFun_MPU-9250-DMP_Arduino_Library-master/
　│     ├ src/
  │     │  ├ util/
  │     │  ├ MPU9250_RegisterMap.h
  │     │  ├ SparkFunMPU9250-DMP.cpp
  │     │  └ SparkFunMPU9250-DMP.h
  │     ├ LICENSE.md
  │     ├ README.md
  │     ├ keywords.txt
  │     └ library.properties
　└ src/
　 　└ main.cpp

ライブラリ化されているのでI2Cがどうとか気にせず加速度センサなどの値を読み取ることができます。

DMPを使ってクォータニオンの値をシリアルモニタに表示するExampleが
examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino
にあるので、全コピーしてmain.cppに貼り付けます。しかし、ペーストしてもコンパイルエラーが出るので以下を直します。

宣言の修正

ExampleがArduino用にに書かれているので、24行目辺りを以下のように直します。

main.cpp

#include <SparkFunMPU9250-DMP.h>

-#define SerialPort SerialUSB
+#define SerialPort Serial
+void printIMUData(void);

MPU9250_DMP imu;

minの定義

ビルドすると以下のようなエラーが出ます。

.pioenvs\m5stack-core-esp32\lib7f8\libSparkFun_MPU-9250-DMP_Arduino_Library-master.a(inv_mpu.c.o):(.literal.mpu_set_sample_rate+0x0): undefined reference to `min'
.pioenvs\m5stack-core-esp32\lib7f8\libSparkFun_MPU-9250-DMP_Arduino_Library-master.a(inv_mpu.c.o): In function `mpu_set_sample_rate':
inv_mpu.c:(.text.mpu_set_sample_rate+0x7c): undefined reference to `min'
.pioenvs\m5stack-core-esp32\lib7f8\libSparkFun_MPU-9250-DMP_Arduino_Library-master.a(inv_mpu.c.o): In function `mpu_load_firmware':
inv_mpu.c:(.text.mpu_load_firmware+0x43): undefined reference to `min'
collect2.exe: error: ld returned 1 exit status
*** [.pioenvs\m5stack-core-esp32\firmware.elf] Error 1

minという関数の定義が無くundefined referenceとなってしまいます。
よくよく見るとinv_mpu.cのL27からのコメントに

inv_mpu.c

/* The following functions must be defined for this platform:
 * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
 *      unsigned char length, unsigned char const *data)
 * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
 *      unsigned char length, unsigned char *data)
 * delay_ms(unsigned long num_ms)
 * get_ms(unsigned long *count)
 * reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
 * labs(long x)
 * fabsf(float x)
 * min(int a, int b)
 */

と書いてあり、minの定義が必要だと書かれています。

min(int a, int b)はaとbの値の小さい方を返す関数と思われます。
inv_mpu.cがincludeしているArduino.hのL187に

Arduino.h

#define _min(a,b) ((a)<(b)?(a):(b))

という同等の定義があるので、これを流用できるようinv_mpu.cのL39辺りに以下の定義を追加します。(3rdパーティのライブラリに自分で手を入れるのは好ましいことではありませんが、こういう場合は仕方ないでしょう。)

inv_mpu.c

#include <Arduino.h>
+#ifndef min
+#define min _min
+#endif
#define MPU9250

これでビルドできるようになるはずです。

2.3 シリアルモニタでの確認

コードのビルドと書き込み後、vscode下部のコンセントみたいなマークをクリックするとシリアルモニタが開けるのでExampleの出力を確認できます。文字化けしている場合はplatformio.iniの末尾にmonitor_speedの設定を加えてください。

platformio.ini

[env:m5stack-core-esp32]
platform = espressif32
board = m5stack-core-esp32
framework = arduino
+monitor_speed = 115200

シリアルモニタが起動できない場合はvscodeを再起動しましょう。
シリアルモニタはコンソール右上のごみ箱のボタンから閉じるのが正しいようです。

これでクォータニオンの値が表示されるようになりました。

2.4 補足

キーンとうるさい場合はM5Stackをコードから抜いて電源ボタンを2回押すと電源が切れます。1回押しや長押しだと再起動なので切れません。

3. おわりに

GitHubのリンクです。naninunenoy/m5stack_imu_notifier
各センサデータ+クォータニオンをBLEで飛ばしたり出来れば、やれることの幅が広がりそうですね。