M5Stackの回転データが勝手に回り続けることになる問題の対策

#回転情報がおかしい...
M5Stackで回転情報を使おうとした時にこんな問題に遭遇しました。
一応の対策は取れたので、もし同じ現象に悩む人がいた時のためにメモ代わりとして記事に残してみます。

M5Stackの回転情報を取ろうとしたのだけど、静止してても常に回転してることになってるっぽい...(画面右下degreeの上の数字)。
gyroのZ(右上数字)が常に-17くらいになってるけど、これが原因？
動かした時は反応してくれるけど、動いてない時は止まっていて欲しい...
どうしたら良いのやら。#M5Stack pic.twitter.com/h8cyVBWj2V

— foka (@foka22ok) December 27, 2019

現象は↑の通りですが、M5StackのIMUサンプルコードをとりあえず動かしてみたら、pitch・roll・yawの回転情報のうち、静止していてもyawがずっと動き続ける問題に悩まされてしまいました。

#対策を探る

他のセンサー値をよくよく見ると、Z軸のgyro情報が常に-15〜-17になっていることに気付きました。
止まっているのに、Z軸方向に常に回転加速度が加えられている状態のようです。
ここが怪しい気配です。
これが故障なのかどうかはわかりませんが、似たようなissueは発見しました。

↑のissueでは、BMM150を組み合わせてどうにかする的なことが書いてますが、BMM150のサンプルコード見てもいまいち解決に繋がるのか不明でした...。

どうしたものかと思ったけど、gyroのセンサーデータの狂いはどうやらほぼ一定のようです。
pitch・roll・yawは加速度と回転加速度を使って算出するので、gyroのデータに補正をかけてから計算してやると対策できるのでは？と思って試したところ、効果アリでした。

M5StackはGitHubにソースが上がっているので、その中から該当する計算箇所を探してみると、utility/MahonyAHRS.hでpitch・roll・yawを計算している箇所を発見。
https://github.com/m5stack/M5Stack/tree/master/src

これで対策が取れそうだと思って、実際に↓のようなことを試しました。（記事下にコード載せてます）

M5Stackで、勝手に回り続けたことになる問題をとりあえず対策。
数フレーム分のgyroの回転情報を貯めて平均を出し、「M5.IMU.getAhrsData()」を使わずに<utility/MahonyAHRS.h>をincludeして、平均値で補正かけたgyroデータを使って「MahonyAHRSupdateIMU()」に叩き込むと随分改善した。#M5stack pic.twitter.com/h3wtiAoEyA

— foka (@foka22ok) December 27, 2019

#対策できた
というわけでコレができました。
やってることはシンプルですが、物が動くとちょっと楽しい。

M5Stackの回転情報が操れるようになったので、とりあえず試したかったことができました。#M5stack pic.twitter.com/zNNHMf1flr

— foka (@foka22ok) December 27, 2019

#対策したコード
M5StackのIMUサンプルコードを今回の対策用に改造した際のコードを載せておきます。
この方法の場合、起動時に補正用の平均データが溜まるまでは静止させておく必要があります。
あと、根本的な解決策がわかる方いたら教えてくれると嬉しいです。

IMU.ino

// define must ahead #include <M5Stack.h>
#define M5STACK_MPU6886 
// #define M5STACK_MPU9250 
// #define M5STACK_MPU6050
// #define M5STACK_200Q

#include <M5Stack.h>
//[追加]MahonyAHRSupdateIMU()を呼べるようにするため
#include <utility/MahonyAHRS.h>

float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;

float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;

float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;

float temp = 0.0F;

//[追加]GyroZのデータを蓄積するための変数
float stockedGyroZs[10];
int stockCnt=0;
float adjustGyroZ=0;
int stockedGyroZLength=0;

// the setup routine runs once when M5Stack starts up
void setup(){

  stockedGyroZLength=sizeof(stockedGyroZs)/sizeof(int);

  // Initialize the M5Stack object
  M5.begin();
  /*
    Power chip connected to gpio21, gpio22, I2C device
    Set battery charging voltage and current
    If used battery, please call this function in your project
  */
  M5.Power.begin();
    
  M5.IMU.Init();

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);
}

// the loop routine runs over and over again forever
void loop() {
    // put your main code here, to run repeatedly:
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //[変更]これは使わない
  // M5.IMU.getAhrsData(&pitch,&roll,&yaw); 

  //[追加]起動時にstockedGyroZLengthの数だけデータを貯める
  if(stockCnt<stockedGyroZLength){
    stockedGyroZs[stockCnt]=gyroZ;
    stockCnt++;
  }else{
    if(adjustGyroZ==0){
      for(int i=0;i<stockedGyroZLength;i++){
        adjustGyroZ+=stockedGyroZs[i]/stockedGyroZLength;
      }
    }
    //貯めたデータの平均値を使ってgyroZを補正する
    gyroZ-=adjustGyroZ; 
    //ここでaccelデータと補正したgyroデータを使ってpitch・roll・yawを計算する
    MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ, &pitch, &roll, &yaw);
  }

  //ここから下は変更無し
  M5.IMU.getTempData(&temp);
  
  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
  M5.Lcd.setCursor(220, 42);
  M5.Lcd.print(" o/s");
  M5.Lcd.setCursor(0, 65);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
  M5.Lcd.setCursor(220, 87);
  M5.Lcd.print(" G");
  M5.Lcd.setCursor(0, 110);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
  M5.Lcd.setCursor(220, 132);
  M5.Lcd.print(" degree");
  M5.Lcd.setCursor(0, 155);
  M5.Lcd.printf("Temperature : %.2f C", temp);

  delay(1);
}