aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: handle queue push and pop failing

Browse Source
This commit is contained in:
Peter Barker 2020-06-03 16:05:12 +10:00 committed by Randy Mackay
parent 2ace0f0a1f
commit efb576292f
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp
View File

@ -118,9 +118,13 @@ void SLCANRouter::slcan2can_router_trampoline(void)
chSysUnlock(); chSysUnlock();
_slcan_if.reader(); _slcan_if.reader();
while (_can_tx_queue.available() && _can_if) { while (_can_tx_queue.available() && _can_if) {
_can_tx_queue.peek(it); if (!_can_tx_queue.peek(it)) {
break;
}
if (_can_if->send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) { if (_can_if->send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) {
_can_tx_queue.pop(); if (!_can_tx_queue.pop()) {
break;
}
} else { } else {
break; break;
} }
@ -142,9 +146,13 @@ void SLCANRouter::can2slcan_router_trampoline(void)
chSysUnlock(); chSysUnlock();
_update_event->wait(uavcan::MonotonicDuration::fromUSec(1000)); _update_event->wait(uavcan::MonotonicDuration::fromUSec(1000));
while (_slcan_tx_queue.available()) { while (_slcan_tx_queue.available()) {
_slcan_tx_queue.peek(it); if (!_slcan_tx_queue.peek(it)) {
break;
}
if (_slcan_if.send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) { if (_slcan_if.send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) {
_slcan_tx_queue.pop(); if (!_slcan_tx_queue.pop()) {
break;
}
} else { } else {
break; break;
} }