From 71b2315d7805e87f34514e9266a5a6c6a5fd1d4d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 6 Nov 2019 17:56:13 +1100
Subject: [PATCH] AP_Periph: switch to callback based CAN receive

this is much more reliable
---
 Tools/AP_Periph/can.cpp | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index c30ebf67b6..85bed2cf3f 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -47,6 +47,7 @@
 #include <drivers/stm32/canard_stm32.h>
 #include <AP_HAL/I2CDevice.h>
 #include "../AP_Bootloader/app_comms.h"
+#include <AP_HAL/utility/RingBuffer.h>
 
 #include "i2c.h"
 #include <utility>
@@ -787,10 +788,22 @@ static void processTx(void)
     }
 }
 
+static ObjectBuffer<CANRxFrame> rxbuffer{16};
+
+static void can_rxfull_cb(CANDriver *canp, uint32_t flags)
+{
+    CANRxFrame rxmsg;
+    chSysLockFromISR();
+    while (canTryReceiveI(canp, CAN_ANY_MAILBOX, &rxmsg) == false) {
+        rxbuffer.push_force(rxmsg);
+    }
+    chSysUnlockFromISR();
+}
+
 static void processRx(void)
 {
-    CANRxFrame rxmsg {};
-    while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
+    CANRxFrame rxmsg;
+    while (rxbuffer.pop(rxmsg)) {
         CanardCANFrame rx_frame {};
 
         //palToggleLine(HAL_GPIO_PIN_LED);
@@ -999,6 +1012,8 @@ void AP_Periph_FW::can_start()
         PreferredNodeID = g.can_node;
     }
 
+    CAND1.rxfull_cb = can_rxfull_cb;
+
     canStart(&CAND1, &cancfg);
 
     canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),