【M5Atom入門】内臓IMUを使い倒す♬

先日のM5atomの内臓IMUの精度がいまいちだったので、精度を追求してみた。
そもそも、MPU6886は内臓ゆえにもともと利用できるスケッチがあるが、どうもその全体像がはっきりしない。ということで、MP6886データシートを張ります。だいたい、MPU6050と同じような仕様です。
【参考】
・MPU6886データシート

やったこと

・スケッチ例と関数をみる
・精度を見る
・結果

・スケッチ例と関数をみる

参考を見れば、m5atomのスケッチ例は以下のとおりです。
【参考】
①M5Atom/examples/Basics/MPU6886/MPU6886.ino
主要な利用法を抜き出すと、以下の通りです。

#include "M5Atom.h"
...
void setup()
{
    M5.begin(true, true, true);
    M5.IMU.Init();
}
void loop()
{
    delay(50);
    M5.IMU.getAttitude(&pitch, &roll);
...
    Serial.printf("%.2f,%.2f,%.2f,%.2f\n", pitch, roll, arc, val);
...
}

実は、このM5.IMU.getAttitude(&pitch, &roll);の精度がいまいちでした。
というか、たぶん素で出力しているようです。

次に、参考になるスケッチは以前のようにm5stickcのコードです。
m5stickcでは該当する部分は、以下のように実装しています。
【参考】
②【倒立振り子入門】ArduinoNanoとm5stickcをそれぞれ利用して、PID制御で倒立振り子を立てる♪

#include <M5StickC.h>

void setup() {
  M5.begin();
...
  M5.MPU6886.Init();
}
void loop() {
  M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
...
}

同様な関数は無いかm5atom.hを眺めると以下のとおりありました。
まず、m5stickcではM5.MPU6886としていましたが、atomではm5atom.hで以下のようにMPU6886 IMU;でIMUというインスタンスで呼べるようになっています。
※その他の関数も以下で定義されているので、冗長ですが示しておきます
【参考】
③M5Atom/src/M5Atom.h

class M5Atom
{
private:
    bool _isInited = false;
    /* data */
public:
    M5Atom(/* args */);
    ~M5Atom();

    MPU6886 IMU;
    LED_DisPlay dis;

    Button Btn = Button(39, true, 10);

    void begin(bool SerialEnable = true, bool I2CEnable = true, bool DisplayEnable = false);
    void update();
};

extern M5Atom M5;

そして、MPU6886.hを見ると使える関数が分かります。get関数に混じって初期化関数setもあります。
getAttitude(double *pitch, double *roll);に加えてgetAhrsData(float *pitch,float *roll,float *yaw);も使えそうです。

【参考】
④M5Atom/src/utility/MPU6886.h

...
class MPU6886 {
    public:
      MPU6886();
      int Init(void);
      void getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az);
      void getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz);
      void getTempAdc(int16_t *t);

      void getAccelData(float* ax, float* ay, float* az);
      void getGyroData(float* gx, float* gy, float* gz);
      void getTempData(float *t);

      void SetGyroFsr(Gscale scale);
      void SetAccelFsr(Ascale scale);

      void getAhrsData(float *pitch,float *roll,float *yaw);
      void getAttitude(double *pitch, double *roll);

これらの違いを見ておきましょう。
【参考】
⑤M5Atom/src/utility/MPU6886.cpp
参考⑤の当該関数の部分を見ると以下のように実装されています。
※m5stackというかArduinoではこういう構造で実装するんだなというのが分かります
中身を見ると、getAhrsData(float *pitch,float *roll,float *yaw)はMahonyAHRSupdateIMUを利用して計算しているのが分かります。
この関数は以下のコード見るとmadgwickを組み込んでフィルターした結果を出力しているようです。
【参考】
⑥M5Atom/src/utility/MahonyAHRS.cpp
⑦M5Atom/src/utility/MahonyAHRS.h

void MPU6886::getAhrsData(float *pitch,float *roll,float *yaw){

  float accX = 0; 
  float accY = 0;
  float accZ = 0;

  float gyroX = 0;
  float gyroY = 0;
  float gyroZ = 0;

  getGyroData(&gyroX,&gyroY,&gyroZ);
  getAccelData(&accX,&accY,&accZ);

  MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw);

}

一方、getAttitudeは加速度からarcsinとarctanを利用して計算しています。
これだと、一定のドリフトやノイズが乗るのはやむをえないという状況です。
因みに、57.295=180/πです。

void MPU6886::getAttitude(double *pitch, double *roll)
{
    float accX = 0;
    float accY = 0;
    float accZ = 0;

    float gyroX = 0;
    float gyroY = 0;
    float gyroZ = 0;

    getGyroData(&gyroX, &gyroY, &gyroZ);
    getAccelData(&accX, &accY, &accZ);

    if ((accX < 1) && (accX > -1))
    {
        *pitch = asin(-accX) * 57.295;
    }
    if (accZ != 0)
    {
        *roll = atan(accY / accZ) * 57.295;
    }

    ( *pitch ) = _alpha * ( *pitch ) + (1 - _alpha) * _last_theta;
    ( *roll ) = _alpha * ( *roll ) + (1 - _alpha) * _last_phi;
}

