M5stickで姿勢推定.cpp

//ライブラリのインポート
#include <Arduino.h>
#include <M5Unified.h>//6軸センサの値取得時に使う
#include <MadgwickAHRS.h>//姿勢推定に用いる

Madgwick MadgwickFilter;

float ax,ay,az;
float gx,gy,gz;
float roll,pitch,yaw;

void setup() {
    auto cfg = M5.config();   // M5Stack の初期化
    M5.begin(cfg);
    Serial.begin(115200);
    delay(500);

    //Madgwickフィルタ初期化
    MadgwickFilter.begin(100);//フィルタのサンプリングレートを設定(今回は10Hz)
    delay(1000);
}

void loop(){

    M5.Imu.getGyro(&gx, &gy, &gz);//ジャイロセンサの値を取得
    M5.Imu.getAccel(&ax, &ay, &az);//加速度センサの値を取得

    MadgwickFilter.updateIMU(gx,gy,gz,ax,ay,az);センサの値をMadgwickフィルタに与える

　　//ロール・ピッチ・ヨー角を計算
    roll = -MadgwickFilter.getRoll();
    pitch = MadgwickFilter.getPitch();
    yaw = MadgwickFilter.getYaw();
　　
  　//計算結果をTelePlotに表示
    Serial.printf(">ROLL:%f\n", roll);
    Serial.printf(">PITCH:%f\n", pitch);
    Serial.printf(">YAW:%f\n", yaw);
    delay(10);/サンプリングレート100Hzなので10ms遅らせる
}