Skip to content

Commit

Permalink
HAL_Linux: reduce delay(ms) jitter
Browse files Browse the repository at this point in the history
Fix delay(1) rarely returning immediately.
On my RPi4, this once per 5-20k calls that worked.

Reduce the last call to microsleep according to the
remaining time needed in the last loop iteration.
  • Loading branch information
rsaxvc committed Feb 11, 2024
1 parent 1862b5d commit 1708562
Showing 1 changed file with 9 additions and 4 deletions.
13 changes: 9 additions & 4 deletions libraries/AP_HAL_Linux/Scheduler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -173,15 +173,20 @@ void Scheduler::delay(uint16_t ms)
return;
}

uint64_t start = AP_HAL::millis64();
if (ms == 0) {
return;
}

while ((AP_HAL::millis64() - start) < ms) {
uint64_t now = AP_HAL::micros64();
uint64_t end = now + 1000UL * ms + 1U;
do {
// this yields the CPU to other apps
microsleep(1000);
microsleep(MIN(1000UL, end-now));
if (in_main_thread() && _min_delay_cb_ms <= ms) {
call_delay_cb();
}
}
now = AP_HAL::micros64();
} while (now < end);
}

void Scheduler::delay_microseconds(uint16_t us)
Expand Down

0 comments on commit 1708562

Please sign in to comment.