AP_DroneCAN: support an aux 11 bit protocol with DroneCAN

ArduPilot · Nov 28, 2023 · ad59f6d · ad59f6d
1 parent d17a1ca
commit ad59f6d
Show file tree

Hide file tree

Showing 4 changed files with 70 additions and 1 deletion.
diff --git 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
@@ -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 @@ class CanardInterface : public Canard::Interface {
 
     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 @@ class CanardInterface : public Canard::Interface {
     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
+#endif // HAL_ENABLE_DRONECAN_DRIVERS
diff --git 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
@@ -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 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     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