Tactility/Drivers/mpu6886-module/source/mpu6886.cpp

// SPDX-License-Identifier: Apache-2.0
#include <drivers/mpu6886.h>
#include <tactility/device.h>
#include <tactility/drivers/i2c_controller.h>
#include <tactility/log.h>

#define TAG "MPU6886"

// Register map
static constexpr uint8_t REG_SMPLRT_DIV    = 0x19; // sample rate divider
static constexpr uint8_t REG_CONFIG        = 0x1A; // DLPF config
static constexpr uint8_t REG_GYRO_CONFIG   = 0x1B; // gyro full-scale select
static constexpr uint8_t REG_ACCEL_CONFIG  = 0x1C; // accel full-scale select
static constexpr uint8_t REG_ACCEL_CONFIG2 = 0x1D; // accel low-pass filter
static constexpr uint8_t REG_INT_PIN_CFG   = 0x37; // interrupt pin config
static constexpr uint8_t REG_INT_ENABLE    = 0x38; // interrupt enable
static constexpr uint8_t REG_ACCEL_XOUT_H  = 0x3B; // first accel output register
static constexpr uint8_t REG_TEMP_OUT_H    = 0x41; // temperature output register
static constexpr uint8_t REG_GYRO_XOUT_H   = 0x43; // first gyro output register
static constexpr uint8_t REG_USER_CTRL     = 0x6A; // user control (DMP, FIFO, I2C)
static constexpr uint8_t REG_PWR_MGMT_1    = 0x6B; // power management 1
static constexpr uint8_t REG_PWR_MGMT_2    = 0x6C; // power management 2
static constexpr uint8_t REG_FIFO_EN       = 0x23; // FIFO enable
static constexpr uint8_t REG_WHO_AM_I      = 0x75; // chip ID — expect 0x19

static constexpr uint8_t WHO_AM_I_VALUE    = 0x19;
static constexpr uint8_t WHO_AM_I_ATTEMPTS = 5;

// Configuration values
// GYRO_CONFIG: FS_SEL=3 (±2000°/s), FCHOICE_B=00 → 0x18
static constexpr uint8_t GYRO_CONFIG_VAL   = 0x18;
// ACCEL_CONFIG: AFS_SEL=2 (±8g) → 0x10
static constexpr uint8_t ACCEL_CONFIG_VAL  = 0x10;
// CONFIG: DLPF_CFG=1 → gyro BW=176Hz, temp BW=188Hz → 0x01
static constexpr uint8_t CONFIG_VAL        = 0x01;
// SMPLRT_DIV: sample rate = 1kHz / (1 + 5) = 166Hz → 0x05
static constexpr uint8_t SMPLRT_DIV_VAL   = 0x05;

// Scaling: full-scale / 2^15
static constexpr float ACCEL_SCALE = 8.0f / 32768.0f;    // g per LSB (±8g)
static constexpr float GYRO_SCALE  = 2000.0f / 32768.0f; // °/s per LSB (±2000°/s)

static constexpr TickType_t I2C_TIMEOUT_TICKS = pdMS_TO_TICKS(10);

#define GET_CONFIG(device) (static_cast<const Mpu6886Config*>((device)->config))

// region Driver lifecycle

// Applies the devicetree mount matrix (sensor frame -> device frame) to a 3-axis reading.
// Flat row-major [9]: x' = m0*x + m1*y + m2*z, y' = m3*x + m4*y + m5*z, z' = m6*x + m7*y + m8*z.
static void applyMountMatrix(const Mpu6886Config& config, float& x, float& y, float& z) {
    const int32_t* m = config.mount_matrix;
    const float rx = m[0] * x + m[1] * y + m[2] * z;
    const float ry = m[3] * x + m[4] * y + m[5] * z;
    const float rz = m[6] * x + m[7] * y + m[8] * z;
    x = rx;
    y = ry;
    z = rz;
}

