diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index ad796fd8a82..b040c34e346 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -31,8 +31,9 @@ static etx_i2c_bus_t s_i2c_bus; static uint16_t s_i2c_addr; static constexpr uint32_t RESET_DELAY_MS = 150; -static constexpr uint32_t READY_TIMEOUT_MS = 200; -static constexpr uint32_t POWER_DELAY_MS = 20; +static constexpr uint32_t READY_TIMEOUT_MS = 500; +static constexpr uint32_t POWER_ON_SETTLE_US = 200; +static constexpr uint32_t GYRO_STARTUP_DELAY_MS = 45; static constexpr uint32_t CONFIG_DELAY_MS = 1; static constexpr uint8_t BURST_READ_LEN = 14; @@ -75,7 +76,10 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) return -1; } - if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) { + // The chip can take longer than a couple of quick retries to start + // acknowledging on I2C after power-up, so give the initial probe the + // same generous retry window used after a soft reset below. + if (wait_for_device_ready(READY_TIMEOUT_MS) < 0) { TRACE("ICM42627: device did not acknowledge on I2C"); return -1; } @@ -103,17 +107,28 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) return -1; } - if (write_cmd(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1) < 0) { - TRACE("ICM42627: failed to write PWR_MGMT0 stage 1"); + // Per datasheet section 12.6 ("Register Values Modification"), registers + // other than ODR/FSR/mode selects must only be changed while the + // accel/gyro are off. Configure the signal path here, while PWR_MGMT0 + // still has gyro and accel powered down, then power them on last. + if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK1) < 0) { + TRACE("ICM42627: failed to select register bank 1"); return -1; } - delay_ms(POWER_DELAY_MS); + delay_ms(CONFIG_DELAY_MS); - if (write_cmd(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2) < 0) { - TRACE("ICM42627: failed to write PWR_MGMT0 stage 2"); + if (write_cmd(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2) < 0) { + TRACE("ICM42627: failed to write GYRO_STATIC2"); + write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK0); + return -1; + } + delay_ms(CONFIG_DELAY_MS); + + if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK0) < 0) { + TRACE("ICM42627: failed to restore register bank 0"); return -1; } - delay_ms(POWER_DELAY_MS); + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG) < 0) { TRACE("ICM42627: failed to write GYRO_CONFIG0"); @@ -145,25 +160,23 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) } delay_ms(CONFIG_DELAY_MS); - if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK1) < 0) { - TRACE("ICM42627: failed to select register bank 1"); - return -1; - } - delay_ms(CONFIG_DELAY_MS); - - if (write_cmd(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2) < 0) { - TRACE("ICM42627: failed to write GYRO_STATIC2"); + // Power on gyro and accel in Low Noise mode last, now that the signal + // path is fully configured. The datasheet requires no register writes + // for 200us after an OFF -> mode transition, and the gyro must be kept + // on for at least 45ms before its output is valid. + if (write_cmd(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1) < 0) { + TRACE("ICM42627: failed to write PWR_MGMT0 stage 1"); return -1; } - delay_ms(CONFIG_DELAY_MS); + delay_us(POWER_ON_SETTLE_US); - if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK0) < 0) { - TRACE("ICM42627: failed to restore register bank 0"); + if (write_cmd(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2) < 0) { + TRACE("ICM42627: failed to write PWR_MGMT0 stage 2"); return -1; } - delay_ms(CONFIG_DELAY_MS); + delay_ms(GYRO_STARTUP_DELAY_MS); - // Re-read WHO_AM_I after reset and banked configuration to catch a device + // Re-read WHO_AM_I after reset and configuration to catch a device // that stopped responding even though the initial probe succeeded. if (read_cmd(ICM42627_WHO_AM_I_REG, &who_am_i) < 0 || who_am_i != ICM42627_WHO_AM_I) { diff --git a/radio/src/drivers/icm42627.h b/radio/src/drivers/icm42627.h index 70c9395a6aa..1b9df0f26b7 100644 --- a/radio/src/drivers/icm42627.h +++ b/radio/src/drivers/icm42627.h @@ -23,6 +23,8 @@ #include "hal/imu.h" +#define ICM42627_I2C_BASE_ADDR 0x68 + #define ICM42627_WHO_AM_I_REG 0x75 #define ICM42627_WHO_AM_I 0x20 diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index 80c04f93a8f..6f974fec098 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -116,6 +116,8 @@ if (PCB STREQUAL X10) set(AUX2_SERIAL OFF) set(CSD203_SENSOR ON) set(FLYSKY_GIMBAL ON) + add_definitions(-DIMU_ICM42627) + add_definitions(-DIMU_SC7U22) if (BLUETOOTH) set(AUX_SERIAL OFF) endif() @@ -285,6 +287,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL targets/common/arm/stm32/heartbeat_driver.cpp drivers/lsm6ds.cpp drivers/icm42627.cpp + drivers/sc7u22.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/module_timer_driver.cpp targets/common/arm/stm32/sticks_pwm_driver.cpp diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index bbe71bb0a48..49fa1b5d8c9 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -52,8 +52,22 @@ #include "csd203_sensor.h" #endif -#if defined(IMU) && defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS) +#if defined(IMU) && defined(IMU_I2C_BUS) && \ + (defined(IMU_I2C_ADDRESS) || defined(IMU_ICM42627) || \ + defined(IMU_SC7U22) || defined(IMU_LSM6DS33)) #define HAS_IMU + #include "gyro.h" + #include "stm32_i2c_driver.h" + #if defined(IMU_ICM42627) + #include "drivers/icm42627.h" + #endif + #if defined(IMU_SC7U22) + #include "drivers/sc7u22.h" + #endif + #if defined(IMU_LSM6DS33) || \ + (!defined(IMU_ICM42627) && !defined(IMU_SC7U22) && defined(IMU_I2C_ADDRESS)) + #include "drivers/lsm6ds.h" + #endif #endif HardwareOptions hardwareOptions; @@ -133,25 +147,26 @@ void audioInit() #endif #if defined(HAS_IMU) -#include "gyro.h" -#include "stm32_i2c_driver.h" -#include "drivers/icm42627.h" -#include "drivers/lsm6ds.h" +static const etx_imu_t _imu_candidates[] = { +#if defined(IMU_ICM42627) + { &imu_icm42627_driver, IMU_I2C_BUS, ICM42627_I2C_BASE_ADDR }, + { &imu_icm42627_driver, IMU_I2C_BUS, ICM42627_I2C_BASE_ADDR + 1 }, +#endif +#if defined(IMU_SC7U22) + { &imu_sc7u22_driver, IMU_I2C_BUS, SC7U22_I2C_BASE_ADDR }, + { &imu_sc7u22_driver, IMU_I2C_BUS, SC7U22_I2C_BASE_ADDR + 1 }, +#endif +#if defined(IMU_LSM6DS33) || \ + (!defined(IMU_ICM42627) && !defined(IMU_SC7U22) && defined(IMU_I2C_ADDRESS)) + { &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS }, +#endif +}; static void gyroInit() { -#if defined(RADIO_V16) - const etx_imu_driver_t *driver = &imu_icm42627_driver; -#else - const etx_imu_driver_t *driver = &imu_lsm6ds_driver; -#endif - - const etx_imu_t candidates[] = { - { driver, IMU_I2C_BUS, IMU_I2C_ADDRESS }, - }; - - gyroStart(imuDetect(candidates, DIM(candidates))); + gyroStart(imuDetect(_imu_candidates, DIM(_imu_candidates))); } + #endif void boardInit() diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index ac93332d46e..868c9d621d7 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -466,7 +466,6 @@ #define IMU_I2C_ADDRESS 0x6A #elif defined(RADIO_V16) #define IMU_I2C_BUS I2C_Bus_1 - #define IMU_I2C_ADDRESS 0x69 #elif !defined(AUX_SERIAL) && defined(IMU_LSM6DS33) #define IMU_I2C_BUS I2C_Bus_2 #define IMU_I2C_ADDRESS 0x6A