昔買ったまま放置していたM5GOで何か出来ないかと考えていたところ、初期のM5GOには地磁気センサーが標準搭載されている事に気づきました。
(M5GOには昔から搭載されていないと思い違いをしていました)
加速度センサーで歩数を、地磁気センサーで方位を測定して、1歩ごとの方位を記録すれば簡易移動ロガーができそうです。
GPSが無かった頃の初期のカーナビみたいなやり方です。
加速度センサーは標準のライブラリを使い、地磁気センサーのBMM150はライブラリマネージャでインストールします。私は、M5_BMM150を使用しました。
作り方を調べてみましたが、歩数計の部分の作り方は大体同じです。
方位磁石もサンプルスケッチを参考にしました。
どちらのデータもちらつくので、直前までのデータを何個か保存しておいて、平均をとります。保存する個数はソフトを作りながら決定しました。
1歩ごとに方位を記録していきます。
M5GOの仕事はここまでで、データはCSVファイルにしてパソコンで加工します。
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];
以下、セットアップ部分になります。
まずは加速度センサーMPU6886の初期化です。
void setup(void){
M5.begin();
M5.Power.begin();
Wire.begin();
M5.Lcd.setTextSize(2);
adc_power_acquire();
M5.Mpu6886.Init();
M5.Mpu6886.setAccelFsr(M5.Mpu6886.AFS_2G);
地磁気センサーBMM150の初期化です。
地磁気センサーも補正が必要なのですが、サンプルスケッチでは補正値はNVS領域に保存されています。
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);
以下ループ部分です。地磁気センサーと加速度センサーを読み出します。
地磁気は±180度に変換し、10度単位にします。
加速度はXYZ軸全部を2乗して足して平方根をとります。
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();
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++;
歩数カウントがアップしたら測定データを保存し、一時データを初期化します。
フラグをONして、加速度が1G以下になるまでスルーさせます。
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;
}