From d25a784a3ae100f9a0bbd740b33fe47510c4b60c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 23 May 2022 10:16:55 -0400 Subject: [PATCH] icm42688p: only check configured registers periodically (as intended) --- .../imu/invensense/icm42688p/ICM42688P.cpp | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 32513feeea..8f149bdf5d 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -274,20 +274,22 @@ void ICM42688P::RunImpl() } } - // check configuration registers periodically or immediately following any failure - if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) - && RegisterCheck(_register_bank1_cfg[_checked_register_bank1]) - && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) - ) { - _last_config_check_timestamp = now; - _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; - _checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg; - _checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg; + 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_bank1_cfg[_checked_register_bank1]) + && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) + ) { + _last_config_check_timestamp = now; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + _checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg; + _checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg; - } else { - // register check failed, force reset - perf_count(_bad_register_perf); - Reset(); + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } } }