IMU

sensors/ICM42688P/ICM42688P.c

@@ -0,0 +1,297 @@
+#include "ICM42688P_regs.h"
+#include "ICM42688P.h"
+
+#define TAG "ICM42688P"
+
+#define ICM42688P_TIMEOUT 100
+
+struct ICM42688P {
+    FuriHalSpiBusHandle* spi_bus;
+    const GpioPin* irq_pin;
+    float accel_scale;
+    float gyro_scale;
+};
+
+static const struct AccelFullScale {
+    float value;
+    uint8_t reg_mask;
+} accel_fs_modes[] = {
+    [AccelFullScale16G] = {16.f, ICM42688_AFS_16G},
+    [AccelFullScale8G] = {8.f, ICM42688_AFS_8G},
+    [AccelFullScale4G] = {4.f, ICM42688_AFS_4G},
+    [AccelFullScale2G] = {2.f, ICM42688_AFS_2G},
+};
+
+static const struct GyroFullScale {
+    float value;
+    uint8_t reg_mask;
+} gyro_fs_modes[] = {
+    [GyroFullScale2000DPS] = {2000.f, ICM42688_GFS_2000DPS},
+    [GyroFullScale1000DPS] = {1000.f, ICM42688_GFS_1000DPS},
+    [GyroFullScale500DPS] = {500.f, ICM42688_GFS_500DPS},
+    [GyroFullScale250DPS] = {250.f, ICM42688_GFS_250DPS},
+    [GyroFullScale125DPS] = {125.f, ICM42688_GFS_125DPS},
+    [GyroFullScale62_5DPS] = {62.5f, ICM42688_GFS_62_5DPS},
+    [GyroFullScale31_25DPS] = {31.25f, ICM42688_GFS_31_25DPS},
+    [GyroFullScale15_625DPS] = {15.625f, ICM42688_GFS_15_625DPS},
+};
+
+static bool icm42688p_write_reg(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t value) {
+    bool res = false;
+    furi_hal_spi_acquire(spi_bus);
+    do {
+        uint8_t cmd_data[2] = {addr & 0x7F, value};
+        if(!furi_hal_spi_bus_tx(spi_bus, cmd_data, 2, ICM42688P_TIMEOUT)) break;
+        res = true;
+    } while(0);
+    furi_hal_spi_release(spi_bus);
+    return res;
+}
+
+static bool icm42688p_read_reg(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t* value) {
+    bool res = false;
+    furi_hal_spi_acquire(spi_bus);
+    do {
+        uint8_t cmd_byte = addr | (1 << 7);
+        if(!furi_hal_spi_bus_tx(spi_bus, &cmd_byte, 1, ICM42688P_TIMEOUT)) break;
+        if(!furi_hal_spi_bus_rx(spi_bus, value, 1, ICM42688P_TIMEOUT)) break;
+        res = true;
+    } while(0);
+    furi_hal_spi_release(spi_bus);
+    return res;
+}
+
+static bool
+    icm42688p_read_mem(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t* data, uint8_t len) {
+    bool res = false;
+    furi_hal_spi_acquire(spi_bus);
+    do {
+        uint8_t cmd_byte = addr | (1 << 7);
+        if(!furi_hal_spi_bus_tx(spi_bus, &cmd_byte, 1, ICM42688P_TIMEOUT)) break;
+        if(!furi_hal_spi_bus_rx(spi_bus, data, len, ICM42688P_TIMEOUT)) break;
+        res = true;
+    } while(0);
+    furi_hal_spi_release(spi_bus);
+    return res;
+}
+
+bool icm42688p_accel_config(
+    ICM42688P* icm42688p,
+    ICM42688PAccelFullScale full_scale,
+    ICM42688PDataRate rate) {
+    icm42688p->accel_scale = accel_fs_modes[full_scale].value;
+    uint8_t reg_value = accel_fs_modes[full_scale].reg_mask | rate;
+    return icm42688p_write_reg(icm42688p->spi_bus, ICM42688_ACCEL_CONFIG0, reg_value);
+}
+
+float icm42688p_accel_get_full_scale(ICM42688P* icm42688p) {
+    return icm42688p->accel_scale;
+}
+
+bool icm42688p_gyro_config(
+    ICM42688P* icm42688p,
+    ICM42688PGyroFullScale full_scale,
+    ICM42688PDataRate rate) {
+    icm42688p->gyro_scale = gyro_fs_modes[full_scale].value;
+    uint8_t reg_value = gyro_fs_modes[full_scale].reg_mask | rate;
+    return icm42688p_write_reg(icm42688p->spi_bus, ICM42688_GYRO_CONFIG0, reg_value);
+}
+
+float icm42688p_gyro_get_full_scale(ICM42688P* icm42688p) {
+    return icm42688p->gyro_scale;
+}
+
+bool icm42688p_read_accel_raw(ICM42688P* icm42688p, ICM42688PRawData* data) {
+    bool ret = icm42688p_read_mem(
+        icm42688p->spi_bus, ICM42688_ACCEL_DATA_X1, (uint8_t*)data, sizeof(ICM42688PRawData));
+    return ret;
+}
+
+bool icm42688p_read_gyro_raw(ICM42688P* icm42688p, ICM42688PRawData* data) {
+    bool ret = icm42688p_read_mem(
+        icm42688p->spi_bus, ICM42688_GYRO_DATA_X1, (uint8_t*)data, sizeof(ICM42688PRawData));
+    return ret;
+}
+
+bool icm42688p_write_gyro_offset(ICM42688P* icm42688p, ICM42688PScaledData* scaled_data) {
+    if((fabsf(scaled_data->x) > 64.f) || (fabsf(scaled_data->y) > 64.f) ||
+       (fabsf(scaled_data->z) > 64.f)) {
+        return false;
+    }
+
+    uint16_t offset_x = (uint16_t)(-(int16_t)(scaled_data->x * 32.f) * 16) >> 4;
+    uint16_t offset_y = (uint16_t)(-(int16_t)(scaled_data->y * 32.f) * 16) >> 4;
+    uint16_t offset_z = (uint16_t)(-(int16_t)(scaled_data->z * 32.f) * 16) >> 4;
+
+    uint8_t offset_regs[9];
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 4);
+    icm42688p_read_mem(icm42688p->spi_bus, ICM42688_OFFSET_USER0, offset_regs, 5);
+
+    offset_regs[0] = offset_x & 0xFF;
+    offset_regs[1] = (offset_x & 0xF00) >> 8;
+    offset_regs[1] |= (offset_y & 0xF00) >> 4;
+    offset_regs[2] = offset_y & 0xFF;
+    offset_regs[3] = offset_z & 0xFF;
+    offset_regs[4] &= 0xF0;
+    offset_regs[4] |= (offset_z & 0x0F00) >> 8;
+
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER0, offset_regs[0]);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER1, offset_regs[1]);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER2, offset_regs[2]);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER3, offset_regs[3]);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER4, offset_regs[4]);
+
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0);
+    return true;
+}
+
+void icm42688p_apply_scale(ICM42688PRawData* raw_data, float full_scale, ICM42688PScaledData* data) {
+    data->x = ((float)(raw_data->x)) / 32768.f * full_scale;
+    data->y = ((float)(raw_data->y)) / 32768.f * full_scale;
+    data->z = ((float)(raw_data->z)) / 32768.f * full_scale;
+}
+
+void icm42688p_apply_scale_fifo(
+    ICM42688P* icm42688p,
+    ICM42688PFifoPacket* fifo_data,
+    ICM42688PScaledData* accel_data,
+    ICM42688PScaledData* gyro_data) {
+    float full_scale = icm42688p->accel_scale;
+    accel_data->x = ((float)(fifo_data->a_x)) / 32768.f * full_scale;
+    accel_data->y = ((float)(fifo_data->a_y)) / 32768.f * full_scale;
+    accel_data->z = ((float)(fifo_data->a_z)) / 32768.f * full_scale;
+
+    full_scale = icm42688p->gyro_scale;
+    gyro_data->x = ((float)(fifo_data->g_x)) / 32768.f * full_scale;
+    gyro_data->y = ((float)(fifo_data->g_y)) / 32768.f * full_scale;
+    gyro_data->z = ((float)(fifo_data->g_z)) / 32768.f * full_scale;
+}
+
+float icm42688p_read_temp(ICM42688P* icm42688p) {
+    uint8_t reg_val[2];
+
+    icm42688p_read_mem(icm42688p->spi_bus, ICM42688_TEMP_DATA1, reg_val, 2);
+    int16_t temp_int = (reg_val[0] << 8) | reg_val[1];
+    return ((float)temp_int / 132.48f) + 25.f;
+}
+
+void icm42688_fifo_enable(
+    ICM42688P* icm42688p,
+    ICM42688PIrqCallback irq_callback,
+    void* irq_context) {
+    // FIFO mode: stream
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG, (1 << 6));
+    // Little-endian data, FIFO count in records
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INTF_CONFIG0, (1 << 7) | (1 << 6));
+    // FIFO partial read, FIFO packet: gyro + accel TODO: 20bit
+    icm42688p_write_reg(
+        icm42688p->spi_bus, ICM42688_FIFO_CONFIG1, (1 << 6) | (1 << 5) | (1 << 1) | (1 << 0));
+    // FIFO irq watermark
+    uint16_t fifo_watermark = 1;
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG2, fifo_watermark & 0xFF);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG3, fifo_watermark >> 8);
+
+    // IRQ1: push-pull, active high
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG, (1 << 1) | (1 << 0));
+    // Clear IRQ on status read
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG0, 0);
+    // IRQ pulse duration
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG1, (1 << 6) | (1 << 5));
+
+    uint8_t reg_data = 0;
+    icm42688p_read_reg(icm42688p->spi_bus, ICM42688_INT_STATUS, &reg_data);
+
+    furi_hal_gpio_init(icm42688p->irq_pin, GpioModeInterruptRise, GpioPullDown, GpioSpeedVeryHigh);
+    furi_hal_gpio_remove_int_callback(icm42688p->irq_pin);
+    furi_hal_gpio_add_int_callback(icm42688p->irq_pin, irq_callback, irq_context);
+
+    // IRQ1 source: FIFO threshold
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, (1 << 2));
+}
+
+void icm42688_fifo_disable(ICM42688P* icm42688p) {
+    furi_hal_gpio_remove_int_callback(icm42688p->irq_pin);
+    furi_hal_gpio_init(icm42688p->irq_pin, GpioModeAnalog, GpioPullNo, GpioSpeedLow);
+
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, 0);
+
+    // FIFO mode: bypass
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG, 0);
+}
+
+uint16_t icm42688_fifo_get_count(ICM42688P* icm42688p) {
+    uint16_t reg_val = 0;
+    icm42688p_read_mem(icm42688p->spi_bus, ICM42688_FIFO_COUNTH, (uint8_t*)&reg_val, 2);
+    return reg_val;
+}
+
+bool icm42688_fifo_read(ICM42688P* icm42688p, ICM42688PFifoPacket* data) {
+    icm42688p_read_mem(
+        icm42688p->spi_bus, ICM42688_FIFO_DATA, (uint8_t*)data, sizeof(ICM42688PFifoPacket));
+    return (data->header) & (1 << 7);
+}
+
+ICM42688P* icm42688p_alloc(FuriHalSpiBusHandle* spi_bus, const GpioPin* irq_pin) {
+    ICM42688P* icm42688p = malloc(sizeof(ICM42688P));
+    icm42688p->spi_bus = spi_bus;
+    icm42688p->irq_pin = irq_pin;
+    return icm42688p;
+}
+
+void icm42688p_free(ICM42688P* icm42688p) {
+    free(icm42688p);
+}
+
+bool icm42688p_init(ICM42688P* icm42688p) {
+    furi_hal_spi_bus_handle_init(icm42688p->spi_bus);
+
+    // Software reset
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); // Set reg bank to 0
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_DEVICE_CONFIG, 0x01); // SPI Mode 0, SW reset
+    furi_delay_ms(1);
+
+    uint8_t reg_value = 0;
+    bool read_ok = icm42688p_read_reg(icm42688p->spi_bus, ICM42688_WHO_AM_I, &reg_value);
+    if(!read_ok) {
+        FURI_LOG_E(TAG, "Chip ID read failed");
+        return false;
+    } else if(reg_value != ICM42688_WHOAMI) {
+        FURI_LOG_E(
+            TAG, "Sensor returned wrong ID 0x%02X, expected 0x%02X", reg_value, ICM42688_WHOAMI);
+        return false;
+    }
+
+    // Disable all interrupts
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, 0);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE1, 0);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE3, 0);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE4, 0);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 4);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE6, 0);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE7, 0);
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0);
+
+    // Data format: little endian
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INTF_CONFIG0, 0);
+
+    // Enable all sensors
+    icm42688p_write_reg(
+        icm42688p->spi_bus,
+        ICM42688_PWR_MGMT0,
+        ICM42688_PWR_TEMP_ON | ICM42688_PWR_GYRO_MODE_LN | ICM42688_PWR_ACCEL_MODE_LN);
+    furi_delay_ms(45);
+
+    icm42688p_accel_config(icm42688p, AccelFullScale16G, DataRate1kHz);
+    icm42688p_gyro_config(icm42688p, GyroFullScale2000DPS, DataRate1kHz);
+
+    return true;
+}
+
+bool icm42688p_deinit(ICM42688P* icm42688p) {
+    // Software reset
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); // Set reg bank to 0
+    icm42688p_write_reg(icm42688p->spi_bus, ICM42688_DEVICE_CONFIG, 0x01); // SPI Mode 0, SW reset
+
+    furi_hal_spi_bus_handle_deinit(icm42688p->spi_bus);
+    return true;
+}

