From 78faa5c6b03111208e94178b5f791989117485de Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 1 Sep 2023 19:04:38 +1000
Subject: [PATCH] AP_Periph: implement can inface masking per message

this prevents sending duplicate messages on an interface when a send
fails on a 2nd interface
---
 Tools/AP_Periph/can.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 0efc3ec17b..38f9661a1e 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -36,7 +36,7 @@
 #include <AP_HAL_SITL/CANSocketIface.h>
 #endif
 
-#define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES+1U))-1U)
+#define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES))-1U)
 
 #include "i2c.h"
 #include <utility>
@@ -1091,7 +1091,7 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature,
 
 void AP_Periph_FW::processTx(void)
 {
-    for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) {
+    for (CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&dronecan.canard)) != NULL;) {
         AP_HAL::CANFrame txmsg {};
         txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len);
         memcpy(txmsg.data, txf->data, txf->data_len);
@@ -1130,6 +1130,10 @@ void AP_Periph_FW::processTx(void)
                 if (stats == nullptr || now_us - stats->last_transmit_us < 2000000UL) {
                     sent = false;
                 }
+            } else {
+#if CANARD_MULTI_IFACE
+                txf->iface_mask &= ~(1U<<_ins.index);
+#endif
             }
         }
         if (sent) {