From 21bf312d2b1d7cf6ae33482313859725770e5ba8 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 6 Aug 2026 09:34:12 -0500 Subject: [PATCH 1/2] Add ICM40609D IMU driver Wire in a gyro/accel driver for the TDK ICM40609D, modeled on the existing ICM42605 driver. Register map, WHO_AM_I, and scale factors verified directly against the ICM-40609-D datasheet (DS-000272 rev 0.8) rather than trusted from Betaflight's driver, which mislabels ACCEL_FS_SEL=0 as +/-16g when the datasheet specifies +/-32g, and originally reset via the wrong register (0x4C instead of 0x11, fixed upstream in betaflight/betaflight#14415). --- src/main/CMakeLists.txt | 2 + src/main/drivers/accgyro/accgyro_icm40609d.c | 238 +++++++++++++++++++ src/main/drivers/accgyro/accgyro_icm40609d.h | 21 ++ src/main/drivers/accgyro/accgyro_mpu.h | 1 + src/main/drivers/bus.h | 1 + src/main/fc/cli.c | 2 +- src/main/fc/settings.yaml | 2 +- src/main/sensors/acceleration.c | 13 + src/main/sensors/acceleration.h | 1 + src/main/sensors/gyro.c | 10 + src/main/sensors/gyro.h | 1 + 11 files changed, 290 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_icm40609d.c create mode 100644 src/main/drivers/accgyro/accgyro_icm40609d.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f4ab6479a5f..4a311fae17e 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -89,6 +89,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_icm42605.h drivers/accgyro/accgyro_icm45686.c drivers/accgyro/accgyro_icm45686.h + drivers/accgyro/accgyro_icm40609d.c + drivers/accgyro/accgyro_icm40609d.h drivers/accgyro/accgyro_mpu.c drivers/accgyro/accgyro_mpu.h drivers/accgyro/accgyro_mpu6000.c diff --git a/src/main/drivers/accgyro/accgyro_icm40609d.c b/src/main/drivers/accgyro/accgyro_icm40609d.c new file mode 100644 index 00000000000..cca9eca0202 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_icm40609d.c @@ -0,0 +1,238 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/accgyro/accgyro_icm40609d.h" + +#if defined(USE_IMU_ICM40609D) + +// Register addresses/values verified against TDK ICM-40609-D datasheet +// (DS-000272 rev 0.8), not solely against Betaflight PR #14367/#14415. + +#define ICM40609D_RA_DEVICE_CONFIG 0x11 +#define ICM40609D_DEVICE_CONFIG_SOFT_RESET (1 << 0) + +#define ICM40609D_RA_INT_CONFIG 0x14 +#define ICM40609D_INT1_MODE_PULSED (0 << 2) +#define ICM40609D_INT1_DRIVE_CIRCUIT_PP (1 << 1) +#define ICM40609D_INT1_POLARITY_ACTIVE_HIGH (1 << 0) + +#define ICM40609D_RA_PWR_MGMT0 0x4E +#define ICM40609D_PWR_MGMT0_ACCEL_MODE_LN (3 << 0) +#define ICM40609D_PWR_MGMT0_GYRO_MODE_LN (3 << 2) +#define ICM40609D_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5) + +#define ICM40609D_RA_GYRO_CONFIG0 0x4F +#define ICM40609D_RA_ACCEL_CONFIG0 0x50 +#define ICM40609D_RA_GYRO_ACCEL_CONFIG0 0x52 + +// GYRO_FS_SEL = 0 -> +/-2000dps, 16.4 LSB/(deg/s) +#define ICM40609D_GYRO_FS_SEL_2000DPS (0 << 5) +#define ICM40609D_GYRO_ODR_1KHZ 6 + +// ACCEL_FS_SEL = 1 -> +/-16g, 2048 LSB/g (FS_SEL=0 is +/-32g on this chip, +// unlike ICM42605 where 0 is the narrowest common range) +#define ICM40609D_ACCEL_FS_SEL_16G (1 << 5) +#define ICM40609D_ACCEL_ODR_1KHZ 6 + +// Low-latency UI filter bandwidth select, both fields set to the "trivial" +// low-latency option (verified numeric meaning against the same bit layout +// as ICM42605's GYRO_ACCEL_CONFIG0, confirmed by Betaflight PR #14367's +// identically-valued ICM40609_*_UI_FILT_BW_LP_TRIVIAL_200HZ_8XODR macros) +#define ICM40609D_ACCEL_UI_FILT_BW_LOW_LATENCY (15 << 4) +#define ICM40609D_GYRO_UI_FILT_BW_LOW_LATENCY (15 << 0) + +#define ICM40609D_RA_GYRO_DATA_X1 0x25 +#define ICM40609D_RA_ACCEL_DATA_X1 0x1F +#define ICM40609D_RA_TEMP_DATA1 0x1D + +#define ICM40609D_RA_INT_CONFIG0 0x63 +#define ICM40609D_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) | (0 << 4)) + +#define ICM40609D_RA_INT_SOURCE0 0x65 +#define ICM40609D_UI_DRDY_INT1_EN_ENABLED (1 << 3) + +static void icm40609dAccInit(accDev_t *acc) +{ + acc->acc_1G = 512 * 4; // 2048 LSB/g, matches ACCEL_FS_SEL=1 (+/-16g) +} + +static bool icm40609dAccRead(accDev_t *acc) +{ + uint8_t data[6]; + + const bool ack = busReadBuf(acc->busDev, ICM40609D_RA_ACCEL_DATA_X1, data, 6); + if (!ack) { + return false; + } + + acc->ADCRaw[X] = (float) int16_val_big_endian(data, 0); + acc->ADCRaw[Y] = (float) int16_val_big_endian(data, 1); + acc->ADCRaw[Z] = (float) int16_val_big_endian(data, 2); + + return true; +} + +bool icm40609dAccDetect(accDev_t *acc) +{ + acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM40609D, acc->imuSensorToUse); + if (acc->busDev == NULL) { + return false; + } + + mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev); + if (ctx->chipMagicNumber != 0x4609) { + return false; + } + + acc->initFn = icm40609dAccInit; + acc->readFn = icm40609dAccRead; + acc->accAlign = acc->busDev->param; + + return true; +} + +static void icm40609dAccAndGyroInit(gyroDev_t *gyro) +{ + busDevice_t * dev = gyro->busDev; + + gyro->sampleRateIntervalUs = 1000; // 1kHz ODR + + busSetSpeed(dev, BUS_SPEED_INITIALIZATION); + + busWrite(dev, ICM40609D_RA_PWR_MGMT0, ICM40609D_PWR_MGMT0_TEMP_DISABLE_OFF | ICM40609D_PWR_MGMT0_ACCEL_MODE_LN | ICM40609D_PWR_MGMT0_GYRO_MODE_LN); + delay(15); + + busWrite(dev, ICM40609D_RA_GYRO_CONFIG0, ICM40609D_GYRO_FS_SEL_2000DPS | ICM40609D_GYRO_ODR_1KHZ); + delay(15); + + busWrite(dev, ICM40609D_RA_ACCEL_CONFIG0, ICM40609D_ACCEL_FS_SEL_16G | ICM40609D_ACCEL_ODR_1KHZ); + delay(15); + + // Low latency, same convention as ICM42605 + busWrite(dev, ICM40609D_RA_GYRO_ACCEL_CONFIG0, ICM40609D_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM40609D_GYRO_UI_FILT_BW_LOW_LATENCY); + delay(15); + + busWrite(dev, ICM40609D_RA_INT_CONFIG, ICM40609D_INT1_MODE_PULSED | ICM40609D_INT1_DRIVE_CIRCUIT_PP | ICM40609D_INT1_POLARITY_ACTIVE_HIGH); + delay(15); + + busWrite(dev, ICM40609D_RA_INT_CONFIG0, ICM40609D_UI_DRDY_INT_CLEAR_ON_SBR); + delay(100); + + busWrite(dev, ICM40609D_RA_INT_SOURCE0, ICM40609D_UI_DRDY_INT1_EN_ENABLED); + delay(15); + + busSetSpeed(dev, BUS_SPEED_FAST); +} + +static bool icm40609dDeviceDetect(busDevice_t * dev) +{ + uint8_t tmp; + uint8_t attemptsRemaining = 5; + + busSetSpeed(dev, BUS_SPEED_INITIALIZATION); + + // DEVICE_CONFIG bit0 = SOFT_RESET_CONFIG. Betaflight PR #14367 originally + // wrote this reset bit to register 0x4C (INTF_CONFIG0) instead of the + // correct 0x11 (DEVICE_CONFIG) -- fixed upstream in PR #14415. Verified + // against the datasheet directly rather than trusting either revision. + busWrite(dev, ICM40609D_RA_DEVICE_CONFIG, ICM40609D_DEVICE_CONFIG_SOFT_RESET); + + do { + delay(150); + + busRead(dev, MPU_RA_WHO_AM_I, &tmp); + + if (tmp == ICM40609D_WHO_AM_I_CONST) { + return true; + } + } while (attemptsRemaining--); + + return false; +} + +static bool icm40609dGyroRead(gyroDev_t *gyro) +{ + uint8_t data[6]; + + const bool ack = busReadBuf(gyro->busDev, ICM40609D_RA_GYRO_DATA_X1, data, 6); + if (!ack) { + return false; + } + + gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0); + gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1); + gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2); + + return true; +} + +static bool icm40609dReadTemperature(gyroDev_t *gyro, int16_t * temp) +{ + uint8_t data[2]; + + const bool ack = busReadBuf(gyro->busDev, ICM40609D_RA_TEMP_DATA1, data, 2); + if (!ack) { + return false; + } + // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25 + *temp = ( int16_val_big_endian(data, 0) / 13.248 ) + 250; // Temperature stored as degC*10 + + return true; +} + +bool icm40609dGyroDetect(gyroDev_t *gyro) +{ + gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM40609D, gyro->imuSensorToUse, OWNER_MPU); + if (gyro->busDev == NULL) { + return false; + } + + if (!icm40609dDeviceDetect(gyro->busDev)) { + busDeviceDeInit(gyro->busDev); + return false; + } + + // Magic number for ACC detection to indicate that we have detected icm40609d gyro + mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev); + ctx->chipMagicNumber = 0x4609; + + gyro->initFn = icm40609dAccAndGyroInit; + gyro->readFn = icm40609dGyroRead; + gyro->intStatusFn = gyroCheckDataReady; + gyro->temperatureFn = icm40609dReadTemperature; + gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor, matches GYRO_FS_SEL=0 (+/-2000dps) + gyro->gyroAlign = gyro->busDev->param; + + return true; +} + +#endif diff --git a/src/main/drivers/accgyro/accgyro_icm40609d.h b/src/main/drivers/accgyro/accgyro_icm40609d.h new file mode 100644 index 00000000000..c1528b82b16 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_icm40609d.h @@ -0,0 +1,21 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +bool icm40609dAccDetect(accDev_t *acc); +bool icm40609dGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 4adf54e6f38..95646addd0c 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -32,6 +32,7 @@ #define ICM42686P_WHO_AM_I_CONST (0x44) #define ICM42688P_WHO_AM_I_CONST (0x47) #define ICM45686_WHO_AM_I_CONST (0xE9) +#define ICM40609D_WHO_AM_I_CONST (0x3B) // RA = Register Address diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 385fbd36c0e..1f02c29b40c 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -86,6 +86,7 @@ typedef enum { DEVHW_BMI270, DEVHW_LSM6D, DEVHW_ICM45686, + DEVHW_ICM40609D, /* Combined ACC/GYRO/MAG chips */ DEVHW_MPU9250, diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 8ed295c2d3f..05c4dcedca6 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -236,7 +236,7 @@ static const char *debugModeNames[DEBUG_COUNT] = { // sync with gyroSensor_e static const char *const gyroNames[] = { "NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", - "ICM20689", "BMI088", "ICM42605", "BMI270", "LSM6DXX", "ICM45686", "FAKE"}; + "ICM20689", "BMI088", "ICM42605", "BMI270", "LSM6DXX", "ICM45686", "ICM40609D", "FAKE"}; // sync this with sensors_e static const char * const sensorTypeNames[] = { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 74ad48e8d58..8849ccdaf28 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2,7 +2,7 @@ tables: - name: alignment values: ["DEFAULT", "CW0", "CW90", "CW180", "CW270", "CW0FLIP", "CW90FLIP", "CW180FLIP", "CW270FLIP"] - name: acc_hardware - values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "ICM45686", "FAKE"] + values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "ICM45686", "ICM40609D", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"] diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index cc52be637a9..88d98f78f3b 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -46,6 +46,7 @@ #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_icm42605.h" #include "drivers/accgyro/accgyro_icm45686.h" +#include "drivers/accgyro/accgyro_icm40609d.h" #include "drivers/accgyro/accgyro_lsm6dxx.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/sensor.h" @@ -260,6 +261,18 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) } FALLTHROUGH; #endif +#ifdef USE_IMU_ICM40609D + case ACC_ICM40609D: + if (icm40609dAccDetect(dev)) { + accHardware = ACC_ICM40609D; + break; + } + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (accHardwareToUse != ACC_AUTODETECT) { + break; + } + FALLTHROUGH; +#endif #ifdef USE_IMU_FAKE case ACC_FAKE: if (fakeAccDetect(dev)) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index ea76b63ca75..5ed555c6586 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -45,6 +45,7 @@ typedef enum { ACC_BMI270, ACC_LSM6DXX, ACC_ICM45686, + ACC_ICM40609D, ACC_FAKE, ACC_MAX = ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f55ba1292e2..38b56b22ae0 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -48,6 +48,7 @@ #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_icm42605.h" #include "drivers/accgyro/accgyro_icm45686.h" +#include "drivers/accgyro/accgyro_icm40609d.h" #include "drivers/accgyro/accgyro_lsm6dxx.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/io.h" @@ -241,6 +242,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard FALLTHROUGH; #endif +#ifdef USE_IMU_ICM40609D + case GYRO_ICM40609D: + if (icm40609dGyroDetect(dev)) { + gyroHardware = GYRO_ICM40609D; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_IMU_FAKE case GYRO_FAKE: if (fakeGyroDetect(dev)) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 18fe5d9e517..e8167d7d7d4 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -44,6 +44,7 @@ typedef enum { GYRO_BMI270, GYRO_LSM6DXX, GYRO_ICM45686, + GYRO_ICM40609D, GYRO_FAKE } gyroSensor_e; From b73253c735cb5513d70bb30393d418010f56c372 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 6 Aug 2026 09:46:14 -0500 Subject: [PATCH 2/2] Honor requested gyro sample rate instead of hard-coding 1kHz Add a rate-config table (mirroring ICM42605's) so the driver selects GYRO_CONFIG0/ACCEL_CONFIG0 ODR from gyro->requestedSampleIntervalUs instead of always running at 1kHz, which would have silently capped the PID loop rate on any board using this chip. Also document why the ICM42605-style INT_CONFIG1/ASYNC_RESET clear step is omitted: that register isn't present in the ICM-40609-D register map. --- src/main/CMakeLists.txt | 4 +-- src/main/drivers/accgyro/accgyro_icm40609d.c | 30 ++++++++++++++++---- 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 4a311fae17e..4fffe6f5a41 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -85,12 +85,12 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_fake.h drivers/accgyro/accgyro_icm20689.c drivers/accgyro/accgyro_icm20689.h + drivers/accgyro/accgyro_icm40609d.c + drivers/accgyro/accgyro_icm40609d.h drivers/accgyro/accgyro_icm42605.c drivers/accgyro/accgyro_icm42605.h drivers/accgyro/accgyro_icm45686.c drivers/accgyro/accgyro_icm45686.h - drivers/accgyro/accgyro_icm40609d.c - drivers/accgyro/accgyro_icm40609d.h drivers/accgyro/accgyro_mpu.c drivers/accgyro/accgyro_mpu.h drivers/accgyro/accgyro_mpu6000.c diff --git a/src/main/drivers/accgyro/accgyro_icm40609d.c b/src/main/drivers/accgyro/accgyro_icm40609d.c index cca9eca0202..5bef1a8a4bc 100644 --- a/src/main/drivers/accgyro/accgyro_icm40609d.c +++ b/src/main/drivers/accgyro/accgyro_icm40609d.c @@ -56,12 +56,10 @@ // GYRO_FS_SEL = 0 -> +/-2000dps, 16.4 LSB/(deg/s) #define ICM40609D_GYRO_FS_SEL_2000DPS (0 << 5) -#define ICM40609D_GYRO_ODR_1KHZ 6 // ACCEL_FS_SEL = 1 -> +/-16g, 2048 LSB/g (FS_SEL=0 is +/-32g on this chip, // unlike ICM42605 where 0 is the narrowest common range) #define ICM40609D_ACCEL_FS_SEL_16G (1 << 5) -#define ICM40609D_ACCEL_ODR_1KHZ 6 // Low-latency UI filter bandwidth select, both fields set to the "trivial" // low-latency option (verified numeric meaning against the same bit layout @@ -80,6 +78,20 @@ #define ICM40609D_RA_INT_SOURCE0 0x65 #define ICM40609D_UI_DRDY_INT1_EN_ENABLED (1 << 3) +// ODR select values (bits[3:0] of GYRO_CONFIG0/ACCEL_CONFIG0) verified +// against the datasheet's ODR tables -- identical encoding to ICM42605 +// for these five rates, so reusing the same gyroFilterAndRateConfig_t +// mechanism and DLPF tag (unused here; this driver doesn't vary the UI +// filter bandwidth by requested LPF, see icm40609dAccAndGyroInit). +static const gyroFilterAndRateConfig_t icm40609dGyroConfigs[] = { + /* DLPF ODR */ + { GYRO_LPF_256HZ, 8000, { 0, 3 } }, + { GYRO_LPF_256HZ, 4000, { 0, 4 } }, + { GYRO_LPF_256HZ, 2000, { 0, 5 } }, + { GYRO_LPF_256HZ, 1000, { 0, 6 } }, + { GYRO_LPF_256HZ, 500, { 0, 15 } }, +}; + static void icm40609dAccInit(accDev_t *acc) { acc->acc_1G = 512 * 4; // 2048 LSB/g, matches ACCEL_FS_SEL=1 (+/-16g) @@ -123,18 +135,19 @@ bool icm40609dAccDetect(accDev_t *acc) static void icm40609dAccAndGyroInit(gyroDev_t *gyro) { busDevice_t * dev = gyro->busDev; - - gyro->sampleRateIntervalUs = 1000; // 1kHz ODR + const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs, + &icm40609dGyroConfigs[0], ARRAYLEN(icm40609dGyroConfigs)); + gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz; busSetSpeed(dev, BUS_SPEED_INITIALIZATION); busWrite(dev, ICM40609D_RA_PWR_MGMT0, ICM40609D_PWR_MGMT0_TEMP_DISABLE_OFF | ICM40609D_PWR_MGMT0_ACCEL_MODE_LN | ICM40609D_PWR_MGMT0_GYRO_MODE_LN); delay(15); - busWrite(dev, ICM40609D_RA_GYRO_CONFIG0, ICM40609D_GYRO_FS_SEL_2000DPS | ICM40609D_GYRO_ODR_1KHZ); + busWrite(dev, ICM40609D_RA_GYRO_CONFIG0, ICM40609D_GYRO_FS_SEL_2000DPS | (config->gyroConfigValues[1] & 0x0F)); delay(15); - busWrite(dev, ICM40609D_RA_ACCEL_CONFIG0, ICM40609D_ACCEL_FS_SEL_16G | ICM40609D_ACCEL_ODR_1KHZ); + busWrite(dev, ICM40609D_RA_ACCEL_CONFIG0, ICM40609D_ACCEL_FS_SEL_16G | (config->gyroConfigValues[1] & 0x0F)); delay(15); // Low latency, same convention as ICM42605 @@ -144,6 +157,11 @@ static void icm40609dAccAndGyroInit(gyroDev_t *gyro) busWrite(dev, ICM40609D_RA_INT_CONFIG, ICM40609D_INT1_MODE_PULSED | ICM40609D_INT1_DRIVE_CIRCUIT_PP | ICM40609D_INT1_POLARITY_ACTIVE_HIGH); delay(15); + // Unlike ICM42605, this chip's Bank 0 register map has no INT_CONFIG1 + // (0x64) / ASYNC_RESET bit -- confirmed absent from DS-000272 rev 0.8 + // (register map jumps from INT_CONFIG0 at 0x63 straight to INT_SOURCE0 + // at 0x65). ICM42605's "clear ASYNC_RESET for proper INT1/INT2 + // operation" erratum step is therefore intentionally omitted here. busWrite(dev, ICM40609D_RA_INT_CONFIG0, ICM40609D_UI_DRDY_INT_CLEAR_ON_SBR); delay(100);