M5StackGrayでMPU-9250のDMPを使う

# 1. M5Stack
M5Stackはesp32を搭載した開発モジュールです。
カラーディスプレイとボタン3つを備えているのが特徴です。
<img src="https://d2air1d4eqhwg2.cloudfront.net/images/3647/500x500/e665ce7c-4044-4536-8edd-43a5eb053ccc.jpg">
esp32がBLEとWifiのモジュールを標準搭載しているためか、IoT分野でも開発が盛んなようです。
豊富なArduinoのライブラリとの互換性を持てているのも利点です。

## 1.1 M5Stack Gray
GrayというモデルがIMU(MPU-9250)を搭載しており、3軸加速度・3軸角速度・3軸地磁気のデータを得ることができます。
<img src="https://d2air1d4eqhwg2.cloudfront.net/images/3648/500x500/d52de1b8-d6b8-4b62-bd54-54e3f473ac89.jpg">

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

# 2. 実装
## 2.1 PlatformIO
M5Stackにプログラムを書き込むための環境を構築します。マルチプラットフォームであり、vscodeによるIDEの補完が期待できることからPlatformIOを選択しました。
https://platformio.org/
<img src="https://platformio.org/images/platformio-logo.17fdc3bc.png" width=30%>
具体的な構築手順はこちらの記事が詳しいです。
[M5Stackの開発環境を整える - PlatformIO IDE編](https://qiita.com/lutecia16v/items/1c560bdd7eac7ebeaff7)


## 2.2 DMPライブラリ
SparkFunのリポジトリにMPU-9250のDMP用のライブラリがあるのでソースコードを落とします。
[sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library](https://github.com/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](https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/blob/master/examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino)
にあるので、全コピーして`main.cpp`に貼り付けます。しかし、ペーストしてもコンパイルエラーが出るので以下を直します。

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

```diff: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からのコメントに

```plaintext: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に

```plaintext:Arduino.h
#define _min(a,b) ((a)<(b)?(a):(b))
```

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


```diff:inv_mpu.c
#include <Arduino.h>
+#ifndef min
+#define min _min
+#endif
#define MPU9250
```

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

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

```diff:platformio.ini
[env:m5stack-core-esp32]
platform = espressif32
board = m5stack-core-esp32
framework = arduino
+monitor_speed = 115200
```

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

これでクォータニオンの値が表示されるようになりました。
![無題.png](https://qiita-image-store.s3.amazonaws.com/0/274974/fea077d8-f096-8647-0e0a-48aceaa48c86.png)

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

# 3. おわりに
GitHubのリンクです。[naninunenoy/m5stack_imu_notifier](https://github.com/naninunenoy/m5stack_imu_notifier)
各センサデータ+クォータニオンをBLEで飛ばしたり出来れば、やれることの幅が広がりそうですね。