これ以外に、カルマンフィルターで処理するのもよさそうですが、ここでは取り上げません。exampleとしてMPU6050のコードがあるので、同様に動くと思います。
【参考】
⑧1軸 姿勢制御モジュール の倒立に向けた準備 ーリアクションホイールへの道6ー
⑨TKJElectronics/KalmanFilter

初期化メソッドを利用

参考⑧では、以下の初期化メソッドを利用しています。

M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS); 
M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);

パラメーターは、初期値等は以下のとおり定義されています。それを上記のように設定変更しています。

class MPU6886 {
    public:
      enum Ascale {
        AFS_2G = 0,
        AFS_4G,
        AFS_8G,
        AFS_16G
      };

      enum Gscale {
        GFS_250DPS = 0,
        GFS_500DPS,
        GFS_1000DPS,
        GFS_2000DPS
      };

      Gscale Gyscale = GFS_2000DPS;
      Ascale Acscale = AFS_8G;

・精度を見る

ということで、検証するコードは以下のように書けます。
一応、検証としてmadgwickfilterもコメントアウトして残している。
また、yawについては、時系列に比例するドリフトが残っており、それを改善できないかと思い、線形の寄与分を差し引くファクターをfpとしてボタン押下で算出するようにしたが、残念ながらボタンが押しにくく上手く機能していない。、
以下の元コードを見るとLED表示はおまけと考えると上記の関数を利用することにより、必要なコードは極めて短くなった。

元のコード

#include "M5Atom.h"

//#include <MadgwickAHRS.h>   // 角度と角速度にMadgwickフィルタをかけるライブラリ
//Madgwick MadgwickFilter;    // Madgwickフィルタのオブジェクトを設定

float roll, pitch, yaw,yaw0;
//float accX=0,accY=0, accZ = 0;
//float gyroX=0 ,gyroY=0,gyroZ = 0;
//float temp = 0;
//float r_rand = 180 / PI;
//float dt=0, st=0, fp =0.3;

void setup() {
  M5.begin(true, true, true);

  M5.IMU.Init();
  M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS); //250,,500,1000,2000
  M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); //2,4,8,16
  //MadgwickFilter.begin(100);  // Madgwickフィルタの周波数を100Hzに設定。

  //st= micros();
  //update_fp();
}
/*
void update_fp(){
  M5.IMU.getAhrsData(&pitch,&roll,&yaw0);
  delay(10);
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  dt = (micros()-st)/1000000;
  fp = (yaw-yaw0)/dt;
}
*/
void loop() {
  /*
  if (M5.Btn.wasPressed())
  {
    update_fp();
  }
  */
  //dt = (micros()-st)/1000000;
  //M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  //M5.IMU.getAccelData(&accX,&accY,&accZ);
  //MadgwickFilter.updateIMU(gyroX, gyroY, gyroZ, accX-accX0, accY-accY0, accZ-accZ0);
  //float pitch  = MadgwickFilter.getPitch();
  //float roll  = MadgwickFilter.getRoll();
  //float yaw  = MadgwickFilter.getYaw();

  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  Serial.printf("%.2f,%.2f,%.2f,\n", pitch, roll, yaw); //-fp*dt-yaw0); 

  //LED表示
  int line1 = map(pitch, -90, 90, 4, 0);
  int line2 = map(roll, -90, 90, 4, 0);
  int line3 = map(yaw, -180, 180, 0xf00000, 0);
  if(line1 >= 0 && line1 < 5){
    M5.dis.clear(); 
    if(line2 >= 0 && line2 < 5){
      M5.dis.drawpix(line1 * 5 + line2, line3); // 0xf00000);
    }
  }

  delay(5);
  M5.update();
}

必要なコード

#include "M5Atom.h"

float roll, pitch, yaw;

void setup() {
  M5.begin(true, true, true);
  M5.IMU.Init();
  M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS); //250,,500,1000,2000
  M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); //2,4,8,16
}

void loop() {
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  Serial.printf("%.2f,%.2f,%.2f,\n", pitch, roll, yaw);

  delay(5);
  M5.update();
}

・結果

結果以下のように測定できた。
上でも書いたが、pitch, rollはノイズもほとんどなく、綺麗なデータが並びかつ机を振動させると反応よく振動を表している。
一方、上でも書いたがyawはドリフトが少し入ってこれは取り除けなかった。

裏返しが正しい位置(笑) pic.twitter.com/nIb43PHyKL

— ウワン (@MuAuan) June 18, 2021

まとめ

・m5atomの内臓IMUを精度良く利用できるようになった
・getAhrsData関数は、内部でmadgwickフィルターを利用していた
・初期化も関数で簡単に出来る
・atomの関数群の構築の仕方が理解できた

・これを使って精度のよい倒立振り子を作ろう