dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

icm42688p: only check configured registers periodically (as intended)

This commit is contained in:
Daniel Agar 2022-05-23 10:16:55 -04:00
parent c57c575cfe
commit d25a784a3a
1 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

@ -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();
}
}
}