From efb576292fdeae31f447915f41e8e731b1a2b6ea Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 3 Jun 2020 16:05:12 +1000
Subject: [PATCH] AP_HAL_ChibiOS: handle queue push and pop failing

---
 libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp b/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp
index 6f1c2c5894..810fade190 100644
--- a/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp
+++ b/libraries/AP_HAL_ChibiOS/CANSerialRouter.cpp
@@ -118,9 +118,13 @@ void SLCANRouter::slcan2can_router_trampoline(void)
         chSysUnlock();
         _slcan_if.reader();
         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)) {
-                _can_tx_queue.pop();
+                if (!_can_tx_queue.pop()) {
+                    break;
+                }
             } else {
                 break;
             }
@@ -142,9 +146,13 @@ void SLCANRouter::can2slcan_router_trampoline(void)
         chSysUnlock();
         _update_event->wait(uavcan::MonotonicDuration::fromUSec(1000));
         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)) {
-                _slcan_tx_queue.pop();
+                if (!_slcan_tx_queue.pop()) {
+                    break;
+                }
             } else {
                 break;
             }