CoreMatrix IMU 姿态传感器

#include <M5Unified.h>

static void drawStatus(bool ready, int x = 7, int y = 7)
{
  M5.Display.startWrite();
  M5.Display.fillScreen(TFT_BLACK);
  M5.Display.drawRect(0, 0, 16, 16, TFT_WHITE);

  if (!ready) {
    M5.Display.drawLine(3, 3, 12, 12, TFT_WHITE);
    M5.Display.drawLine(12, 3, 3, 12, TFT_WHITE);
  } else {
    M5.Display.fillRect(7, 7, 2, 2, TFT_WHITE);
    M5.Display.fillRect(x, y, 3, 3, TFT_WHITE);
  }

  M5.Display.endWrite();
  M5.Display.display();
}

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

  M5.Display.setColorDepth(8);
  M5.Display.setRotation(1);
  M5.Display.setBrightness(128);

  Serial.printf("BMI270 enabled: %s\n", M5.Imu.isEnabled() ? "yes" : "no");
  drawStatus(M5.Imu.isEnabled());
}

void loop()
{
  if (!M5.Imu.isEnabled()) {
    delay(1000);
    return;
  }

  const auto updated = M5.Imu.update();
  if (updated & m5::IMU_Class::sensor_mask_accel) {
    const auto& data = M5.Imu.getImuData();

    const int x = constrain(static_cast<int>((data.accel.y + 1.0f) * 7.5f), 1, 13);
    const int y = constrain(static_cast<int>((data.accel.x + 1.0f) * 7.5f), 1, 13);
    drawStatus(true, x, y);

    Serial.printf("ax: %.3f  ay: %.3f  az: %.3f\n",
                  data.accel.x, data.accel.y, data.accel.z);
    Serial.printf("gx: %.3f  gy: %.3f  gz: %.3f\n",
                  data.gyro.x, data.gyro.y, data.gyro.z);
  }

  delay(50);
}