M5StackにM5StickCのHATを繋ぐのを試してみた、という話の続き。
ENV-HATの磁気センサーを実際にM5Stackで動かしてみた。
やり方としては、Arduino IDEのライブラリのM5StickC ENV-HAT用の exampleスケッチを元にして、少し書き換えて動かした。
このときM5StackのIMU用のexampleスケッチのソースコードと合体させて、ジャイロセンサーと加速度センサーの計測値もついでに表示させるようにしてみた。
ソースコードは次のとおり。
// define must ahead #include <M5Stack.h>
// #define M5STACK_MPU6886
#define M5STACK_MPU9250
// #define M5STACK_MPU6050
// #define M5STACK_200Q
#include <M5Stack.h>
#include <Wire.h>
#include "bmm150.h"
#include "bmm150_defs.h"
BMM150 bmm = BMM150();
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;
float magX,magY,magZ;
float pitch = 0.0F;
float roll = 0.0F;
float yaw = 0.0F;
// the setup routine runs once when M5Stack starts up
void setup(){
// Initialize the M5Stack object
M5.begin();
/*
Power chip connected to gpio21, gpio22, I2C device
Set battery charging voltage and current
If used battery, please call this function in your project
*/
M5.Power.begin();
M5.IMU.Init();
Wire.begin();
if(bmm.initialize() == BMM150_E_ID_NOT_CONFORM) {
M5.Lcd.setCursor(0, 10);
M5.Lcd.println("BMM150 not found!");
while(1);
}
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(2);
}
// the loop routine runs over and over again forever
void loop() {
// put your main code here, to run repeatedly:
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
M5.IMU.getAccelData(&accX,&accY,&accZ);
M5.IMU.getAhrsData(&pitch,&roll,&yaw);
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;
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("%6.2f %6.2f %6.2f ", gyroX, gyroY, gyroZ);
M5.Lcd.setCursor(220, 42);
M5.Lcd.print(" o/s");
M5.Lcd.setCursor(0, 65);
M5.Lcd.printf(" %5.2f %5.2f %5.2f ", accX, accY, accZ);
M5.Lcd.setCursor(220, 87);
M5.Lcd.print(" G");
M5.Lcd.setCursor(0, 110);
M5.Lcd.printf(" %5.2f %5.2f %5.2f ", magX, magY, magZ);
M5.Lcd.setCursor(220, 132);
M5.Lcd.print(" uT");
M5.Lcd.setCursor(0, 150);
M5.Lcd.printf(" %5.2f %5.2f %5.2f ", pitch, roll, yaw);
M5.Lcd.setCursor(220, 172);
M5.Lcd.print(" degree");
delay(1);
}
磁気の表示の単位としてμT(マイクロテスラ)と書いたのだが、mG(ミリガウス)の間違いかもしれない(?) 。要確認。ちなみに1μT=10mGで換算できる。9DoFの姿勢計算では正規化して方向を表すベクトルとして使うので、姿勢計算に使うのならば、どちらであるかを気にしなくてもよかったりする。
Roll,Pitch,Yawについては、M5.IMU.getAhrsData(&pitch,&roll,&yaw);という部分で計算しているのだが、M5.IMUのライブラリではジャイロセンサーと加速度センサーの6軸分の計測値のみから姿勢計算しているようなので、Yawについてはすぐにずれてしまう。
磁気について、ここではキャリブレーション処理を入れていないので、方位を求めるのに使うことができないだろう。
キャリブレーション処理は、このあいだ作ったM5StickC用のソースには書いて入れてあるので、こちらに追加するのは簡単だろう。