M5Stackで、M5StickC用のENV-HATの磁気センサーを動かしてみた

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(&amp;gyroX,&amp;gyroY,&amp;gyroZ);
  M5.IMU.getAccelData(&amp;accX,&amp;accY,&amp;accZ);
  M5.IMU.getAhrsData(&amp;pitch,&amp;roll,&amp;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用のソースには書いて入れてあるので、こちらに追加するのは簡単だろう。

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

Time limit is exhausted. Please reload CAPTCHA.