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

先日のM5atomの内臓IMUの精度がいまいちだったので、精度を追求してみた。
そもそも、MPU6886は内臓ゆえにもともと利用できるスケッチがあるが、どうもその全体像がはっきりしない。ということで、MP6886データシートを張ります。だいたい、MPU6050と同じような仕様です。
【参考】
・[MPU6886データシート](https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/docs/datasheet/core/MPU-6886-000193%2Bv1.1_GHIC_en.pdf)

###やったこと
・スケッチ例と関数をみる
・精度を見る
・結果
###・スケッチ例と関数をみる
参考を見れば、m5atomのスケッチ例は以下のとおりです。
【参考】
①[M5Atom/examples/Basics/MPU6886/MPU6886.ino](https://github.com/m5stack/M5Atom/blob/master/examples/Basics/MPU6886/MPU6886.ino)
主要な利用法を抜き出すと、以下の通りです。

```c
#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制御で倒立振り子を立てる♪](https://qiita.com/MuAuan/items/ee0a9d1385d2158cd647)

```c
#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](https://github.com/m5stack/M5Atom/blob/master/src/M5Atom.h)

```c
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](https://github.com/m5stack/M5Atom/blob/master/src/utility/MPU6886.h)

```c
...
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](https://github.com/m5stack/M5Atom/blob/master/src/utility/MPU6886.cpp)
参考⑤の当該関数の部分を見ると以下のように実装されています。
※m5stackというかArduinoではこういう構造で実装するんだなというのが分かります
中身を見ると、getAhrsData(float *pitch,float *roll,float *yaw)はMahonyAHRSupdateIMUを利用して計算しているのが分かります。
この関数は以下のコード見るとmadgwickを組み込んでフィルターした結果を出力しているようです。
【参考】
⑥[M5Atom/src/utility/MahonyAHRS.cpp](https://github.com/m5stack/M5Atom/blob/master/src/utility/MahonyAHRS.cpp)
⑦[M5Atom/src/utility/MahonyAHRS.h](https://github.com/m5stack/M5Atom/blob/master/src/utility/MahonyAHRS.h)

```c
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/πです。

```c
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ー](https://homemadegarbage.com/reactionwheel06)
⑨[TKJElectronics/KalmanFilter](https://github.com/TKJElectronics/KalmanFilter)

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

```c
M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS); 
M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);
```
パラメーターは、初期値等は以下のとおり定義されています。それを上記のように設定変更しています。


```c
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表示はおまけと考えると上記の関数を利用することにより、必要なコードは極めて短くなった。

<details><summary>元のコード</summary><div>

```c
#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();
}
```
</div></details>

####必要なコード

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

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">裏返しが正しい位置(笑) <a href="https://t.co/nIb43PHyKL">pic.twitter.com/nIb43PHyKL</a></p>&mdash; ウワン (@MuAuan) <a href="https://twitter.com/MuAuan/status/1405762480786137098?ref_src=twsrc%5Etfw">June 18, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
###まとめ
・m5atomの内臓IMUを精度良く利用できるようになった
・getAhrsData関数は、内部でmadgwickフィルターを利用していた
・初期化も関数で簡単に出来る
・atomの関数群の構築の仕方が理解できた

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