mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_HAL_SITL: correct I2C callback interval
This commit is contained in:
parent
324ff178d8
commit
3c16f164df
@ -85,7 +85,7 @@ void I2CBus::_timer_tick()
|
||||
{
|
||||
const uint64_t now = AP_HAL::micros64();
|
||||
for (struct callback_info *ci = callbacks; ci != nullptr; ci = ci->next) {
|
||||
if (ci->next_usec >= now) {
|
||||
if (ci->next_usec < now) {
|
||||
WITH_SEMAPHORE(sem);
|
||||
ci->cb();
|
||||
ci->next_usec += ci->period_usec;
|
||||
|
Loading…
Reference in New Issue
Block a user