Unit DoF10 Arduino Tutorial

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

constexpr uint8_t BMI270_ADDRESS = 0x69;

m5::unit::UnitUnified units;
m5::unit::UnitDoF10 dof10{BMI270_ADDRESS};
m5::imu::imu_convert_param_t imu_param{};
m5::imu::imu_convert_param_t mag_param{};

constexpr uint16_t COLOR_BG     = TFT_BLACK;
constexpr uint16_t COLOR_CARD   = TFT_BLACK;
constexpr uint16_t COLOR_BORDER = TFT_BLACK;
constexpr uint16_t COLOR_ACCENT = TFT_CYAN;
constexpr uint16_t COLOR_MUTED  = TFT_DARKGREY;
constexpr uint16_t COLOR_GOOD   = 0x5CE0;

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

void drawStatus(const char* message, uint16_t color) {
    M5.Display.fillScreen(COLOR_BG);
    drawHeader(false);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(color, COLOR_BG);
    M5.Display.setCursor(12, 72);
    M5.Display.print(message);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(COLOR_MUTED, COLOR_BG);
    M5.Display.setCursor(12, 108);
    M5.Display.print("I2C bus / sensor status");
}

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, COLOR_CARD);
    M5.Display.drawRoundRect(x, y, 148, 68, 8, COLOR_BORDER);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(COLOR_ACCENT, COLOR_CARD);
    M5.Display.setCursor(x + 10, y);
    M5.Display.printf("%s (%s)", title, unit);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_WHITE, COLOR_CARD);
    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, float pressure, float altitude) {
    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, COLOR_BG);
    M5.Display.fillRect(12, 138, 308, 20, COLOR_BG);
    M5.Display.fillRect(12, 158, 308, 24, COLOR_BG);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(COLOR_ACCENT, COLOR_CARD);
    M5.Display.setCursor(18, 126);
    M5.Display.print("MAG");
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_WHITE, COLOR_CARD);
    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(COLOR_ACCENT, COLOR_CARD);
    M5.Display.setCursor(18, 166);
    M5.Display.print("BARO");
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_WHITE, COLOR_CARD);
    M5.Display.setCursor(72, 166);
    M5.Display.printf("P %+7.1f  ALT %+6.1f m", pressure, altitude);
    M5.Display.setTextColor(COLOR_MUTED, COLOR_BG);
    M5.Display.setCursor(12, 218);
    M5.Display.print("LIVE  |  500 ms  |  I2C 400 kHz");
    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 DoF10...");

    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...", COLOR_ACCENT);

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

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

    dof10.imu().imuConvertParam(imu_param);
    dof10.mag().imuConvertParam(mag_param);
    Serial.printf("Unit DoF10 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(dof10.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(dof10.mag().readImuRaw(mag_raw));
    if (mag_updated) {
        Serial.printf("Mag:   %.2f, %.2f, %.2f uT\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);
    }

    const bool baro_updated = dof10.baro().updated();
    if (baro_updated) {
        Serial.printf("Pressure: %.2f hPa\n", dof10.baro().pressure() / 100.0f);
        Serial.printf("Temperature: %.2f C\n", dof10.baro().temperature());
        Serial.printf("Relative altitude: %.2f m\n\n", dof10.baro().altitude());
    }

    if (imu_updated || mag_updated || baro_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,
                 dof10.baro().pressure() / 100.0f, dof10.baro().altitude());
    } else {
        if (!waiting_shown) {
            waiting_shown = true;
            Serial.println("Waiting for Unit DoF10 data...");
            drawStatus("Waiting for data...", COLOR_ACCENT);
        }
    }
}