M5UnifiedでIMUの各値を棒グラフで表示するサンプルプログラム。roll,pitch,yawは相補フィルタで計算して表示

M5Unified_IMU_graph.cpp

#include <M5Unified.h>


// Strength of the calibration operation;
// 0: disables calibration.
// 1 is weakest and 255 is strongest.
static constexpr const uint8_t calib_value = 64;

// 姿勢角用の設定
static constexpr float COMPLEMENTARY_FILTER = 0.96f;    // 相補フィルター係数


// 姿勢角計算用の変数
static float roll = 0.0f;
static float pitch = 0.0f;
static float yaw = 0.0f;
static uint32_t last_update_time = 0;

// ローパスフィルター関数
float lowPassFilter(float current, float previous, float alpha) {
    return previous + alpha * (current - previous);
}

// 姿勢角を計算する関数
void updateAttitude(const m5::imu_data_t& data) {
    uint32_t current_time = millis();
    float dt = (current_time - last_update_time) / 1000.0f;
    last_update_time = current_time;

    if (dt <= 0.0f || dt > 0.2f) {
        dt = 0.01f;  // 初回や異常値の場合のデフォルト値
    }

    // 加速度からの姿勢角計算
    float accel_roll = atan2(data.accel.y, data.accel.z) * 180.0f / PI;
    float accel_pitch = atan2(-data.accel.x, sqrt(data.accel.y * data.accel.y + data.accel.z * data.accel.z)) * 180.0f / PI;

    // ジャイロ積分値の計算
    float gyro_roll = roll + data.gyro.x * dt;
    float gyro_pitch = pitch + data.gyro.y * dt;
    float gyro_yaw = yaw + data.gyro.z * dt;

    // 相補フィルターで組み合わせ
    roll = COMPLEMENTARY_FILTER * gyro_roll + (1.0f - COMPLEMENTARY_FILTER) * accel_roll;
    pitch = COMPLEMENTARY_FILTER * gyro_pitch + (1.0f - COMPLEMENTARY_FILTER) * accel_pitch;
    yaw = gyro_yaw;  // ヨーは補正なし

    // ±180度に正規化
    if (yaw > 180.0f) yaw -= 360.0f;
    else if (yaw < -180.0f) yaw += 360.0f;
}


struct rect_t
{
  int32_t x;
  int32_t y;
  int32_t w;
  int32_t h;
};

static constexpr const uint32_t color_tbl[18] = 
{
0xFF0000u, 0xCCCC00u, 0xCC00FFu,
0xFFCC00u, 0x00FF00u, 0x0088FFu,
0xFF00CCu, 0x00FFCCu, 0x0000FFu,
0xFF0000u, 0xCCCC00u, 0xCC00FFu,
0xFFCC00u, 0x00FF00u, 0x0088FFu,
0xFF00CCu, 0x00FFCCu, 0x0000FFu,
};
static constexpr const float coefficient_tbl[3] = { 0.5f, (1.0f / 256.0f), (1.0f / 1024.0f) };

static constexpr const char* sensor_labels[9] = {
  "accel_x", "accel_y", "accel_z",
  "gyro_x", "gyro_y", "gyro_z",
  "roll", "pitch", "yaw"
};

static auto &dsp = (M5.Display);
static rect_t rect_graph_area;
static rect_t rect_text_area;

static uint8_t calib_countdown = 0;

static int prev_xpos[18];

void drawBar(int32_t ox, int32_t oy, int32_t nx, int32_t px, int32_t h, uint32_t color)
{
  uint32_t bgcolor = (color >> 3) & 0x1F1F1Fu;
  if (px && ((nx < 0) != (px < 0)))
  {
    dsp.fillRect(ox, oy, px, h, bgcolor);
    px = 0;
  }
  if (px != nx)
  {
    if ((nx > px) != (nx < 0))
    {
      bgcolor = color;
    }
    dsp.setColor(bgcolor);
    dsp.fillRect(nx + ox, oy, px - nx, h);
  }
}