sensors/ICM42688P/ICM42688P.h

@@ -0,0 +1,127 @@
+#pragma once
+
+#include <furi.h>
+#include <furi_hal.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+    DataRate32kHz = 0x01,
+    DataRate16kHz = 0x02,
+    DataRate8kHz = 0x03,
+    DataRate4kHz = 0x04,
+    DataRate2kHz = 0x05,
+    DataRate1kHz = 0x06,
+    DataRate200Hz = 0x07,
+    DataRate100Hz = 0x08,
+    DataRate50Hz = 0x09,
+    DataRate25Hz = 0x0A,
+    DataRate12_5Hz = 0x0B,
+    DataRate6_25Hz = 0x0C, // Accelerometer only
+    DataRate3_125Hz = 0x0D, // Accelerometer only
+    DataRate1_5625Hz = 0x0E, // Accelerometer only
+    DataRate500Hz = 0x0F,
+} ICM42688PDataRate;
+
+typedef enum {
+    AccelFullScale16G = 0,
+    AccelFullScale8G,
+    AccelFullScale4G,
+    AccelFullScale2G,
+    AccelFullScaleTotal,
+} ICM42688PAccelFullScale;
+
+typedef enum {
+    GyroFullScale2000DPS = 0,
+    GyroFullScale1000DPS,
+    GyroFullScale500DPS,
+    GyroFullScale250DPS,
+    GyroFullScale125DPS,
+    GyroFullScale62_5DPS,
+    GyroFullScale31_25DPS,
+    GyroFullScale15_625DPS,
+    GyroFullScaleTotal,
+} ICM42688PGyroFullScale;
+
+typedef struct {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+} __attribute__((packed)) ICM42688PRawData;
+
+typedef struct {
+    uint8_t header;
+    int16_t a_x;
+    int16_t a_y;
+    int16_t a_z;
+    int16_t g_x;
+    int16_t g_y;
+    int16_t g_z;
+    uint8_t temp;
+    uint16_t ts;
+} __attribute__((packed)) ICM42688PFifoPacket;
+
+typedef struct {
+    float x;
+    float y;
+    float z;
+} ICM42688PScaledData;
+
+typedef struct ICM42688P ICM42688P;
+
+typedef void (*ICM42688PIrqCallback)(void* ctx);
+
+ICM42688P* icm42688p_alloc(FuriHalSpiBusHandle* spi_bus, const GpioPin* irq_pin);
+
+bool icm42688p_init(ICM42688P* icm42688p);
+
+bool icm42688p_deinit(ICM42688P* icm42688p);
+
+void icm42688p_free(ICM42688P* icm42688p);
+
+bool icm42688p_accel_config(
+    ICM42688P* icm42688p,
+    ICM42688PAccelFullScale full_scale,
+    ICM42688PDataRate rate);
+
+float icm42688p_accel_get_full_scale(ICM42688P* icm42688p);
+
+bool icm42688p_gyro_config(
+    ICM42688P* icm42688p,
+    ICM42688PGyroFullScale full_scale,
+    ICM42688PDataRate rate);
+
+float icm42688p_gyro_get_full_scale(ICM42688P* icm42688p);
+
+bool icm42688p_read_accel_raw(ICM42688P* icm42688p, ICM42688PRawData* data);
+
+bool icm42688p_read_gyro_raw(ICM42688P* icm42688p, ICM42688PRawData* data);
+
+bool icm42688p_write_gyro_offset(ICM42688P* icm42688p, ICM42688PScaledData* scaled_data);
+
+void icm42688p_apply_scale(ICM42688PRawData* raw_data, float full_scale, ICM42688PScaledData* data);
+
+void icm42688p_apply_scale_fifo(
+    ICM42688P* icm42688p,
+    ICM42688PFifoPacket* fifo_data,
+    ICM42688PScaledData* accel_data,
+    ICM42688PScaledData* gyro_data);
+
+float icm42688p_read_temp(ICM42688P* icm42688p);
+
+void icm42688_fifo_enable(
+    ICM42688P* icm42688p,
+    ICM42688PIrqCallback irq_callback,
+    void* irq_context);
+
+void icm42688_fifo_disable(ICM42688P* icm42688p);
+
+uint16_t icm42688_fifo_get_count(ICM42688P* icm42688p);
+
+bool icm42688_fifo_read(ICM42688P* icm42688p, ICM42688PFifoPacket* data);
+
+#ifdef __cplusplus
+}
+#endif

