昔買ったＭ５ＧＯで移動計を作る

昔買ったまま放置していたＭ５ＧＯで何か出来ないかと考えていたところ、初期のＭ５ＧＯには地磁気センサーが標準搭載されている事に気づきました。
（Ｍ５ＧＯには昔から搭載されていないと思い違いをしていました）

加速度センサーで歩数を、地磁気センサーで方位を測定して、１歩ごとの方位を記録すれば簡易移動ロガーができそうです。
ＧＰＳが無かった頃の初期のカーナビみたいなやり方です。
加速度センサーは標準のライブラリを使い、地磁気センサーのＢＭＭ１５０はライブラリマネージャでインストールします。私は、M5_BMM150を使用しました。
作り方を調べてみましたが、歩数計の部分の作り方は大体同じです。
方位磁石もサンプルスケッチを参考にしました。

どちらのデータもちらつくので、直前までのデータを何個か保存しておいて、平均をとります。保存する個数はソフトを作りながら決定しました。
１歩ごとに方位を記録していきます。
Ｍ５ＧＯの仕事はここまでで、データはＣＳＶファイルにしてパソコンで加工します。

ＢＭＭ１５０のサンプルスケッチがベースになっています。追加した部分のみ掲載します。
データ保存とかはそれぞれだと思いますので掲載していません。
加速度センサーは使う前に平らな場所で補正する必要があります。
地磁気センサーは毎回補正しなくてもいいようです。

補正値を計測する部分と、それを適用する部分です。
私はいつも、サンプルコードのオリジナル部分と区別するため、｛｝の付け方とタブ数を変えています。

float acc[3];
float gyro[3];
float aOfs[3];
float gOfs[3];

void readGyro()
{
 M5.MPU6886.getGyroData(&gyro[0], &gyro[1], &gyro[2]);
 M5.MPU6886.getAccelData(&acc[0], &acc[1], &acc[2]);
}

void mpu6886_calibrate()
{
 float gSum[3];
 float aSum[3];
 for(int i = 0; i < 500; i++)
 {
  readGyro();
  gSum[0] += gyro[0];
  gSum[1] += gyro[1];
  gSum[2] += gyro[2];
  aSum[0] += acc[0];
  aSum[1] += acc[1];
  aSum[2] += acc[2];
  delay(2);
 }
 gOfs[0] = gSum[0]/500;
 gOfs[1] = gSum[1]/500;
 gOfs[2] = gSum[2]/500;
 aOfs[0] = aSum[0]/500;
 aOfs[1] = aSum[1]/500;
 aOfs[2] = aSum[2]/500 - 1.0;
}

void apply_mpu6886_calibrate()
{
 gyro[0] -= gOfs[0];
 gyro[1] -= gOfs[1];
 gyro[2] -= gOfs[2];
 acc[0] -= aOfs[0];
 acc[1] -= aOfs[1];
 acc[2] -= aOfs[2];
}

読み出したデータはちらつくので平滑化します。
バッファして平均をとります。

#define ACC_CNT_MAX 8
#define ACC_LOW 1040
float accBuf[ACC_CNT_MAX];

以下、セットアップ部分になります。
まずは加速度センサーＭＰＵ６８８６の初期化です。

void setup(void){
 M5.begin();
 M5.Power.begin();  //Init Power module.
 Wire.begin();
 M5.Lcd.setTextSize(2);
 adc_power_acquire();

 M5.Mpu6886.Init();
 M5.Mpu6886.setAccelFsr(M5.Mpu6886.AFS_2G);

地磁気センサーＢＭＭ１５０の初期化です。
地磁気センサーも補正が必要なのですが、サンプルスケッチでは補正値はＮＶＳ領域に保存されています。

 M5.Lcd.println("BMM150 INIT ...");
 if(bmm150_initialization() != BMM150_OK)
 {
  M5.Lcd.println("BMM150 ERROR ...");
  for(;;)
  {
   delay(100);
  }
 }
 M5.Lcd.println("BMM150 OFFSET LOAD ...");
 bmm150_offset_load();

最初に加速度センサーの補正を行います。

 M5.Lcd.println("MPU6886 CALIBRATION ...");
 mpu6886_calibrate();

地磁気センサーの補正を行います。毎回やらないくてもいいので、スキップできるようにしています。

 M5.Lcd.println("");
 M5.Lcd.println("BMM150 CALIBRATION btnC");
 M5.Lcd.println("      CANCEL btnB");

 for(;;)
 {
  M5.update();
  if(M5.BtnC.wasPressed())
  {
   M5.Lcd.println("FLIP AND ROTATE");
   M5.Lcd.println("for 10 seconds");
   bmm150_calibrate(10000);
   break;
  }
  if(M5.BtnB.wasPressed())
  {
   break;
  }
 }

 M5.Lcd.fillScreen(BLACK);
 M5.Lcd.setTextSize(4);

以下ループ部分です。地磁気センサーと加速度センサーを読み出します。
地磁気は±１８０度に変換し、１０度単位にします。
加速度はＸＹＺ軸全部を２乗して足して平方根をとります。

int max0=-10000;
int min0=10000;
float a0;
float ax, ay, az;
int mark=0;
int accCnt=0;
int flg0=0;
int pedometer=0;
int acc0=0;
int stepNum=0;

struct pedoStruct
{
 int cnt;
 int tm;
 int head;
 int pre;
 int mrk;
} pedoData[3600];

unsigned long tm0=0;

void loop(void){
 tm0=millis()/1000;
 M5.update();  //Read the press state of the key.
 bmm150_read_mag_data(&dev);
 float head_dir=atan2(dev.data.x-mag_offset.x,dev.data.y-mag_offset.y)*180.0/M_PI;
 int head_dir2=((int)head_dir/10)*10;
 readGyro();
 apply_mpu6886_calibrate();

 ax=acc[0];
 ay=acc[1];
 az=acc[2];
 a0=sqrt(ax*ax+ay*ay+az*az);

平滑化するためにバッファします。
計算結果が閾値以上なら歩数カウントアップします。
最大値と最小値も保存しておきます。

 accBuf[accCnt]=a0;
 accCnt++;
 if(accCnt>=ACC_CNT_MAX)
 {
  accCnt=0;
 }

 float accSum=0;
 for(int j=0; j<ACC_CNT_MAX; j++)
 {
  accSum+=accBuf[j];
 }
 acc0=(accSum/ACC_CNT_MAX)*1000;

 if(acc0>max0)
 {
  max0=acc0;
 }
 if(acc0<min0)
 {
  min0=acc0;
 }

 if((flg0==0)&&(acc0>ACC_LOW))
 {
  pedometer++;

歩数カウントがアップしたら測定データを保存し、一時データを初期化します。
フラグをＯＮして、加速度が１Ｇ以下になるまでスルーさせます。

  max0=-10000;
  min0=10000;
  for(int j=0; j<ACC_CNT_MAX; j++)
  {
   accBuf[j]=1;
  }
  pedoData[pedometer].cnt=pedometer;
  pedoData[pedometer].tm=millis()/1000;
  pedoData[pedometer].head=head_dir2;
  accCnt=0;
  flg0=1;
 }

 if((flg0==1)&&(acc0<1000))
 {
  flg0=0;
 }