void drawGraph(const rect_t& r, const m5::imu_data_t& data)
{
  float aw = (128 * r.w) >> 1;
  float gw = (128 * r.w) / 256.0f;
  float mw = (128 * r.w) / 1024.0f;
  int ox = (r.x + r.w)>>1;
  int oy = r.y;
  int h = (r.h / 18) * (calib_countdown ? 1 : 2);
  int bar_count = 9 * (calib_countdown ? 2 : 1);

  // Set text size for sensor values
  dsp.setTextSize(1);

  dsp.startWrite();
  for (int index = 0; index < bar_count; ++index)
  {
    float xval;
    if (index < 9)
    {
      auto coe = coefficient_tbl[index / 3] * r.w;
      xval = data.value[index] * coe;

      // Draw sensor label and value
      dsp.setTextColor(color_tbl[index], TFT_BLACK);
      dsp.setCursor(4, oy + h * index + 2);
      dsp.printf("%s: %.2f", sensor_labels[index], data.value[index]);
    }
    else
    {
      xval = M5.Imu.getOffsetData(index - 9) * (1.0f / (1 << 19));
    }

    float tmp = xval;
    int nx = tmp;
    int px = prev_xpos[index];
    if (nx != px)
    prev_xpos[index] = nx;
    drawBar(ox, oy + h * index, nx, px, h - 1, color_tbl[index]);
  }
  dsp.endWrite();
}

void updateCalibration(uint32_t c, bool clear = false)
{
  calib_countdown = c;

  if (c == 0) {
    clear = true;
  }

  if (clear)
  {
    memset(prev_xpos, 0, sizeof(prev_xpos));
    dsp.fillScreen(TFT_BLACK);

    if (c)
    { // Start calibration.
      M5.Imu.setCalibration(calib_value, calib_value, calib_value);
    }
    else
    { // Stop calibration. (Continue calibration only for the geomagnetic sensor)
      M5.Imu.setCalibration(0, 0, calib_value);
      // save calibration values.
      M5.Imu.saveOffsetToNVS();
    }
  }

  auto backcolor = (c == 0) ? TFT_BLACK : TFT_BLUE;
  dsp.fillRect(rect_text_area.x, rect_text_area.y, rect_text_area.w, rect_text_area.h, backcolor);

  if (c)
  {
    dsp.setCursor(rect_text_area.x + 2, rect_text_area.y + 1);
    dsp.setTextColor(TFT_WHITE, TFT_BLUE);
    dsp.printf("Countdown:%d ", c);
  }
}

void startCalibration(void)
{
  updateCalibration(10, true);
}

void setup(void)
{
  auto cfg = M5.config();
  cfg.serial_baudrate = 9600;
  M5.begin(cfg);

  int32_t w = dsp.width();
  int32_t h = dsp.height();
  if (w < h)
  {
    dsp.setRotation(dsp.getRotation() ^ 1);
    w = dsp.width();
    h = dsp.height();
  }
  int32_t graph_area_h = ((h - 8) / 18) * 18;
  int32_t text_area_h = h - graph_area_h;
  float fontsize = text_area_h / 8;
  dsp.setTextSize(fontsize);

  rect_graph_area = { 0, 0, w, graph_area_h };
  rect_text_area = {0, graph_area_h, w, text_area_h };


  // Read calibration values from NVS.
  if (!M5.Imu.loadOffsetFromNVS())
  {
    startCalibration();
  }
}

void loop(void)
{
  static uint32_t frame_count = 0;
  static uint32_t prev_sec = 0;

  auto imu_update = M5.Imu.update();
  if (imu_update)
  {
    auto data = M5.Imu.getImuData();
    
    // 姿勢角の更新
    updateAttitude(data);

    // グラフ表示用にデータを更新（roll, pitch, yawを表示）
    data.value[6] = roll;   // roll
    data.value[7] = pitch;  // pitch
    data.value[8] = yaw;    // yaw
    
    drawGraph(rect_graph_area, data);

    ++frame_count;
  }
  else
  {
    M5.update();

    // Calibration is initiated when a button or screen is clicked.
    if (M5.BtnA.wasClicked() || M5.BtnPWR.wasClicked() || M5.Touch.getDetail().wasClicked())
    {
      startCalibration();
    }
  }

  int32_t sec = millis() / 1000;
  if (prev_sec != sec)
  {
    prev_sec = sec;
    M5_LOGI("sec:%d  frame:%d", sec, frame_count);
    frame_count = 0;

    if (calib_countdown)
    {
      updateCalibration(calib_countdown - 1);
    }

    if ((sec & 7) == 0)
    { // prevent WDT.
      vTaskDelay(1);
    }
  }
}