LoginSignup
13
9

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@MuAuan(ムー ウワン)

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

Last updated at Posted at 2021-06-18

先日の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はドリフトが少し入ってこれは取り除けなかった。

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

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

13
9
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
13
9

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?