M5Stack のIMUでProcessingのモデルをぐりぐりしてみた。

スイッチサイエンスのサイトにM5Stackの３DモデルをM5の動きに合わせて動かすサイトがあって、
pages.switch-science.com

面白いなぁ～と思い、自分もグリグリやってみたいなぁと思ったんですが、あいにく、IMUのチップがサイトのものと違っていました。
自分のは、IMU6886＋BMM150のようで、サイトのスケッチそのままでは、動かないみたいです。
幸い、BMM150のサンプルプログラムも出ていたので、これと組み合わせて同じことをやってみました。
なんとか、同じようにできることはできたのですが、どうも、ジャイロの値が安定しない。
キャリブレーションもやってみたのだけど、いまいち。
何か、プログラムが悪いのかなぁ～？　あるいは、こんなもん？　またまた、はずれセンサーを引いちゃった？
まぁ、今後の参考までに、とりあえず、スケッチを置いておこう。
M5側のスケッチを置き換えるだけで、後は、Processingなどの部分は、上記のサイトと全く同じでグリグリできるようになると思います。
ちなみに、表示部分は、自分のM5のTFTがある日突然オシャカになってしまったので、ちゃんと確認できてませんので、なんか、変かも…。

#define M5STACK_MPU6886 

#include <Arduino.h>
#include <Wire.h>
#include "Preferences.h"
#include "M5Stack.h"
#include "math.h"
#include "bmm150.h"
#include "bmm150_defs.h"
#include <MadgwickAHRS.h>
#include "BluetoothSerial.h"

Preferences prefs;
BluetoothSerial SerialBT;
Madgwick filter;
unsigned long microsPerReading, microsPrevious;

struct bmm150_dev dev;
bmm150_mag_data mag_offset;
bmm150_mag_data mag_max;
bmm150_mag_data mag_min;
TFT_eSprite img = TFT_eSprite(&M5.Lcd);

float accX = 0.0F, accY = 0.0F, accZ = 0.0F;
float gyroX = 0.0F, gyroY = 0.0F, gyroZ = 0.0F;
float pitch = 0.0F, roll = 0.0F, yaw = 0.0F;

float accOffset[3];
float gyroOffset[3];

#define INTERVAL  5
float rolls[INTERVAL], pitchs[INTERVAL], yaws[INTERVAL];
int ma_idx = 0; // moving average index


int8_t i2c_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len)
{
    if(M5.I2C.readBytes(dev_id, reg_addr, len, read_data))
    {
        return BMM150_OK;
    }
    else
    {
        return BMM150_E_DEV_NOT_FOUND;
    }
}

int8_t i2c_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len)
{
    if(M5.I2C.writeBytes(dev_id, reg_addr, read_data, len))
    {
        return BMM150_OK;
    }
    else
    {
        return BMM150_E_DEV_NOT_FOUND;
    }
}

int8_t bmm150_initialization()
{
    int8_t rslt = BMM150_OK;

    /* Sensor interface over SPI with native chip select line */
    dev.dev_id = 0x10;
    dev.intf = BMM150_I2C_INTF;
    dev.read = i2c_read;
    dev.write = i2c_write;
    dev.delay_ms = delay;

    /* make sure max < mag data first  */
    mag_max.x = -2000;
    mag_max.y = -2000;
    mag_max.z = -2000;

    /* make sure min > mag data first  */
    mag_min.x = 2000;
    mag_min.y = 2000;
    mag_min.z = 2000;

    rslt = bmm150_init(&dev);
    dev.settings.pwr_mode = BMM150_NORMAL_MODE;
    rslt |= bmm150_set_op_mode(&dev);
    dev.settings.preset_mode = BMM150_PRESETMODE_ENHANCED;
    rslt |= bmm150_set_presetmode(&dev);
    return rslt;
}

void bmm150_offset_save()
{
    prefs.begin("bmm150", false);
    prefs.putBytes("offset", (uint8_t *)&mag_offset, sizeof(bmm150_mag_data));
    prefs.end();
}

void bmm150_offset_load()
{
    if(prefs.begin("bmm150", true))
    {
        prefs.getBytes("offset", (uint8_t *)&mag_offset, sizeof(bmm150_mag_data));
        prefs.end();
        Serial.printf("bmm150 load offset finish.... \r\n");
    }
    else
    {
        Serial.printf("bmm150 load offset failed.... \r\n");
    }
}

void setup() 
{
    M5.begin(true, false, true, false);
    M5.Power.begin();
    
    M5.IMU.Init();
    Serial.begin(115200);
    Wire.begin(21, 22, 400000);
    SerialBT.begin("M5Stack");

    filter.begin(10); // 10Hz filterを初期化する
    microsPerReading = 1000000 / 10;
    microsPrevious = micros();
    
    img.setColorDepth(1);
    img.setTextColor(TFT_WHITE);
    img.createSprite(320, 240);
    img.setBitmapColor(TFT_WHITE, 0);

    if(bmm150_initialization() != BMM150_OK)
    {
        img.fillSprite(0);
        img.drawCentreString("BMM150 init failed", 160, 110, 4);
        img.pushSprite(0, 0);
        for(;;)
        {
            delay(100);
        }
    }

    bmm150_offset_load();

    for (int i = 0; i<INTERVAL; i++) {
      rolls[i] = 0;
      pitchs[i] = 0;
      yaws[i] = 0;
    }

}

