From b94f08998485fe137fcb3f1d6663cb513760eaa7 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 20 Mar 2020 22:37:37 +0530 Subject: [PATCH] HAL_ChibiOS: improve throughput of slcan router --- libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp b/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp index 12e6a1d1c4..6f1c2c5894 100644 --- a/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp +++ b/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp @@ -117,12 +117,16 @@ void SLCANRouter::slcan2can_router_trampoline(void) } chSysUnlock(); _slcan_if.reader(); - if (_can_tx_queue.available() && _can_if) { + while (_can_tx_queue.available() && _can_if) { _can_tx_queue.peek(it); if (_can_if->send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) { _can_tx_queue.pop(); + } else { + break; } + hal.scheduler->delay_microseconds(100); } + hal.scheduler->delay_microseconds(100); } } @@ -137,12 +141,16 @@ void SLCANRouter::can2slcan_router_trampoline(void) } chSysUnlock(); _update_event->wait(uavcan::MonotonicDuration::fromUSec(1000)); - if (_slcan_tx_queue.available()) { + while (_slcan_tx_queue.available()) { _slcan_tx_queue.peek(it); if (_slcan_if.send(it.frame, uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 1000), 0)) { _slcan_tx_queue.pop(); + } else { + break; } + hal.scheduler->delay_microseconds(100); } + hal.scheduler->delay_microseconds(100); } }