static error_t start(Device* device) {
    auto* i2c_controller = device_get_parent(device);
    if (device_get_type(i2c_controller) != &I2C_CONTROLLER_TYPE) {
        LOG_E(TAG, "Parent is not an I2C controller");
        return ERROR_RESOURCE;
    }

    auto address = GET_CONFIG(device)->address;

    // Verify chip ID: it might take more than 1 attempt at power up (on M5Stack Core2)
    uint8_t who_am_i = 0;
    for (int i = 0; i < WHO_AM_I_ATTEMPTS; i++) {
        if (i2c_controller_register8_get(i2c_controller, address, REG_WHO_AM_I, &who_am_i, I2C_TIMEOUT_TICKS) == ERROR_NONE) {
            break;
        }
        vTaskDelay(10);
    }

    if (who_am_i != WHO_AM_I_VALUE) {
        LOG_E(TAG, "WHO_AM_I mismatch: got 0x%02X, expected 0x%02X", who_am_i, WHO_AM_I_VALUE);
        return ERROR_RESOURCE;
    }

    // Wake from sleep (clear all PWR_MGMT_1 bits)
    if (i2c_controller_register8_set(i2c_controller, address, REG_PWR_MGMT_1, 0x00, I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    vTaskDelay(pdMS_TO_TICKS(10));

    // Device reset (bit 7)
    if (i2c_controller_register8_set(i2c_controller, address, REG_PWR_MGMT_1, 0x80, I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    vTaskDelay(pdMS_TO_TICKS(10));

    // Select auto clock (CLKSEL=1: PLL with gyro reference when available)
    if (i2c_controller_register8_set(i2c_controller, address, REG_PWR_MGMT_1, 0x01, I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    vTaskDelay(pdMS_TO_TICKS(10));

    // Configure accel: ±8g
    if (i2c_controller_register8_set(i2c_controller, address, REG_ACCEL_CONFIG,  ACCEL_CONFIG_VAL, I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // Configure gyro: ±2000°/s
    if (i2c_controller_register8_set(i2c_controller, address, REG_GYRO_CONFIG,   GYRO_CONFIG_VAL,  I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // DLPF: gyro BW=176Hz
    if (i2c_controller_register8_set(i2c_controller, address, REG_CONFIG,        CONFIG_VAL,       I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // Sample rate: 1kHz / (1+5) = 166Hz
    if (i2c_controller_register8_set(i2c_controller, address, REG_SMPLRT_DIV,    SMPLRT_DIV_VAL,   I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // Clear interrupt enables before reconfiguring
    if (i2c_controller_register8_set(i2c_controller, address, REG_INT_ENABLE,    0x00,             I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // Accel low-pass filter: ACCEL_FCHOICE_B=0, A_DLPF_CFG=0
    if (i2c_controller_register8_set(i2c_controller, address, REG_ACCEL_CONFIG2, 0x00,             I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // Disable DMP and FIFO
    if (i2c_controller_register8_set(i2c_controller, address, REG_USER_CTRL,     0x00,             I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    if (i2c_controller_register8_set(i2c_controller, address, REG_FIFO_EN,       0x00,             I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // Interrupt: active-high (ACTL=0), push-pull (OPEN=0), latched (LATCH_INT_EN=1), cleared on any read (INT_ANYRD_2CLEAR=1) → 0x30
    if (i2c_controller_register8_set(i2c_controller, address, REG_INT_PIN_CFG,   0x30,             I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;
    // Enable DATA_RDY interrupt
    if (i2c_controller_register8_set(i2c_controller, address, REG_INT_ENABLE,    0x01,             I2C_TIMEOUT_TICKS) != ERROR_NONE) return ERROR_RESOURCE;

    return ERROR_NONE;
}

static error_t stop(Device* device) {
    auto* i2c_controller = device_get_parent(device);
    if (device_get_type(i2c_controller) != &I2C_CONTROLLER_TYPE) {
        LOG_E(TAG, "Parent is not an I2C controller");
        return ERROR_RESOURCE;
    }

    auto address = GET_CONFIG(device)->address;

    // Put device to sleep (set SLEEP bit in PWR_MGMT_1)
    if (i2c_controller_register8_set(i2c_controller, address, REG_PWR_MGMT_1, 0x40, I2C_TIMEOUT_TICKS) != ERROR_NONE) {
        LOG_E(TAG, "Failed to put MPU6886 to sleep");
        return ERROR_RESOURCE;
    }

    return ERROR_NONE;
}

// endregion

extern "C" {

error_t mpu6886_read_accel(Device* device, ImuAccelData* data) {
    auto* i2c_controller = device_get_parent(device);
    auto address = GET_CONFIG(device)->address;

    auto toI16 = [](uint8_t hi, uint8_t lo) -> int16_t {
        return static_cast<int16_t>(static_cast<uint16_t>(hi) << 8 | lo);
    };

    uint8_t buf[6] = {};
    error_t error = i2c_controller_read_register(i2c_controller, address, REG_ACCEL_XOUT_H, buf, sizeof(buf), I2C_TIMEOUT_TICKS);
    if (error != ERROR_NONE) return error;

    float axes[3] = {
        toI16(buf[0], buf[1]) * ACCEL_SCALE,
        toI16(buf[2], buf[3]) * ACCEL_SCALE,
        toI16(buf[4], buf[5]) * ACCEL_SCALE,
    };
    applyMountMatrix(*GET_CONFIG(device), axes[0], axes[1], axes[2]);
    data->ax = axes[0];
    data->ay = axes[1];
    data->az = axes[2];

    return ERROR_NONE;
}

error_t mpu6886_read_gyro(Device* device, ImuGyroData* data) {
    auto* i2c_controller = device_get_parent(device);
    auto address = GET_CONFIG(device)->address;

    auto toI16 = [](uint8_t hi, uint8_t lo) -> int16_t {
        return static_cast<int16_t>(static_cast<uint16_t>(hi) << 8 | lo);
    };

    uint8_t buf[6] = {};
    error_t error = i2c_controller_read_register(i2c_controller, address, REG_GYRO_XOUT_H, buf, sizeof(buf), I2C_TIMEOUT_TICKS);
    if (error != ERROR_NONE) return error;

    float gx = toI16(buf[0], buf[1]) * GYRO_SCALE;
    float gy = toI16(buf[2], buf[3]) * GYRO_SCALE;
    float gz = toI16(buf[4], buf[5]) * GYRO_SCALE;
    applyMountMatrix(*GET_CONFIG(device), gx, gy, gz);
    data->gx = gx;
    data->gy = gy;
    data->gz = gz;

    return ERROR_NONE;
}

error_t mpu6886_read_temperature(Device* device, float* temperature_c) {
    auto* i2c_controller = device_get_parent(device);
    auto address = GET_CONFIG(device)->address;

    uint8_t buf[2] = {};
    error_t error = i2c_controller_read_register(i2c_controller, address, REG_TEMP_OUT_H, buf, sizeof(buf), I2C_TIMEOUT_TICKS);
    if (error != ERROR_NONE) return error;

    auto raw = static_cast<int16_t>(static_cast<uint16_t>(buf[0]) << 8 | buf[1]);

    // Datasheet: temperature = raw / 326.8 + 25 (°C)
    *temperature_c = static_cast<float>(raw) / 326.8f + 25.0f;
    return ERROR_NONE;
}

ImuApi mpu6886_imu_api = {
    .read_accel = mpu6886_read_accel,
    .read_gyro = mpu6886_read_gyro,
    .read_temperature = mpu6886_read_temperature
};

Driver mpu6886_driver = {
    .name = "mpu6886",
    .compatible = (const char*[]) { "invensense,mpu6886", nullptr },
    .start_device = start,
    .stop_device = stop,
    .api = &mpu6886_imu_api,
    .device_type = &IMU_TYPE,
    .internal = nullptr
};

} // extern "C"