HAL_ChibiOS: fixed a bug in wait_timeout() on UARTs

wait_timeout() could return true on a read of less bytes than are
expected.
This commit is contained in:
Andrew Tridgell 2019-08-13 17:56:26 +10:00
parent eb9e5df0dd
commit afc8a70ce4
1 changed files with 11 additions and 7 deletions

View File

@ -647,14 +647,18 @@ size_t UARTDriver::write_locked(const uint8_t *buffer, size_t size, uint32_t key
*/
bool UARTDriver::wait_timeout(uint16_t n, uint32_t timeout_ms)
{
chEvtGetAndClearEvents(EVT_DATA);
if (available() >= n) {
return true;
uint32_t t0 = AP_HAL::millis();
while (available() < n) {
chEvtGetAndClearEvents(EVT_DATA);
_wait.n = n;
_wait.thread_ctx = chThdGetSelfX();
uint32_t now = AP_HAL::millis();
if (now - t0 >= timeout_ms) {
break;
}
chEvtWaitAnyTimeout(EVT_DATA, chTimeMS2I(timeout_ms - (now - t0)));
}
_wait.n = n;
_wait.thread_ctx = chThdGetSelfX();
eventmask_t mask = chEvtWaitAnyTimeout(EVT_DATA, chTimeMS2I(timeout_ms));
return (mask & EVT_DATA) != 0;
return available() >= n;
}
/*