M5StickCに取り付けるENV HATという拡張モジュールを手に入れた、という話の続きだ。
M5StickCとENV HATを使って、ジャイロセンサー+加速度センサー+地磁気センサーの9DoFを試してみる。
Arduino IDEで前回試してみたENV-HAT用のENVというexampleのスケッチに手を入れて作ってみた。
ジャイロセンサー+加速度センサー+地磁気センサーの9つの値の計測を行い、計測値を液晶に表示し、UARTに計測値の9つの値を出力している。
次のようなコードを作成した。(bmm150.cpp、bmm150.h、bmm150_defs.hがビルドには必要なので注意。)
#include <M5StickC.h>
#include <Wire.h>
#include "bmm150.h"
#include "bmm150_defs.h"
BMM150 bmm = BMM150();
float gyroX,gyroY,gyroZ;
float accX,accY,accZ;
float magX,magY,magZ;
float t,t0;
float magXofs,magYofs,magZofs;
float magXmax,magYmax,magZmax;
float magXmin,magYmin,magZmin;
int calibration_mode;
void setup() {
// put your setup code here, to run once:
M5.begin();
Serial.begin(115200);
Wire.begin(0,26);
M5.IMU.Init();
pinMode(M5_BUTTON_HOME, INPUT);
M5.Lcd.setRotation(3);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Lcd.setCursor(20, 0);
M5.Lcd.println("9DoF TEST (w/ENV-HAT)");
M5.Lcd.setCursor(0, 10);
M5.Lcd.println(" X Y Z");
if(bmm.initialize() == BMM150_E_ID_NOT_CONFORM) {
M5.Lcd.setCursor(0, 10);
M5.Lcd.println("BMM150(ENV-HAT) not found!");
while(1);
}
t=0; t0=0;
calibration_mode=0;
magXofs=0;
magYofs=0;
magZofs=0;
}
void loop() {
// put your main code here, to run repeatedly:
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
M5.IMU.getAccelData(&accX,&accY,&accZ);
bmm.read_mag_data();
magX = bmm.raw_mag_data.raw_datax;
magY = bmm.raw_mag_data.raw_datay;
magZ = bmm.raw_mag_data.raw_dataz;
if (calibration_mode==1) {
if (magX>magXmax) { magXmax=magX; }
if (magX<magXmin) { magXmin=magX; }
if (magY>magYmax) { magYmax=magY; }
if (magY<magYmin) { magYmin=magY; }
if (magZ>magZmax) { magZmax=magZ; }
if (magZ<magZmin) { magZmin=magZ; }
}
if (digitalRead(M5_BUTTON_HOME) == LOW) {
if (calibration_mode==0) {
calibration_mode=1;
magXmax=magX; magXmin=magX;
magYmax=magY; magYmin=magY;
magZmax=magZ; magZmin=magZ;
} else {
calibration_mode=0;
magXofs=(magXmax+magXmin)/2;
magYofs=(magYmax+magYmin)/2;
magZofs=(magZmax+magZmin)/2;
}
delay(500);
}
magX = magX - magXofs;
magY = magY - magYofs;
magZ = magZ - magZofs;
t0=t; t=millis();
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("%6.2f %6.2f %6.2f ", gyroX, gyroY, gyroZ);
M5.Lcd.setCursor(140, 20);
M5.Lcd.print("o/s");
M5.Lcd.setCursor(0, 30);
M5.Lcd.printf(" %5.2f %5.2f %5.2f ", accX, accY, accZ);
M5.Lcd.setCursor(140, 30);
M5.Lcd.print("G");
M5.Lcd.setCursor(0, 40);
M5.Lcd.printf(" %5.0f %5.0f %5.0f ", magX, magY, magZ);
M5.Lcd.setCursor(140, 40);
M5.Lcd.print("uT");
M5.Lcd.setCursor(0, 70);
M5.Lcd.printf(" dt=%.0f ", t-t0);
M5.Lcd.setCursor(140, 70);
if (calibration_mode==1) {
M5.Lcd.printf("CAL");
} else {
M5.Lcd.printf(" ");
}
Serial.print(gyroX); Serial.print(",");
Serial.print(gyroY); Serial.print(",");
Serial.print(gyroZ); Serial.print(",");
Serial.print(accX); Serial.print(",");
Serial.print(accY); Serial.print(",");
Serial.print(accZ); Serial.print(",");
Serial.print(magX); Serial.print(",");
Serial.print(magY); Serial.print(",");
Serial.print(magZ); Serial.println("");
}
実行すると、このような液晶画面の表示となる。
ジャイロセンサーのX,Y,Z軸の計測値と、
加速度センサーのX,Y,Z軸の計測値と、
地磁気センサーのX,Y,Z軸の計測値が表示される。
あと、測定周期がdtとしてミリ秒単位で表示されている。今回の場合9ミリ秒が測定周期となっている。
地磁気センサーのキャリブレーションのソフトを自前で作り直している。
本当はもっとちゃんとしたキャリブレーションソフトも作れるのだが、とりあえず元のENVのexampleスケッチと同様な簡易版を書いた。(磁気センサーのキャリブレーションは、感度まで合わせようとするとちょっと計算が大変で、昔そういうのを開発する仕事をしてた。)
とりあえず今回実装したのは、暫く測定して最大最小の中間を原点オフセットにするという簡易なキャリブレーション方法のものだ。
使い方としては、
(1)HOMEボタン(M5のロゴのボタン)を1度押すと、画面に「CAL」と表示される。キャリブレーション(Calibration)中であるという表示だ。
(2)CAL表示中に、M5StickC+ENV HATを、水平に±180度回すようにYaw方向に動かし、次に、Roll方向にも±180度回すように動かし、Pitch方向にも±180度回すように動かす。
(3)動かし終わったら、HOMEボタン(M5のロゴのボタン)を1度押すと、キャリブレーションを終了し、動かしていた間の磁気センサーX,Y,Z軸それぞれの計測値の最大値と最小値の中間を原点のオフセット値として採用する。画面のCAL表示は消える。
という使い方となっている。
また、計測値はUARTに出力される。
Arduino IDEのシリアルコンソールで115200bpsで表示させると、確認できる。あるいはWindows PCでTeraTermなどを使って受信させることも可能だ。
—
9DoFのMadgwickフィルターというのを使って、M5StickCの姿勢(Roll,Pitch,Yaw)を計算させるというのをこれから試してみる。(前提として、それぞれのセンサーのX,Y,Z軸とか極性とか合わせておかないといけないか?)
あと、有線のUARTでなくて、代わりにBluetooth SPPで無線UARTに出力するのも試してみる予定だ。
腕時計タイプのバンドを使って腕に取り付けて、モーションキャプチャー的に使ってみようと考えている。 1個だけだと本物のモーションキャプチャーみたいな使い方はできないけど、かといって複数を同時にPCにつなぐソフトを作ろうとすると面倒そうだ。
つづく