sensors/ICM42688P/ICM42688P_regs.h

@@ -0,0 +1,176 @@
+#pragma once
+
+#define ICM42688_WHOAMI 0x47
+
+// Bank 0
+#define ICM42688_DEVICE_CONFIG 0x11
+#define ICM42688_DRIVE_CONFIG 0x13
+#define ICM42688_INT_CONFIG 0x14
+#define ICM42688_FIFO_CONFIG 0x16
+#define ICM42688_TEMP_DATA1 0x1D
+#define ICM42688_TEMP_DATA0 0x1E
+#define ICM42688_ACCEL_DATA_X1 0x1F
+#define ICM42688_ACCEL_DATA_X0 0x20
+#define ICM42688_ACCEL_DATA_Y1 0x21
+#define ICM42688_ACCEL_DATA_Y0 0x22
+#define ICM42688_ACCEL_DATA_Z1 0x23
+#define ICM42688_ACCEL_DATA_Z0 0x24
+#define ICM42688_GYRO_DATA_X1 0x25
+#define ICM42688_GYRO_DATA_X0 0x26
+#define ICM42688_GYRO_DATA_Y1 0x27
+#define ICM42688_GYRO_DATA_Y0 0x28
+#define ICM42688_GYRO_DATA_Z1 0x29
+#define ICM42688_GYRO_DATA_Z0 0x2A
+#define ICM42688_TMST_FSYNCH 0x2B
+#define ICM42688_TMST_FSYNCL 0x2C
+#define ICM42688_INT_STATUS 0x2D
+#define ICM42688_FIFO_COUNTH 0x2E
+#define ICM42688_FIFO_COUNTL 0x2F
+#define ICM42688_FIFO_DATA 0x30
+#define ICM42688_APEX_DATA0 0x31
+#define ICM42688_APEX_DATA1 0x32
+#define ICM42688_APEX_DATA2 0x33
+#define ICM42688_APEX_DATA3 0x34
+#define ICM42688_APEX_DATA4 0x35
+#define ICM42688_APEX_DATA5 0x36
+#define ICM42688_INT_STATUS2 0x37
+#define ICM42688_INT_STATUS3 0x38
+#define ICM42688_SIGNAL_PATH_RESET 0x4B
+#define ICM42688_INTF_CONFIG0 0x4C
+#define ICM42688_INTF_CONFIG1 0x4D
+#define ICM42688_PWR_MGMT0 0x4E
+#define ICM42688_GYRO_CONFIG0 0x4F
+#define ICM42688_ACCEL_CONFIG0 0x50
+#define ICM42688_GYRO_CONFIG1 0x51
+#define ICM42688_GYRO_ACCEL_CONFIG0 0x52
+#define ICM42688_ACCEL_CONFIG1 0x53
+#define ICM42688_TMST_CONFIG 0x54
+#define ICM42688_APEX_CONFIG0 0x56
+#define ICM42688_SMD_CONFIG 0x57
+#define ICM42688_FIFO_CONFIG1 0x5F
+#define ICM42688_FIFO_CONFIG2 0x60
+#define ICM42688_FIFO_CONFIG3 0x61
+#define ICM42688_FSYNC_CONFIG 0x62
+#define ICM42688_INT_CONFIG0 0x63
+#define ICM42688_INT_CONFIG1 0x64
+#define ICM42688_INT_SOURCE0 0x65
+#define ICM42688_INT_SOURCE1 0x66
+#define ICM42688_INT_SOURCE3 0x68
+#define ICM42688_INT_SOURCE4 0x69
+#define ICM42688_FIFO_LOST_PKT0 0x6C
+#define ICM42688_FIFO_LOST_PKT1 0x6D
+#define ICM42688_SELF_TEST_CONFIG 0x70
+#define ICM42688_WHO_AM_I 0x75
+#define ICM42688_REG_BANK_SEL 0x76
+
+// Bank 1
+#define ICM42688_SENSOR_CONFIG0 0x03
+#define ICM42688_GYRO_CONFIG_STATIC2 0x0B
+#define ICM42688_GYRO_CONFIG_STATIC3 0x0C
+#define ICM42688_GYRO_CONFIG_STATIC4 0x0D
+#define ICM42688_GYRO_CONFIG_STATIC5 0x0E
+#define ICM42688_GYRO_CONFIG_STATIC6 0x0F
+#define ICM42688_GYRO_CONFIG_STATIC7 0x10
+#define ICM42688_GYRO_CONFIG_STATIC8 0x11
+#define ICM42688_GYRO_CONFIG_STATIC9 0x12
+#define ICM42688_GYRO_CONFIG_STATIC10 0x13
+#define ICM42688_XG_ST_DATA 0x5F
+#define ICM42688_YG_ST_DATA 0x60
+#define ICM42688_ZG_ST_DATA 0x61
+#define ICM42688_TMSTVAL0 0x62
+#define ICM42688_TMSTVAL1 0x63
+#define ICM42688_TMSTVAL2 0x64
+#define ICM42688_INTF_CONFIG4 0x7A
+#define ICM42688_INTF_CONFIG5 0x7B
+#define ICM42688_INTF_CONFIG6 0x7C
+
+// Bank 2
+#define ICM42688_ACCEL_CONFIG_STATIC2 0x03
+#define ICM42688_ACCEL_CONFIG_STATIC3 0x04
+#define ICM42688_ACCEL_CONFIG_STATIC4 0x05
+#define ICM42688_XA_ST_DATA 0x3B
+#define ICM42688_YA_ST_DATA 0x3C
+#define ICM42688_ZA_ST_DATA 0x3D
+
+// Bank 4
+#define ICM42688_APEX_CONFIG1 0x40
+#define ICM42688_APEX_CONFIG2 0x41
+#define ICM42688_APEX_CONFIG3 0x42
+#define ICM42688_APEX_CONFIG4 0x43
+#define ICM42688_APEX_CONFIG5 0x44
+#define ICM42688_APEX_CONFIG6 0x45
+#define ICM42688_APEX_CONFIG7 0x46
+#define ICM42688_APEX_CONFIG8 0x47
+#define ICM42688_APEX_CONFIG9 0x48
+#define ICM42688_ACCEL_WOM_X_THR 0x4A
+#define ICM42688_ACCEL_WOM_Y_THR 0x4B
+#define ICM42688_ACCEL_WOM_Z_THR 0x4C
+#define ICM42688_INT_SOURCE6 0x4D
+#define ICM42688_INT_SOURCE7 0x4E
+#define ICM42688_INT_SOURCE8 0x4F
+#define ICM42688_INT_SOURCE9 0x50
+#define ICM42688_INT_SOURCE10 0x51
+#define ICM42688_OFFSET_USER0 0x77
+#define ICM42688_OFFSET_USER1 0x78
+#define ICM42688_OFFSET_USER2 0x79
+#define ICM42688_OFFSET_USER3 0x7A
+#define ICM42688_OFFSET_USER4 0x7B
+#define ICM42688_OFFSET_USER5 0x7C
+#define ICM42688_OFFSET_USER6 0x7D
+#define ICM42688_OFFSET_USER7 0x7E
+#define ICM42688_OFFSET_USER8 0x7F
+
+// PWR_MGMT0
+#define ICM42688_PWR_TEMP_ON (0 << 5)
+#define ICM42688_PWR_TEMP_OFF (1 << 5)
+#define ICM42688_PWR_IDLE (1 << 4)
+#define ICM42688_PWR_GYRO_MODE_OFF (0 << 2)
+#define ICM42688_PWR_GYRO_MODE_LN (3 << 2)
+#define ICM42688_PWR_ACCEL_MODE_OFF (0 << 0)
+#define ICM42688_PWR_ACCEL_MODE_LP (2 << 0)
+#define ICM42688_PWR_ACCEL_MODE_LN (3 << 0)
+
+// GYRO_CONFIG0
+#define ICM42688_GFS_2000DPS (0x00 << 5)
+#define ICM42688_GFS_1000DPS (0x01 << 5)
+#define ICM42688_GFS_500DPS (0x02 << 5)
+#define ICM42688_GFS_250DPS (0x03 << 5)
+#define ICM42688_GFS_125DPS (0x04 << 5)
+#define ICM42688_GFS_62_5DPS (0x05 << 5)
+#define ICM42688_GFS_31_25DPS (0x06 << 5)
+#define ICM42688_GFS_15_625DPS (0x07 << 5)
+
+#define ICM42688_GODR_32kHz 0x01
+#define ICM42688_GODR_16kHz 0x02
+#define ICM42688_GODR_8kHz 0x03
+#define ICM42688_GODR_4kHz 0x04
+#define ICM42688_GODR_2kHz 0x05
+#define ICM42688_GODR_1kHz 0x06
+#define ICM42688_GODR_200Hz 0x07
+#define ICM42688_GODR_100Hz 0x08
+#define ICM42688_GODR_50Hz 0x09
+#define ICM42688_GODR_25Hz 0x0A
+#define ICM42688_GODR_12_5Hz 0x0B
+#define ICM42688_GODR_500Hz 0x0F
+
+// ACCEL_CONFIG0
+#define ICM42688_AFS_16G (0x00 << 5)
+#define ICM42688_AFS_8G (0x01 << 5)
+#define ICM42688_AFS_4G (0x02 << 5)
+#define ICM42688_AFS_2G (0x03 << 5)
+
+#define ICM42688_AODR_32kHz 0x01
+#define ICM42688_AODR_16kHz 0x02
+#define ICM42688_AODR_8kHz 0x03
+#define ICM42688_AODR_4kHz 0x04
+#define ICM42688_AODR_2kHz 0x05
+#define ICM42688_AODR_1kHz 0x06
+#define ICM42688_AODR_200Hz 0x07
+#define ICM42688_AODR_100Hz 0x08
+#define ICM42688_AODR_50Hz 0x09
+#define ICM42688_AODR_25Hz 0x0A
+#define ICM42688_AODR_12_5Hz 0x0B
+#define ICM42688_AODR_6_25Hz 0x0C
+#define ICM42688_AODR_3_125Hz 0x0D
+#define ICM42688_AODR_1_5625Hz 0x0E
+#define ICM42688_AODR_500Hz 0x0F

