Unit DoF6 Arduino Tutorial

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

constexpr uint8_t BMI270_ADDRESS = 0x69;

m5::unit::UnitUnified units;
m5::unit::UnitDoF6 dof6{BMI270_ADDRESS};
m5::imu::imu_convert_param_t convert_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, 8);
    M5.Display.print("UNIT DoF6");
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(connected ? TFT_GREEN : TFT_ORANGE, TFT_BLACK);
    M5.Display.setCursor(240, 8);
    M5.Display.print(connected ? "READY" : "WAIT");
}

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) {
    // Clear the whole card before redrawing, otherwise old glyphs remain.
    M5.Display.fillRect(x, y - 4, 148, 88, TFT_BLACK);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_CYAN, TFT_BLACK);
    M5.Display.setCursor(x + 10, y + 8);
    M5.Display.printf("%s  (%s)   ", title, unit);
    M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);
    M5.Display.setCursor(x + 10, y + 28);
    M5.Display.printf("X  %+8.2f   ", x_value);
    M5.Display.setCursor(x + 10, y + 48);
    M5.Display.printf("Y  %+8.2f   ", y_value);
    M5.Display.setCursor(x + 10, y + 68);
    M5.Display.printf("Z  %+8.2f   ", z_value);
}

void drawData(float ax, float ay, float az, float gx, float gy, float gz, float temperature) {
    M5.Display.startWrite();
    drawVectorCard(8, 48, "ACCEL", "g", ax, ay, az);
    drawVectorCard(164, 48, "GYRO", "dps", gx, gy, gz);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_CYAN, TFT_BLACK);
    M5.Display.setCursor(8, 138);
    M5.Display.print("TEMPERATURE");
    M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);
    M5.Display.setTextSize(1);
    M5.Display.setCursor(8, 158);
    M5.Display.printf("%+7.1f C", temperature);
    M5.Display.setTextSize(1);
    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 DoF6...");

    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 = dof6.component_config();
    cfg.clock = 400000;
    dof6.component_config(cfg);

    if (!M5.Ex_I2C.begin() || !units.add(dof6, M5.Ex_I2C) || !units.begin()) {
        while (true) {
            Serial.println("Unit DoF6 not found. Check wiring and BMI270 address.");
            drawStatus("Sensor not found", TFT_ORANGE);
            delay(1000);
        }
    }

    dof6.imuConvertParam(convert_param);
    Serial.printf("Unit DoF6 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 raw{};
    static bool waiting_shown = false;
    if (m5::imu::any(dof6.readImuRaw(raw))) {
        waiting_shown = false;
        const float ax = raw.accel[0] * convert_param.accel_res;
        const float ay = raw.accel[1] * convert_param.accel_res;
        const float az = raw.accel[2] * convert_param.accel_res;
        const float gx = raw.gyro[0] * convert_param.gyro_res;
        const float gy = raw.gyro[1] * convert_param.gyro_res;
        const float gz = raw.gyro[2] * convert_param.gyro_res;
        const float temperature = raw.temp * convert_param.temp_res + convert_param.temp_offset;

        Serial.printf("Accel: %.3f, %.3f, %.3f g\n",
                      ax, ay, az);
        Serial.printf("Gyro:  %.3f, %.3f, %.3f dps\n",
                      gx, gy, gz);
        Serial.printf("Temperature: %.2f C\n\n", temperature);

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