diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index 9bd89a7b71..f138cc55a7 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -51,7 +51,7 @@ ICM20948::ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) { if (drdy_gpio != 0) { - _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval"); + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); @@ -82,7 +82,7 @@ ICM20948::~ICM20948() perf_free(_fifo_empty_perf); perf_free(_fifo_overflow_perf); perf_free(_fifo_reset_perf); - perf_free(_drdy_interval_perf); + perf_free(_drdy_missed_perf); delete _slave_ak09916_magnetometer; } @@ -125,7 +125,7 @@ void ICM20948::print_status() perf_print_counter(_fifo_empty_perf); perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_reset_perf); - perf_print_counter(_drdy_interval_perf); + perf_print_counter(_drdy_missed_perf); if (_slave_ak09916_magnetometer) { @@ -154,7 +154,7 @@ void ICM20948::RunImpl() // PWR_MGMT_1: Device Reset RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); _reset_timestamp = now; - _consecutive_failures = 0; + _failure_count = 0; _state = STATE::WAIT_FOR_RESET; ScheduleDelayed(100_ms); break; @@ -231,8 +231,8 @@ void ICM20948::RunImpl() case STATE::FIFO_READ: { if (_data_ready_interrupt_enabled) { // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) { - perf_count_interval(_drdy_interval_perf, now); + if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -263,22 +263,25 @@ void ICM20948::RunImpl() } else if (samples >= 1) { if (FIFORead(now, samples)) { success = true; - _consecutive_failures = 0; + + if (_failure_count > 0) { + _failure_count--; + } } } } if (!success) { - _consecutive_failures++; + _failure_count++; // full reset if things are failing consistently - if (_consecutive_failures > 10) { + if (_failure_count > 10) { Reset(); return; } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) @@ -442,12 +445,12 @@ int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20948::DataReady() { - const uint8_t count = _drdy_count.fetch_add(1) + 1; - - uint8_t expected = 0; + uint32_t expected = 0; // at least the required number of samples in the FIFO - if ((count >= _fifo_gyro_samples) && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { + if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) + && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { + _drdy_count.store(0); ScheduleNow(); } diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 745c6fba68..2dca68c671 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -163,17 +163,17 @@ private: perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; - perf_counter_t _drdy_interval_perf{nullptr}; + perf_counter_t _drdy_missed_perf{nullptr}; hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; hrt_abstime _temperature_update_timestamp{0}; - unsigned _consecutive_failures{0}; + int _failure_count{0}; enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -186,7 +186,7 @@ private: STATE _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; static constexpr uint8_t size_register_bank0_cfg{6}; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp index 4654453ab7..0c7ed50ab8 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -86,7 +86,7 @@ void ICM20948_AK09916::Run() // CNTL3 SRST: Soft reset _icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL3, CNTL3_BIT::SRST); _reset_timestamp = hrt_absolute_time(); - _consecutive_failures = 0; + _failure_count = 0; _state = STATE::READ_WHO_AM_I; ScheduleDelayed(100_ms); break; @@ -146,15 +146,17 @@ void ICM20948_AK09916::Run() success = true; - _consecutive_failures = 0; + if (_failure_count > 0) { + _failure_count--; + } } } if (!success) { perf_count(_bad_transfer_perf); - _consecutive_failures++; + _failure_count++; - if (_consecutive_failures > 10) { + if (_failure_count > 10) { Reset(); return; } diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp index 3387b4462a..b7e2ac459e 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp @@ -93,7 +93,7 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; - unsigned _consecutive_failures{0}; + int _failure_count{0}; enum class STATE : uint8_t { RESET,