Unit DoF9 Arduino Tutorial

#include <M5Unified.h>
#include <M5UnitUnified.h>
#include <M5UnitUnifiedIMU.h>

constexpr uint8_t BMI270_ADDRESS = 0x69;

m5::unit::UnitUnified units;
m5::unit::UnitDoF9 dof9{BMI270_ADDRESS};
m5::imu::imu_convert_param_t imu_param{};
m5::imu::imu_convert_param_t mag_param{};

void drawHeader(bool connected) {
    M5.Display.fillScreen(TFT_BLACK);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_CYAN, TFT_BLACK);
    M5.Display.setCursor(12, 7);
    M5.Display.print("UNIT DoF9");
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(connected ? TFT_GREEN : TFT_ORANGE, TFT_BLACK);
    M5.Display.setCursor(220, 8);
    M5.Display.print(connected ? "READY" : "WAIT");
    M5.Display.fillCircle(304, 18, 5, connected ? TFT_GREEN : TFT_ORANGE);
}

void drawStatus(const char* message, uint16_t color) {
    M5.Display.fillScreen(TFT_BLACK);
    drawHeader(false);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(color, TFT_BLACK);
    M5.Display.setCursor(12, 72);
    M5.Display.print(message);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_DARKGREY, TFT_BLACK);
    M5.Display.setCursor(12, 108);
    M5.Display.print("I2C 400 kHz");
}

void drawVectorCard(int x, int y, const char* title, const char* unit,
                    float x_value, float y_value, float z_value) {
    M5.Display.fillRoundRect(x, y, 148, 68, 8, TFT_BLACK);
    M5.Display.drawRoundRect(x, y, 148, 68, 8, TFT_BLACK);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_CYAN, TFT_BLACK);
    M5.Display.setCursor(x + 10, y);
    M5.Display.printf("%s (%s)", title, unit);
    M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);
    M5.Display.setCursor(x + 10, y + 18);
    M5.Display.printf("X  %+7.2f", x_value);
    M5.Display.setCursor(x + 10, y + 34);
    M5.Display.printf("Y  %+7.2f", y_value);
    M5.Display.setCursor(x + 10, y + 50);
    M5.Display.printf("Z  %+7.2f", z_value);
}

void drawData(float ax, float ay, float az, float gx, float gy, float gz,
              float mx, float my, float mz) {
    M5.Display.startWrite();
    drawVectorCard(8, 50, "ACCEL", "g", ax, ay, az);
    drawVectorCard(164, 50, "GYRO", "dps", gx, gy, gz);
    M5.Display.fillRect(12, 118, 308, 20, TFT_BLACK);
    M5.Display.fillRect(12, 138, 308, 20, TFT_BLACK);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_CYAN, TFT_BLACK);
    M5.Display.setCursor(18, 126);
    M5.Display.print("MAG");
    M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);
    M5.Display.setCursor(72, 126);
    M5.Display.printf("X %+6.1f", mx);
    M5.Display.setCursor(164, 126);
    M5.Display.printf("Y %+6.1f", my);
    M5.Display.setCursor(72, 142);
    M5.Display.printf("Z %+6.1f uT", mz);
    M5.Display.setTextColor(TFT_DARKGREY, TFT_BLACK);
    M5.Display.setCursor(12, 218);
    M5.Display.print("LIVE  |  500 ms  |  I2C");
    M5.Display.endWrite();
}

void setup() {
    Serial.begin(115200);
    const uint32_t serial_start = millis();
    while (!Serial && millis() - serial_start < 3000) {
        delay(10);
    }
    Serial.println("Starting Unit DoF9...");

    auto config = M5.config();
    M5.begin(config);
    M5.Display.setFont(&fonts::FreeSansBold9pt7b);
    M5.Display.setTextSize(1);
    M5.Display.setRotation(1);
    M5.Display.setTextWrap(false, false);
    drawStatus("Initializing...", TFT_YELLOW);

    auto cfg = dof9.component_config();
    cfg.clock = 400000;
    dof9.component_config(cfg);

    if (!M5.Ex_I2C.begin() || !units.add(dof9, M5.Ex_I2C) || !units.begin()) {
        while (true) {
            Serial.println("Unit DoF9 not found. Check wiring and I2C addresses.");
            drawStatus("Sensor not found", TFT_ORANGE);
            delay(1000);
        }
    }

    dof9.imu().imuConvertParam(imu_param);
    dof9.mag().imuConvertParam(mag_param);
    Serial.printf("Unit DoF9 ready, BMI270 address: 0x%02X\n", BMI270_ADDRESS);
    drawHeader(true);
}

void loop() {
    M5.update();
    units.update();

    static uint32_t last_output = 0;
    if (millis() - last_output < 500) {
        return;
    }
    last_output = millis();

    m5::imu::imu_raw_data_t imu_raw{};
    static bool waiting_shown = false;
    const bool imu_updated = m5::imu::any(dof9.imu().readImuRaw(imu_raw));
    if (imu_updated) {
        Serial.printf("Accel: %.3f, %.3f, %.3f g\n",
                      imu_raw.accel[0] * imu_param.accel_res,
                      imu_raw.accel[1] * imu_param.accel_res,
                      imu_raw.accel[2] * imu_param.accel_res);
        Serial.printf("Gyro:  %.3f, %.3f, %.3f dps\n",
                      imu_raw.gyro[0] * imu_param.gyro_res,
                      imu_raw.gyro[1] * imu_param.gyro_res,
                      imu_raw.gyro[2] * imu_param.gyro_res);
    }

    m5::imu::imu_raw_data_t mag_raw{};
    const bool mag_updated = m5::imu::any(dof9.mag().readImuRaw(mag_raw));
    if (mag_updated) {
        Serial.printf("Mag:   %.2f, %.2f, %.2f uT\n\n",
                      mag_raw.mag[0] * mag_param.mag_res,
                      mag_raw.mag[1] * mag_param.mag_res,
                      mag_raw.mag[2] * mag_param.mag_res);
    }

    if (imu_updated || mag_updated) {
        waiting_shown = false;
        const float ax = imu_raw.accel[0] * imu_param.accel_res;
        const float ay = imu_raw.accel[1] * imu_param.accel_res;
        const float az = imu_raw.accel[2] * imu_param.accel_res;
        const float gx = imu_raw.gyro[0] * imu_param.gyro_res;
        const float gy = imu_raw.gyro[1] * imu_param.gyro_res;
        const float gz = imu_raw.gyro[2] * imu_param.gyro_res;
        const float mx = mag_raw.mag[0] * mag_param.mag_res;
        const float my = mag_raw.mag[1] * mag_param.mag_res;
        const float mz = mag_raw.mag[2] * mag_param.mag_res;

        drawData(ax, ay, az, gx, gy, gz, mx, my, mz);
    } else {
        if (!waiting_shown) {
            waiting_shown = true;
            Serial.println("Waiting for Unit DoF9 data...");
            drawStatus("Waiting for data...", TFT_YELLOW);
        }
    }
}