void bmm150_calibrate(uint32_t calibrate_time)
{

        img.fillSprite(0);
        img.drawCentreString("Flip + rotate core calibration", 160, 110, 4);
        img.pushSprite(0, 0);

    uint32_t calibrate_timeout = 0;

    calibrate_timeout = millis() + calibrate_time;
    Serial.printf("Go calibrate, use %d ms \r\n", calibrate_time);
    Serial.printf("running ...");

    while (calibrate_timeout > millis())
    {
        bmm150_read_mag_data(&dev);
        if(dev.data.x)
        {
            mag_min.x = (dev.data.x < mag_min.x) ? dev.data.x : mag_min.x;
            mag_max.x = (dev.data.x > mag_max.x) ? dev.data.x : mag_max.x;
        }

        if(dev.data.y)
        {
            mag_max.y = (dev.data.y > mag_max.y) ? dev.data.y : mag_max.y;
            mag_min.y = (dev.data.y < mag_min.y) ? dev.data.y : mag_min.y;
        }

        if(dev.data.z)
        {
            mag_min.z = (dev.data.z < mag_min.z) ? dev.data.z : mag_min.z;
            mag_max.z = (dev.data.z > mag_max.z) ? dev.data.z : mag_max.z;
        }
        delay(100);
    }

    mag_offset.x = (mag_max.x + mag_min.x) / 2;
    mag_offset.y = (mag_max.y + mag_min.y) / 2;
    mag_offset.z = (mag_max.z + mag_min.z) / 2;
    bmm150_offset_save();

    Serial.printf("\n calibrate finish ... \r\n");
    Serial.printf("mag_max.x: %.2f x_min: %.2f \t", mag_max.x, mag_min.x);
    Serial.printf("y_max: %.2f y_min: %.2f \t", mag_max.y, mag_min.y);
    Serial.printf("z_max: %.2f z_min: %.2f \r\n", mag_max.z, mag_min.z);
}

void calibrate6886(){
  M5.Lcd.println("calibrate6886");
  float gyroSum[3];
  float accSum[3];
  int counter = 500;
  for(int i = 0; i < counter; i++){
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    gyroSum[0] += gyroX;
    gyroSum[1] += gyroY;
    gyroSum[2] += gyroZ;
    accSum[0] += accX;
    accSum[1] += accY;
    accSum[2] += accZ;
    delay(2);
  }
  gyroOffset[0] = gyroSum[0]/counter;
  gyroOffset[1] = gyroSum[1]/counter;
  gyroOffset[2] = gyroSum[2]/counter;
  accOffset[0] = accSum[0]/counter;
  accOffset[1] = accSum[1]/counter;
  accOffset[2] = (accSum[2]/counter);
}



void loop() 
{
  if (micros() - microsPrevious >= microsPerReading) {
// ----------------- Accl / Gyro -------------------
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    //M5.IMU.getAhrsData(&pitch,&roll,&yaw);

    gyroX -= gyroOffset[0];
    gyroY -= gyroOffset[1];
    gyroZ -= gyroOffset[2];
  
    accX -= accOffset[0];
    accY -= accOffset[1];
    accZ -= accOffset[2]-1.0;
  
    filter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ );
        rolls[ma_idx % INTERVAL] = filter.getRoll();
        pitchs[ma_idx % INTERVAL] = filter.getPitch();
        yaws[ma_idx % INTERVAL] = filter.getYaw();
        ma_idx++;
        if((ma_idx % INTERVAL) == 0) ma_idx = 0;

        float roll = 0.0;
        float pitch = 0.0;
        float yaw = 0.0;
        for (int i = 0; i < INTERVAL; i++) {
            roll += rolls[i];
            pitch += pitchs[i];
            yaw += yaws[i];
        }
        roll /= INTERVAL;
        pitch /= INTERVAL;
        yaw /= INTERVAL;

// ----------------- magnet ------------------------  
      char text_string[100];
      M5.update();
      bmm150_read_mag_data(&dev);
      float head_dir = atan2(dev.data.x -  mag_offset.x, dev.data.y - mag_offset.y) * 180.0 / M_PI;

      
        img.fillSprite(0);
    
        sprintf(text_string, "Gyro: %.2f  %.2f  %.2f", gyroX, gyroY, gyroZ);
        img.drawString(text_string, 10, 20, 4);
        sprintf(text_string, "Accel: %.2f  %.2f  %.2f", accX, accY, accZ);
        img.drawString(text_string, 10, 50, 4);
        sprintf(text_string, "P/R/Y: %.2f  %.2f  %.2f",pitch, roll, yaw);
        img.drawString(text_string, 10, 80, 4); 
        Serial.printf("%6.2f, %6.2f, %6.2f\r\n", roll, pitch, yaw);
        SerialBT.printf("%6.2f, %6.2f, %6.2f\r\n", roll, pitch, yaw);
        sprintf(text_string, "HEAD Angle: %.2f", head_dir);
        img.drawString(text_string, 10, 110, 4);
    
        img.drawCentreString("Press BtnA enter calibrate", 160, 150, 4);
        img.pushSprite(0, 0);
      
      if(M5.BtnA.wasPressed())
      {
          img.fillSprite(0);
          img.drawCentreString("Keep Stable - Gyro Accel calibration", 160, 110, 4);
          img.pushSprite(0, 0);
          calibrate6886();
          bmm150_calibrate(10000);
      }

      microsPrevious = microsPrevious + microsPerReading;
  }  
}