From 6358ec07b60acfdf160cba800bfe227a3c5ba9ae Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Tue, 24 Mar 2026 18:39:38 -0500 Subject: [PATCH] Fix bugs in ICM-45686 IMU driver - Fix settings.yaml: quote "ICM45686" string in acc_hardware table - Fix accel LPF fallback: write BYPASS not ODR_DIV_8 on IREG failure - Fix IREG polling: use delayMicroseconds(10) not delay(1) to match waited_us counter - Fix reset polling: replace while(1) with do/while pattern used by all other drivers - Fix temperature formula: use float literals (12.8f) to avoid double promotion on F4 - Add named constant ICM456XX_INT_CONFIG_DELAY_MS for post-interrupt settle delay - Remove unused endianness defines (ICM456XX_RA_SREG_CTRL, SREG_DATA_ENDIAN_SEL_*) - Add missing trailing newlines to all modified files --- src/main/drivers/accgyro/accgyro_icm45686.c | 32 ++++++++------------- src/main/drivers/accgyro/accgyro_icm45686.h | 2 +- src/main/drivers/accgyro/accgyro_mpu.h | 2 +- src/main/drivers/bus.h | 2 +- src/main/fc/settings.yaml | 2 +- src/main/sensors/acceleration.c | 2 +- src/main/sensors/acceleration.h | 2 +- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 +- src/main/target/common_hardware.c | 2 +- 10 files changed, 21 insertions(+), 29 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_icm45686.c b/src/main/drivers/accgyro/accgyro_icm45686.c index a9e0ee0b8a5..9b249fb607f 100644 --- a/src/main/drivers/accgyro/accgyro_icm45686.c +++ b/src/main/drivers/accgyro/accgyro_icm45686.c @@ -104,16 +104,6 @@ Note: Now implemented only UI Interface with Low-Noise Mode #define ICM456XX_ACCEL_CONFIG0 0x1B #define ICM456XX_PWR_MGMT0 0x10 #define ICM456XX_TEMP_DATA1 0x0C -// Register map IPREG_TOP1 (for future use) -// Note: SREG_CTRL register provides endian selection capability. -// Currently not utilized as UI interface reads are in device's native endianness. -// Kept as reference for potential future optimization. -#define ICM456XX_RA_SREG_CTRL 0xA267 // To access: 0xA200 + 0x67 - -// SREG_CTRL - 0x67 (bits 1:0 select data endianness) -#define ICM456XX_SREG_DATA_ENDIAN_SEL_LITTLE (0 << 1) // Little endian (native) -#define ICM456XX_SREG_DATA_ENDIAN_SEL_BIG (1 << 1) // Big endian (requires IREG write) - // MGMT0 - 0x10 - Gyro #define ICM456XX_GYRO_MODE_OFF (0x00 << 2) #define ICM456XX_GYRO_MODE_STANDBY (0x01 << 2) @@ -264,6 +254,7 @@ Note: Now implemented only UI Interface with Low-Noise Mode #define ICM456XX_ACCEL_STARTUP_TIME_MS 10 // Min accel startup from OFF/STANDBY/LP #define ICM456XX_GYRO_STARTUP_TIME_MS 35 // Min gyro startup from OFF/STANDBY/LP #define ICM456XX_SENSOR_ENABLE_DELAY_MS 1 // Allow sensors to power on and stabilize +#define ICM456XX_INT_CONFIG_DELAY_MS 15 // Register settle time after interrupt config (matches ICM-426xx convention) #define ICM456XX_IREG_TIMEOUT_US 5000 // IREG operation timeout (5ms max) #define ICM456XX_DATA_LENGTH 6 // 3 axes * 2 bytes per axis @@ -305,7 +296,7 @@ static bool icm45686WriteIREG(const busDevice_t *dev, uint16_t reg, uint8_t valu if (misc2 & ICM456XX_BIT_IREG_DONE) { return true; } - delay(1); + delayMicroseconds(10); } return false; // timeout @@ -357,7 +348,7 @@ static bool icm45686ReadTemperature(gyroDev_t *gyro, int16_t * temp) return false; } // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 128) + 25 - *temp = ( int16_val_little_endian(data, 0) / 12.8 ) + 250; // Temperature stored as degC*10 + *temp = ( int16_val_little_endian(data, 0) / 12.8f ) + 250.0f; // Temperature stored as degC*10 return true; } @@ -398,7 +389,7 @@ static void icm45686AccAndGyroInit(gyroDev_t *gyro) // Set the Accel UI LPF bandwidth cut-off to ODR/8 (Section 7.3 of datasheet) if (!icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8)) { // If LPF configuration fails, fallback to BYPASS - icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8); + icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_BYPASS); } // Setup scale and odr values for gyro busWrite(dev, ICM456XX_GYRO_CONFIG0, ICM456XX_GYRO_FS_SEL_2000DPS | config->gyroConfigValues[1]); @@ -409,7 +400,7 @@ static void icm45686AccAndGyroInit(gyroDev_t *gyro) ICM456XX_INT1_POLARITY_ACTIVE_HIGH); busWrite(dev, ICM456XX_INT1_CONFIG0, ICM456XX_INT1_STATUS_EN_DRDY); - delay(15); + delay(ICM456XX_INT_CONFIG_DELAY_MS); busSetSpeed(dev, BUS_SPEED_FAST); } @@ -424,17 +415,18 @@ static bool icm45686DeviceDetect(busDevice_t * dev) // Perform soft reset directly // Soft reset busWrite(dev, ICM456XX_REG_MISC2, ICM456XX_SOFT_RESET); - // Wait for reset to complete (bit 1 of REG_MISC2 becomes 0) - while (1) { + // Poll until soft reset completes (SOFT_RESET bit clears) per datasheet Section 9.4 + do { busRead(dev, ICM456XX_REG_MISC2, &tmp); if (!(tmp & ICM456XX_SOFT_RESET)) { break; } delay(1); waitedMs++; - if (waitedMs >= 20) { - return false; - } + } while (waitedMs < 20); + + if (tmp & ICM456XX_SOFT_RESET) { + return false; } // Initialize power management to a known state after reset // This ensures sensors are off and ready for proper initialization @@ -497,4 +489,4 @@ bool icm45686GyroDetect(gyroDev_t *gyro) } -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/accgyro/accgyro_icm45686.h b/src/main/drivers/accgyro/accgyro_icm45686.h index 83c630e13bc..aac195aa220 100644 --- a/src/main/drivers/accgyro/accgyro_icm45686.h +++ b/src/main/drivers/accgyro/accgyro_icm45686.h @@ -19,4 +19,4 @@ bool icm45686AccDetect(accDev_t *acc); -bool icm45686GyroDetect(gyroDev_t *gyro); \ No newline at end of file +bool icm45686GyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 16089f5f542..0f59d10d3e8 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -182,4 +182,4 @@ const gyroFilterAndRateConfig_t * mpuChooseGyroConfig(uint8_t desiredLpf, uint16 bool mpuGyroRead(struct gyroDev_s *gyro); bool mpuGyroReadScratchpad(struct gyroDev_s *gyro); bool mpuAccReadScratchpad(struct accDev_s *acc); -bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data); \ No newline at end of file +bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 3c26067a557..353a06b306e 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -331,4 +331,4 @@ bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data); bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length); bool busTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * buffers, int count); -bool busIsBusy(const busDevice_t * dev); \ No newline at end of file +bool busIsBusy(const busDevice_t * dev); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index cab48e08a15..0b1e2c9b4f8 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", "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 d0c913623ed..6ad80e504b9 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -709,4 +709,4 @@ void accInitFilters(void) bool accIsHealthy(void) { return true; -} \ No newline at end of file +} diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 1037f85ca37..ea76b63ca75 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -98,4 +98,4 @@ void accSetCalibrationValues(void); void accInitFilters(void); bool accIsHealthy(void); bool accGetCalibrationAxisStatus(int axis); -uint8_t accGetCalibrationAxisFlags(void); \ No newline at end of file +uint8_t accGetCalibrationAxisFlags(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5abc4980f97..8544b45dac1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -619,4 +619,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { float averageAbsGyroRates(void) { return (fabsf(gyro.gyroADCf[ROLL]) + fabsf(gyro.gyroADCf[PITCH]) + fabsf(gyro.gyroADCf[YAW])) / 3.0f; -} \ No newline at end of file +} diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index d04eb2f705d..18fe5d9e517 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -120,4 +120,4 @@ bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); void gyroUpdateDynamicLpf(float cutoffFreq); -float averageAbsGyroRates(void); \ No newline at end of file +float averageAbsGyroRates(void); diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 020e3f7b35c..ddaae1b7b16 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -477,4 +477,4 @@ BUSDEV_REGISTER_I2C(busdev_pcf8574, DEVHW_PCF8574, PCF8574_I2C_BUS, 0x20, NONE, DEVFLAGS_NONE, 0); #endif -#endif // USE_TARGET_HARDWARE_DESCRIPTORS \ No newline at end of file +#endif // USE_TARGET_HARDWARE_DESCRIPTORS