mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-25 01:03:59 -04:00
HAL_AVR: reduce the latency of semaphore waits
this reduces the average cost of waiting for the MPU6000 semaphore from the main loop
This commit is contained in:
parent
cfa1b5353f
commit
ae1fd4baa4
@ -52,14 +52,15 @@ bool AVRSemaphore::_take_from_mainloop(uint32_t timeout_ms) {
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint16_t timeout_ticks = timeout_ms*10;
|
||||||
do {
|
do {
|
||||||
/* Delay 1ms until we can successfully take, or we timed out */
|
/* Delay 1ms until we can successfully take, or we timed out */
|
||||||
hal.scheduler->delay(1);
|
hal.scheduler->delay_microseconds(100);
|
||||||
timeout_ms--;
|
timeout_ticks--;
|
||||||
if (_take_nonblocking()) {
|
if (_take_nonblocking()) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
} while (timeout_ms > 0);
|
} while (timeout_ticks > 0);
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user