fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

icm42688p: only check configured registers periodically (as intended)

Browse Source
This commit is contained in:
Daniel Agar 2022-05-23 10:16:55 -04:00
parent 1f399e5b33
commit b8ce2a6039
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
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();
}
}
}