AP_InertialSensor: yield the CPU for the right time in wait_for_sample()

this improves timing performance
This commit is contained in:
Andrew Tridgell 2013-10-14 08:45:58 +11:00
parent d973730b88
commit 7831113f84
1 changed files with 7 additions and 1 deletions

View File

@ -159,7 +159,13 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
} }
uint32_t start = hal.scheduler->millis(); uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) { while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100); uint64_t tnow = hrt_absolute_time();
// we spin for the last timing_lag microseconds. Before that
// we yield the CPU to allow IO to happen
const uint16_t timing_lag = 400;
if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
}
if (sample_available()) { if (sample_available()) {
return true; return true;
} }