From ad59f6db01b54dd907bdea95a02891cd1eb698b3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 23 Nov 2023 14:16:08 +1100
Subject: [PATCH] AP_DroneCAN: support an aux 11 bit protocol with DroneCAN

---
 libraries/AP_DroneCAN/AP_Canard_iface.cpp | 35 +++++++++++++++++++++++
 libraries/AP_DroneCAN/AP_Canard_iface.h   | 13 ++++++++-
 libraries/AP_DroneCAN/AP_DroneCAN.cpp     | 16 +++++++++++
 libraries/AP_DroneCAN/AP_DroneCAN.h       |  7 +++++
 4 files changed, 70 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
index 60980e510f..86bedca40e 100644
--- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp
+++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
@@ -9,6 +9,7 @@
 extern const AP_HAL::HAL& hal;
 #define LOG_TAG "DroneCANIface"
 #include <canard.h>
+#include <AP_CANManager/AP_CANSensor.h>
 
 #define DEBUG_PKTS 0
 
@@ -346,6 +347,15 @@ void CanardInterface::processRx() {
             if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) {
                 break;
             }
+
+            if (!rxmsg.isExtended()) {
+                // 11 bit frame, see if we have a handler
+                if (aux_11bit_driver != nullptr) {
+                    aux_11bit_driver->handle_frame(rxmsg);
+                }
+                continue;
+            }
+
             rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc);
             memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len);
 #if HAL_CANFD_SUPPORTED
@@ -434,4 +444,29 @@ bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface)
     num_ifaces++;
     return true;
 }
+
+// add an 11 bit auxillary driver
+bool CanardInterface::add_11bit_driver(CANSensor *sensor)
+{
+    if (aux_11bit_driver != nullptr) {
+        // only allow one
+        return false;
+    }
+    aux_11bit_driver = sensor;
+    return true;
+}
+
+// handler for outgoing frames for auxillary drivers
+bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us)
+{
+    bool ret = false;
+    for (uint8_t iface = 0; iface < num_ifaces; iface++) {
+        if (ifaces[iface] == NULL) {
+            continue;
+        }
+        ret |= ifaces[iface]->send(out_frame, timeout_us, 0) > 0;
+    }
+    return ret;
+}
+
 #endif // #if HAL_ENABLE_DRONECAN_DRIVERS
diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h
index aa7533e691..ce9b75ee92 100644
--- a/libraries/AP_DroneCAN/AP_Canard_iface.h
+++ b/libraries/AP_DroneCAN/AP_Canard_iface.h
@@ -5,6 +5,8 @@
 #include <dronecan_msgs.h>
 
 class AP_DroneCAN;
+class CANSensor;
+
 class CanardInterface : public Canard::Interface {
     friend class AP_DroneCAN;
 public:
@@ -48,6 +50,12 @@ public:
 
     bool add_interface(AP_HAL::CANIface *can_drv);
 
+    // add an auxillary driver for 11 bit frames
+    bool add_11bit_driver(CANSensor *sensor);
+
+    // handler for outgoing frames for auxillary drivers
+    bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us);
+    
 #if AP_TEST_DRONECAN_DRIVERS
     static CanardInterface& get_test_iface() { return test_iface; }
     static void processTestRx();
@@ -70,5 +78,8 @@ private:
     HAL_Semaphore _sem_rx;
     CanardTxTransfer tx_transfer;
     dronecan_protocol_Stats protocol_stats;
+
+    // auxillary 11 bit CANSensor
+    CANSensor *aux_11bit_driver;
 };
-#endif // HAL_ENABLE_DRONECAN_DRIVERS
\ No newline at end of file
+#endif // HAL_ENABLE_DRONECAN_DRIVERS
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
index 55d148bf1f..9f34e7f93b 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -1751,4 +1751,20 @@ void AP_DroneCAN::logging(void)
 #endif // HAL_LOGGING_ENABLED
 }
 
+// add an 11 bit auxillary driver
+bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor)
+{
+    return canard_iface.add_11bit_driver(sensor);
+}
+
+// handler for outgoing frames for auxillary drivers
+bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us)
+{
+    if (out_frame.isExtended()) {
+        // don't allow extended frames to be sent by auxillary driver
+        return false;
+    }
+    return canard_iface.write_aux_frame(out_frame, timeout_us);
+}
+
 #endif // HAL_NUM_CAN_IFACES
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
index f502e05a39..61f0528bc9 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -69,6 +69,7 @@
 
 // fwd-declare callback classes
 class AP_DroneCAN_DNA_Server;
+class CANSensor;
 
 class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     friend class AP_DroneCAN_DNA_Server;
@@ -85,6 +86,12 @@ public:
     void init(uint8_t driver_index, bool enable_filters) override;
     bool add_interface(AP_HAL::CANIface* can_iface) override;
 
+    // add an 11 bit auxillary driver
+    bool add_11bit_driver(CANSensor *sensor) override;
+
+    // handler for outgoing frames for auxillary drivers
+    bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) override;
+    
     uint8_t get_driver_index() const { return _driver_index; }
 
     // define string with length structure