sensors/imu.c

@@ -0,0 +1,326 @@
+#include <furi.h>
+#include "imu.h"
+#include "ICM42688P/ICM42688P.h"
+
+#define TAG "IMU"
+
+#define ACCEL_GYRO_RATE DataRate100Hz
+
+#define FILTER_SAMPLE_FREQ 100.f
+#define FILTER_BETA 0.08f
+
+#define SAMPLE_RATE_DIV 5
+
+#define SENSITIVITY_K 30.f
+#define EXP_RATE 1.1f
+
+#define IMU_CALI_AVG 64
+
+typedef enum {
+    ImuStop = (1 << 0),
+    ImuNewData = (1 << 1),
+} ImuThreadFlags;
+
+#define FLAGS_ALL (ImuStop | ImuNewData)
+
+typedef struct {
+    float q0;
+    float q1;
+    float q2;
+    float q3;
+    float roll;
+    float pitch;
+    float yaw;
+} ImuProcessedData;
+
+typedef struct {
+    FuriThread* thread;
+    ICM42688P* icm42688p;
+    ImuProcessedData processed_data;
+} ImuThread;
+
+static void imu_madgwick_filter(
+    ImuProcessedData* out,
+    ICM42688PScaledData* accel,
+    ICM42688PScaledData* gyro);
+
+static void imu_irq_callback(void* context) {
+    furi_assert(context);
+    ImuThread* imu = context;
+    furi_thread_flags_set(furi_thread_get_id(imu->thread), ImuNewData);
+}
+
+static void imu_process_data(ImuThread* imu, ICM42688PFifoPacket* in_data) {
+    ICM42688PScaledData accel_data;
+    ICM42688PScaledData gyro_data;
+
+    // Get accel and gyro data in g and degrees/s
+    icm42688p_apply_scale_fifo(imu->icm42688p, in_data, &accel_data, &gyro_data);
+
+    // Gyro: degrees/s to rads/s
+    gyro_data.x = gyro_data.x / 180.f * M_PI;
+    gyro_data.y = gyro_data.y / 180.f * M_PI;
+    gyro_data.z = gyro_data.z / 180.f * M_PI;
+
+    // Sensor Fusion algorithm
+    ImuProcessedData* out = &imu->processed_data;
+    imu_madgwick_filter(out, &accel_data, &gyro_data);
+
+    // Quaternion to euler angles
+    float roll = atan2f(
+        out->q0 * out->q1 + out->q2 * out->q3, 0.5f - out->q1 * out->q1 - out->q2 * out->q2);
+    float pitch = asinf(-2.0f * (out->q1 * out->q3 - out->q0 * out->q2));
+    float yaw = atan2f(
+        out->q1 * out->q2 + out->q0 * out->q3, 0.5f - out->q2 * out->q2 - out->q3 * out->q3);
+
+    // Euler angles: rads to degrees
+    out->roll = roll / M_PI * 180.f;
+    out->pitch = pitch / M_PI * 180.f;
+    out->yaw = yaw / M_PI * 180.f;
+}
+
+static void calibrate_gyro(ImuThread* imu) {
+    ICM42688PRawData data;
+    ICM42688PScaledData offset_scaled = {.x = 0.f, .y = 0.f, .z = 0.f};
+
+    icm42688p_write_gyro_offset(imu->icm42688p, &offset_scaled);
+    furi_delay_ms(10);
+
+    int32_t avg_x = 0;
+    int32_t avg_y = 0;
+    int32_t avg_z = 0;
+
+    for(uint8_t i = 0; i < IMU_CALI_AVG; i++) {
+        icm42688p_read_gyro_raw(imu->icm42688p, &data);
+        avg_x += data.x;
+        avg_y += data.y;
+        avg_z += data.z;
+        furi_delay_ms(2);
+    }
+
+    data.x = avg_x / IMU_CALI_AVG;
+    data.y = avg_y / IMU_CALI_AVG;
+    data.z = avg_z / IMU_CALI_AVG;
+
+    icm42688p_apply_scale(&data, icm42688p_gyro_get_full_scale(imu->icm42688p), &offset_scaled);
+    FURI_LOG_I(
+        TAG,
+        "Offsets: x %f, y %f, z %f",
+        (double)offset_scaled.x,
+        (double)offset_scaled.y,
+        (double)offset_scaled.z);
+    icm42688p_write_gyro_offset(imu->icm42688p, &offset_scaled);
+}
+
+// static float imu_angle_diff(float a, float b) {
+//     float diff = a - b;
+//     if(diff > 180.f)
+//         diff -= 360.f;
+//     else if(diff < -180.f)
+//         diff += 360.f;
+
+//     return diff;
+// }
+
+static int32_t imu_thread(void* context) {
+    furi_assert(context);
+    ImuThread* imu = context;
+
+    // float yaw_last = 0.f;
+    // float pitch_last = 0.f;
+    // float diff_x = 0.f;
+    // float diff_y = 0.f;
+
+    calibrate_gyro(imu);
+
+    icm42688p_accel_config(imu->icm42688p, AccelFullScale16G, ACCEL_GYRO_RATE);
+    icm42688p_gyro_config(imu->icm42688p, GyroFullScale2000DPS, ACCEL_GYRO_RATE);
+
+    imu->processed_data.q0 = 1.f;
+    imu->processed_data.q1 = 0.f;
+    imu->processed_data.q2 = 0.f;
+    imu->processed_data.q3 = 0.f;
+    icm42688_fifo_enable(imu->icm42688p, imu_irq_callback, imu);
+
+    while(1) {
+        uint32_t events = furi_thread_flags_wait(FLAGS_ALL, FuriFlagWaitAny, FuriWaitForever);
+
+        if(events & ImuStop) {
+            break;
+        }
+
+        if(events & ImuNewData) {
+            uint16_t data_pending = icm42688_fifo_get_count(imu->icm42688p);
+            ICM42688PFifoPacket data;
+            while(data_pending--) {
+                icm42688_fifo_read(imu->icm42688p, &data);
+                imu_process_data(imu, &data);
+            }
+        }
+    }
+
+    icm42688_fifo_disable(imu->icm42688p);
+
+    return 0;
+}
+
+ImuThread* imu_start(ICM42688P* icm42688p) {
+    ImuThread* imu = malloc(sizeof(ImuThread));
+    imu->icm42688p = icm42688p;
+    imu->thread = furi_thread_alloc_ex("ImuThread", 4096, imu_thread, imu);
+
+    furi_thread_start(imu->thread);
+
+    return imu;
+}
+
+void imu_stop(ImuThread* imu) {
+    furi_assert(imu);
+
+    furi_thread_flags_set(furi_thread_get_id(imu->thread), ImuStop);
+
+    furi_thread_join(imu->thread);
+    furi_thread_free(imu->thread);
+
+    free(imu);
+}
+
+static float imu_inv_sqrt(float number) {
+    union {
+        float f;
+        uint32_t i;
+    } conv = {.f = number};
+    conv.i = 0x5F3759Df - (conv.i >> 1);
+    conv.f *= 1.5f - (number * 0.5f * conv.f * conv.f);
+    return conv.f;
+}
+
+/* Simple madgwik filter, based on: https://github.com/arduino-libraries/MadgwickAHRS/ */
+
+static void imu_madgwick_filter(
+    ImuProcessedData* out,
+    ICM42688PScaledData* accel,
+    ICM42688PScaledData* gyro) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-out->q1 * gyro->x - out->q2 * gyro->y - out->q3 * gyro->z);
+    qDot2 = 0.5f * (out->q0 * gyro->x + out->q2 * gyro->z - out->q3 * gyro->y);
+    qDot3 = 0.5f * (out->q0 * gyro->y - out->q1 * gyro->z + out->q3 * gyro->x);
+    qDot4 = 0.5f * (out->q0 * gyro->z + out->q1 * gyro->y - out->q2 * gyro->x);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((accel->x == 0.0f) && (accel->y == 0.0f) && (accel->z == 0.0f))) {
+        // Normalise accelerometer measurement
+        recipNorm = imu_inv_sqrt(accel->x * accel->x + accel->y * accel->y + accel->z * accel->z);
+        accel->x *= recipNorm;
+        accel->y *= recipNorm;
+        accel->z *= recipNorm;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0 = 2.0f * out->q0;
+        _2q1 = 2.0f * out->q1;
+        _2q2 = 2.0f * out->q2;
+        _2q3 = 2.0f * out->q3;
+        _4q0 = 4.0f * out->q0;
+        _4q1 = 4.0f * out->q1;
+        _4q2 = 4.0f * out->q2;
+        _8q1 = 8.0f * out->q1;
+        _8q2 = 8.0f * out->q2;
+        q0q0 = out->q0 * out->q0;
+        q1q1 = out->q1 * out->q1;
+        q2q2 = out->q2 * out->q2;
+        q3q3 = out->q3 * out->q3;
+
+        // Gradient decent algorithm corrective step
+        s0 = _4q0 * q2q2 + _2q2 * accel->x + _4q0 * q1q1 - _2q1 * accel->y;
+        s1 = _4q1 * q3q3 - _2q3 * accel->x + 4.0f * q0q0 * out->q1 - _2q0 * accel->y - _4q1 +
+             _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * accel->z;
+        s2 = 4.0f * q0q0 * out->q2 + _2q0 * accel->x + _4q2 * q3q3 - _2q3 * accel->y - _4q2 +
+             _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * accel->z;
+        s3 = 4.0f * q1q1 * out->q3 - _2q1 * accel->x + 4.0f * q2q2 * out->q3 - _2q2 * accel->y;
+        recipNorm =
+            imu_inv_sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= FILTER_BETA * s0;
+        qDot2 -= FILTER_BETA * s1;
+        qDot3 -= FILTER_BETA * s2;
+        qDot4 -= FILTER_BETA * s3;
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    out->q0 += qDot1 * (1.0f / FILTER_SAMPLE_FREQ);
+    out->q1 += qDot2 * (1.0f / FILTER_SAMPLE_FREQ);
+    out->q2 += qDot3 * (1.0f / FILTER_SAMPLE_FREQ);
+    out->q3 += qDot4 * (1.0f / FILTER_SAMPLE_FREQ);
+
+    // Normalise quaternion
+    recipNorm = imu_inv_sqrt(
+        out->q0 * out->q0 + out->q1 * out->q1 + out->q2 * out->q2 + out->q3 * out->q3);
+    out->q0 *= recipNorm;
+    out->q1 *= recipNorm;
+    out->q2 *= recipNorm;
+    out->q3 *= recipNorm;
+}
+
+/* IMU API */
+
+struct Imu {
+    FuriHalSpiBusHandle* icm42688p_device;
+    ICM42688P* icm42688p;
+    ImuThread* thread;
+    bool present;
+};
+
+Imu* imu_alloc(void) {
+    Imu* imu = malloc(sizeof(Imu));
+    imu->icm42688p_device = malloc(sizeof(FuriHalSpiBusHandle));
+    memcpy(imu->icm42688p_device, &furi_hal_spi_bus_handle_external, sizeof(FuriHalSpiBusHandle));
+    imu->icm42688p_device->cs = &gpio_ext_pc3;
+
+    imu->icm42688p = icm42688p_alloc(imu->icm42688p_device, &gpio_ext_pb2);
+    imu->present = icm42688p_init(imu->icm42688p);
+
+    if(imu->present) {
+        imu->thread = imu_start(imu->icm42688p);
+    }
+
+    return imu;
+}
+
+void imu_free(Imu* imu) {
+    if(imu->present) {
+        imu_stop(imu->thread);
+    }
+    icm42688p_deinit(imu->icm42688p);
+    icm42688p_free(imu->icm42688p);
+    free(imu->icm42688p_device);
+    free(imu);
+}
+
+bool imu_present(Imu* imu) {
+    return imu->present;
+}
+
+float imu_pitch_get(Imu* imu) {
+    // we pretend that reading a float is an atomic operation
+    return imu->thread->processed_data.pitch;
+}
+
+float imu_roll_get(Imu* imu) {
+    // we pretend that reading a float is an atomic operation
+    return imu->thread->processed_data.roll;
+}
+
+float imu_yaw_get(Imu* imu) {
+    // we pretend that reading a float is an atomic operation
+    return imu->thread->processed_data.yaw;
+}

sensors/imu.h

@@ -0,0 +1,15 @@
+#pragma once
+
+typedef struct Imu Imu;
+
+Imu* imu_alloc(void);
+
+void imu_free(Imu* imu);
+
+bool imu_present(Imu* imu);
+
+float imu_pitch_get(Imu* imu);
+
+float imu_roll_get(Imu* imu);
+
+float imu_yaw_get(Imu* imu);