diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index e38c9381b8..77c4012c41 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -107,14 +107,27 @@ void ICM20649::print_status() int ICM20649::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM20649::RunImpl() @@ -153,8 +166,8 @@ void ICM20649::RunImpl() ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 10 ms"); - ScheduleDelayed(10_ms); + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); } } @@ -362,9 +375,9 @@ void ICM20649::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); } -void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index 11eb1a438a..cd1da7992b 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -109,7 +109,7 @@ private: void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index d95cb636c3..ae875a2866 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -49,6 +49,8 @@ ICM20948::ICM20948(const I2CSPIDriverConfig &config) : _px4_accel(get_device_id(), config.rotation), _px4_gyro(get_device_id(), config.rotation) { + _debug_enabled = true; + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -136,14 +138,27 @@ void ICM20948::print_status() int ICM20948::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM20948::RunImpl() @@ -400,9 +415,9 @@ void ICM20948::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); } -void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 3bb4509c63..59ce391510 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -117,7 +117,7 @@ private: void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } void SelectRegisterBank(Register::BANK_3 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_3); } diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp index c5cd3555a0..9146a13694 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp @@ -84,14 +84,27 @@ void ICM20948_I2C_Passthrough::print_status() int ICM20948_I2C_Passthrough::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM20948_I2C_Passthrough::RunImpl() @@ -180,9 +193,9 @@ void ICM20948_I2C_Passthrough::RunImpl() } } -void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2]; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp index bb3a998f49..cc8fa12358 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp @@ -76,7 +76,7 @@ private: bool Configure(); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } template bool RegisterCheck(const T ®_cfg); diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 894c9cd7c2..8dc0f4b795 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -43,11 +43,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) ICM40609D::ICM40609D(const I2CSPIDriverConfig &config) : SPI(config), I2CSPIDriver(config), - _drdy_gpio(drdy_gpio), + _drdy_gpio(config.drdy_gpio), _px4_accel(get_device_id(), config.rotation), _px4_gyro(get_device_id(), config.rotation) { - if (_drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -107,14 +107,27 @@ void ICM40609D::print_status() int ICM40609D::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM40609D::RunImpl() @@ -376,9 +389,9 @@ void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) } } -void ICM40609D::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM40609D::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 48ac7901ca..ab6fcfa1f2 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -104,7 +104,7 @@ private: void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } static int DataReadyInterruptCallback(int irq, void *context, void *arg); diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp index 88ddb8b30e..e805382194 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp @@ -47,7 +47,7 @@ ICM42605::ICM42605(const I2CSPIDriverConfig &config) : _px4_accel(get_device_id(), config.rotation), _px4_gyro(get_device_id(), config.rotation) { - if (_drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -107,14 +107,27 @@ void ICM42605::print_status() int ICM42605::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM42605::RunImpl() @@ -377,9 +390,9 @@ void ICM42605::ConfigureFIFOWatermark(uint8_t samples) } } -void ICM42605::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM42605::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp index 103d05e970..2059b7a1b2 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp @@ -104,7 +104,7 @@ private: void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } static int DataReadyInterruptCallback(int irq, void *context, void *arg); diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 0c8e66f862..32513feeea 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -107,14 +107,27 @@ void ICM42688P::print_status() int ICM42688P::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM42688P::RunImpl() @@ -313,9 +326,9 @@ void ICM42688P::ConfigureFIFOWatermark(uint8_t samples) } } -void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index 8b80447e49..8894532338 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -114,7 +114,7 @@ private: void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); } void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }