mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_VRBRAIN: UARTDriver: fix writting with failures
When the buffer wraps and we do it in 2 steps, we can't actually do the second part if it fails or if we wrote less bytes than we intended, otherwise we will corrupt the data being sent.
This commit is contained in:
parent
68fc08fe52
commit
80cf1207b7
|
@ -445,8 +445,15 @@ void VRBRAINUARTDriver::_timer_tick(void)
|
|||
const auto n_vec = _writebuf.peekiovec(vec, n);
|
||||
for (int i = 0; i < n_vec; i++) {
|
||||
ret = _write_fd(vec[i].data, (uint16_t)vec[i].len);
|
||||
if (ret > 0)
|
||||
if (ret < 0) {
|
||||
break;
|
||||
}
|
||||
_writebuf.advance(ret);
|
||||
|
||||
/* We wrote less than we asked for, stop */
|
||||
if ((unsigned)ret != vec[i].len) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
perf_end(_perf_uart);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue