AP_DroneCAN: support CAN serial ports

this allows any serial protocol to be mapped to a remote DroneCAN node

libraries/AP_DroneCAN/AP_DroneCAN.cpp

@@ -51,6 +51,10 @@
 #include <AP_Mount/AP_Mount_Xacti.h>
 #include <string.h>
 
+#if AP_DRONECAN_SERIAL_ENABLED
+#include "AP_DroneCAN_serial.h"
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 // setup default pool size
@@ -149,6 +153,105 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0),
 
+#if AP_DRONECAN_SERIAL_ENABLED
+    /*
+      due to the parameter tree depth limitation we can't use a sub-table for the serial parameters
+     */
+
+    // @Param: SER_EN
+    // @DisplayName: DroneCAN Serial enable
+    // @Description: Enable DroneCAN virtual serial ports
+    // @Values: 0:Disabled, 1:Enabled
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("SER_EN", 10,  AP_DroneCAN, serial.enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: S1_NOD
+    // @DisplayName: Serial CAN remote node number
+    // @Description: CAN remote node number for serial port
+    // @Range: 0 127
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("S1_NOD", 11,  AP_DroneCAN, serial.ports[0].node, 0),
+
+    // @Param: S1_IDX
+    // @DisplayName: DroneCAN Serial1 index
+    // @Description: Serial port number on remote CAN node
+    // @Range: 0 100
+    // @Values: -1:Disabled,0:Serial0,1:Serial1,2:Serial2,3:Serial3,4:Serial4,5:Serial5,6:Serial6
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("S1_IDX", 12,  AP_DroneCAN, serial.ports[0].idx, -1),
+
+    // @Param: S1_BD
+    // @DisplayName: DroneCAN Serial default baud rate
+    // @Description: Serial baud rate on remote CAN node
+    // @CopyFieldsFrom: SERIAL1_BAUD
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("S1_BD", 13,  AP_DroneCAN, serial.ports[0].state.baud, 57600),
+
+    // @Param: S1_PRO
+    // @DisplayName: Serial protocol of DroneCAN serial port
+    // @Description: Serial protocol of DroneCAN serial port
+    // @CopyFieldsFrom: SERIAL1_PROTOCOL
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("S1_PRO", 14,  AP_DroneCAN, serial.ports[0].state.protocol, -1),
+
+#if AP_DRONECAN_SERIAL_NUM_PORTS > 1
+    // @Param: S2_NOD
+    // @DisplayName: Serial CAN remote node number
+    // @Description: CAN remote node number for serial port
+    // @CopyFieldsFrom: CAN_D1_UC_S1_NOD
+    AP_GROUPINFO("S2_NOD", 15,  AP_DroneCAN, serial.ports[1].node, 0),
+
+    // @Param: S2_IDX
+    // @DisplayName: Serial port number on remote CAN node
+    // @Description: Serial port number on remote CAN node
+    // @CopyFieldsFrom: CAN_D1_UC_S1_IDX
+    AP_GROUPINFO("S2_IDX", 16,  AP_DroneCAN, serial.ports[1].idx, -1),
+
+    // @Param: S2_BD
+    // @DisplayName: DroneCAN Serial default baud rate
+    // @Description: Serial baud rate on remote CAN node
+    // @CopyFieldsFrom: CAN_D1_UC_S1_BD
+    AP_GROUPINFO("S2_BD", 17,  AP_DroneCAN, serial.ports[1].state.baud, 57600),
+
+    // @Param: S2_PRO
+    // @DisplayName: Serial protocol of DroneCAN serial port
+    // @Description: Serial protocol of DroneCAN serial port
+    // @CopyFieldsFrom: CAN_D1_UC_S1_PRO
+    AP_GROUPINFO("S2_PRO", 18,  AP_DroneCAN, serial.ports[1].state.protocol, -1),
+#endif
+
+#if AP_DRONECAN_SERIAL_NUM_PORTS > 2
+    // @Param: S3_NOD
+    // @DisplayName: Serial CAN remote node number
+    // @Description: CAN node number for serial port
+    // @CopyFieldsFrom: CAN_D1_UC_S1_NOD
+    AP_GROUPINFO("S3_NOD", 19,  AP_DroneCAN, serial.ports[2].node, 0),
+
+    // @Param: S3_IDX
+    // @DisplayName: Serial port number on remote CAN node
+    // @Description: Serial port number on remote CAN node
+    // @CopyFieldsFrom: CAN_D1_UC_S1_IDX
+    AP_GROUPINFO("S3_IDX", 20,  AP_DroneCAN, serial.ports[2].idx, 0),
+
+    // @Param: S3_BD
+    // @DisplayName: Serial baud rate on remote CAN node
+    // @Description: Serial baud rate on remote CAN node
+    // @CopyFieldsFrom: CAN_D1_UC_S1_BD
+    AP_GROUPINFO("S3_BD", 21,  AP_DroneCAN, serial.ports[2].state.baud, 57600),
+
+    // @Param: S3_PRO
+    // @DisplayName: Serial protocol of DroneCAN serial port
+    // @Description: Serial protocol of DroneCAN serial port
+    // @CopyFieldsFrom: CAN_D1_UC_S1_PRO
+    AP_GROUPINFO("S3_PRO", 22,  AP_DroneCAN, serial.ports[2].state.protocol, -1),
+#endif
+#endif // AP_DRONECAN_SERIAL_ENABLED
+
     AP_GROUPEND
 };
 
@@ -369,6 +472,10 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
         return;
     }
 
+#if AP_DRONECAN_SERIAL_ENABLED
+    serial.init(this);
+#endif
+
     _initialized = true;
     debug_dronecan(AP_CANManager::LOG_INFO, "DroneCAN: init done\n\r");
 }
@@ -421,6 +528,10 @@ void AP_DroneCAN::loop(void)
                 }
             }
         }
+
+#if AP_DRONECAN_SERIAL_ENABLED
+        serial.update();
+#endif
     }
 }
 

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -34,6 +34,7 @@
 #include "AP_DroneCAN_DNA_Server.h"
 #include <canard.h>
 #include <dronecan_msgs.h>
+#include <AP_SerialManager/AP_SerialManager_config.h>
 
 #ifndef DRONECAN_SRV_NUMBER
 #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS
@@ -58,6 +59,14 @@
 #define AP_DRONECAN_HIMARK_SERVO_SUPPORT (BOARD_FLASH_SIZE>1024)
 #endif
 
+#ifndef AP_DRONECAN_SERIAL_ENABLED
+#define AP_DRONECAN_SERIAL_ENABLED AP_SERIALMANAGER_REGISTER_ENABLED && (BOARD_FLASH_SIZE>1024)
+#endif
+
+#if AP_DRONECAN_SERIAL_ENABLED
+#include "AP_DroneCAN_serial.h"
+#endif
+
 // fwd-declare callback classes
 class AP_DroneCAN_DNA_Server;
 
@@ -258,6 +267,10 @@ private:
 
     CanardInterface canard_iface;
 
+#if AP_DRONECAN_SERIAL_ENABLED
+    AP_DroneCAN_Serial serial;
+#endif
+
     Canard::Publisher<uavcan_protocol_NodeStatus> node_status{canard_iface};
     Canard::Publisher<dronecan_protocol_CanStats> can_stats{canard_iface};
     Canard::Publisher<dronecan_protocol_Stats> protocol_stats{canard_iface};

libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp

@@ -0,0 +1,224 @@
+/*
+  map local serial ports to remote DroneCAN serial ports using the
+  TUNNEL_TARGETTED message
+ */
+
+#include "AP_DroneCAN.h"
+
+#if HAL_ENABLE_DRONECAN_DRIVERS && AP_DRONECAN_SERIAL_ENABLED
+
+#include <AP_Math/AP_Math.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+AP_DroneCAN_Serial *AP_DroneCAN_Serial::serial[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+
+#ifndef AP_DRONECAN_SERIAL_MIN_TXSIZE
+#define AP_DRONECAN_SERIAL_MIN_TXSIZE 2048
+#endif
+
+#ifndef AP_DRONECAN_SERIAL_MIN_RXSIZE
+#define AP_DRONECAN_SERIAL_MIN_RXSIZE 2048
+#endif
+
+/*
+  initialise DroneCAN serial aports
+*/
+void AP_DroneCAN_Serial::init(AP_DroneCAN *_dronecan)
+{
+    if (enable == 0) {
+        return;
+    }
+    const uint8_t driver_index = _dronecan->get_driver_index();
+    if (driver_index >= ARRAY_SIZE(serial)) {
+        return;
+    }
+    serial[driver_index] = this;
+    dronecan = _dronecan;
+
+    const uint8_t base_port = driver_index == 0? AP_SERIALMANAGER_CAN_D1_PORT_1 : AP_SERIALMANAGER_CAN_D2_PORT_1;
+    bool need_subscriber = false;
+
+    for (uint8_t i=0; i<ARRAY_SIZE(ports); i++) {
+        auto &p = ports[i];
+        p.state.idx = base_port + i;
+        if (p.node > 0 && p.idx >= 0) {
+            p.init();
+            AP::serialmanager().register_port(&p);
+            need_subscriber = true;
+        }
+    }
+
+    if (need_subscriber) {
+        if (Canard::allocate_sub_arg_callback(dronecan, &handle_tunnel_targetted, dronecan->get_driver_index()) == nullptr) {
+            AP_BoardConfig::allocation_error("serial_tunnel_sub");
+        }
+        targetted = new Canard::Publisher<uavcan_tunnel_Targetted>(dronecan->get_canard_iface());
+        if (targetted == nullptr) {
+            AP_BoardConfig::allocation_error("serial_tunnel_pub");
+        }
+        targetted->set_timeout_ms(20);
+        targetted->set_priority(CANARD_TRANSFER_PRIORITY_MEDIUM);
+    }
+}
+
+/*
+  update DroneCAN serial ports
+*/
+void AP_DroneCAN_Serial::update(void)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    for (auto &p : ports) {
+        if (p.baudrate == 0) {
+            continue;
+        }
+        if (p.writebuffer == nullptr || p.node <= 0 || p.idx < 0) {
+            continue;
+        }
+        WITH_SEMAPHORE(p.sem);
+        uint32_t avail;
+        const bool send_keepalive = now_ms - p.last_send_ms > 500;
+        const auto *ptr = p.writebuffer->readptr(avail);
+        if (!send_keepalive && (ptr == nullptr || avail <= 0)) {
+            continue;
+        }
+        uavcan_tunnel_Targetted pkt {};
+        auto n = MIN(avail, sizeof(pkt.buffer.data));
+        pkt.target_node = p.node;
+        pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_UNDEFINED;
+        pkt.buffer.len = n;
+        pkt.baudrate = p.baudrate;
+        pkt.serial_id = p.idx;
+        pkt.options = UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT;
+        if (ptr != nullptr) {
+            memcpy(pkt.buffer.data, ptr, n);
+        }
+
+        if (targetted->broadcast(pkt)) {
+            p.writebuffer->advance(n);
+            p.last_send_ms = now_ms;
+        }
+    }
+}
+
+/*
+  handle incoming tunnel serial packet
+ */
+void AP_DroneCAN_Serial::handle_tunnel_targetted(AP_DroneCAN *dronecan,
+                                                 const CanardRxTransfer& transfer,
+                                                 const uavcan_tunnel_Targetted &msg)
+{
+    uint8_t driver_index = dronecan->get_driver_index();
+    if (driver_index >= ARRAY_SIZE(serial) || serial[driver_index] == nullptr) {
+        return;
+    }
+    auto &s = *serial[driver_index];
+    for (auto &p : s.ports) {
+        if (p.idx == msg.serial_id && transfer.source_node_id == p.node) {
+            WITH_SEMAPHORE(p.sem);
+            if (p.readbuffer != nullptr) {
+                p.readbuffer->write(msg.buffer.data, msg.buffer.len);
+                p.last_recv_us = AP_HAL::micros64();
+            }
+            break;
+        }
+    }
+}
+
+/*
+  initialise port
+ */
+void AP_DroneCAN_Serial::Port::init(void)
+{
+    baudrate = state.baud;
+    begin(baudrate, 0, 0);
+}
+
+/*
+  available space in outgoing buffer
+ */
+uint32_t AP_DroneCAN_Serial::Port::txspace(void)
+{
+    WITH_SEMAPHORE(sem);
+    return writebuffer != nullptr ? writebuffer->space() : 0;
+}
+
+void AP_DroneCAN_Serial::Port::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
+{
+    rxS = MAX(rxS, AP_DRONECAN_SERIAL_MIN_RXSIZE);
+    txS = MAX(txS, AP_DRONECAN_SERIAL_MIN_TXSIZE);
+    init_buffers(rxS, txS);
+    if (b != 0) {
+        baudrate = b;
+    }
+}
+
+size_t AP_DroneCAN_Serial::Port::_write(const uint8_t *buffer, size_t size)
+{
+    WITH_SEMAPHORE(sem);
+    return writebuffer != nullptr ? writebuffer->write(buffer, size) : 0;
+}
+
+ssize_t AP_DroneCAN_Serial::Port::_read(uint8_t *buffer, uint16_t count)
+{
+    WITH_SEMAPHORE(sem);
+    return readbuffer != nullptr ? readbuffer->read(buffer, count) : -1;
+}
+
+uint32_t AP_DroneCAN_Serial::Port::_available()
+{
+    WITH_SEMAPHORE(sem);
+    return readbuffer != nullptr ? readbuffer->available() : 0;
+}
+
+
+bool AP_DroneCAN_Serial::Port::_discard_input()
+{
+    WITH_SEMAPHORE(sem);
+    if (readbuffer != nullptr) {
+        readbuffer->clear();
+    }
+    return true;
+}
+
+/*
+  initialise read/write buffers
+ */
+bool AP_DroneCAN_Serial::Port::init_buffers(const uint32_t size_rx, const uint32_t size_tx)
+{
+    if (size_tx == last_size_tx &&
+        size_rx == last_size_rx) {
+        return true;
+    }
+    WITH_SEMAPHORE(sem);
+    if (readbuffer == nullptr) {
+        readbuffer = new ByteBuffer(size_rx);
+    } else {
+        readbuffer->set_size_best(size_rx);
+    }
+    if (writebuffer == nullptr) {
+        writebuffer = new ByteBuffer(size_tx);
+    } else {
+        writebuffer->set_size_best(size_tx);
+    }
+    last_size_rx = size_rx;
+    last_size_tx = size_tx;
+    return readbuffer != nullptr && writebuffer != nullptr;
+}
+
+/*
+  return timestamp estimate in microseconds for when the start of
+  a nbytes packet arrived on the uart.
+*/
+uint64_t AP_DroneCAN_Serial::Port::receive_time_constraint_us(uint16_t nbytes)
+{
+    WITH_SEMAPHORE(sem);
+    uint64_t last_receive_us = last_recv_us;
+    if (baudrate > 0) {
+        // assume 10 bits per byte. 
+        uint32_t transport_time_us = (1000000UL * 10UL / baudrate) * (nbytes+available());
+        last_receive_us -= transport_time_us;
+    }
+    return last_receive_us;
+}
+
+#endif // HAL_ENABLE_DRONECAN_DRIVERS && AP_DRONECAN_SERIAL_ENABLED

libraries/AP_DroneCAN/AP_DroneCAN_serial.h

@@ -0,0 +1,75 @@
+#pragma once
+
+#include <AP_SerialManager/AP_SerialManager.h>
+
+#ifndef AP_DRONECAN_SERIAL_NUM_PORTS
+#define AP_DRONECAN_SERIAL_NUM_PORTS 3
+#endif
+
+class AP_DroneCAN;
+
+class AP_DroneCAN_Serial
+{
+public:
+    /* Do not allow copies */
+    CLASS_NO_COPY(AP_DroneCAN_Serial);
+
+    AP_DroneCAN_Serial() {}
+
+    AP_Int8 enable;
+
+    void init(AP_DroneCAN *dronecan);
+    void update(void);
+
+public:
+    class Port : public AP_SerialManager::RegisteredPort {
+    public:
+        friend class AP_DroneCAN_Serial;
+        void init(void);
+
+        AP_Int8 node;
+        AP_Int8 idx;
+
+    private:
+        bool is_initialized() override {
+            return true;
+        }
+        bool tx_pending() override {
+            return false;
+        }
+
+        bool init_buffers(const uint32_t size_rx, const uint32_t size_tx);
+
+        uint32_t txspace() override;
+        void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
+        size_t _write(const uint8_t *buffer, size_t size) override;
+        ssize_t _read(uint8_t *buffer, uint16_t count) override;
+        uint32_t _available() override;
+        void _end() override {}
+        void _flush() override {}
+        bool _discard_input() override;
+        uint64_t receive_time_constraint_us(uint16_t nbytes) override;
+
+        ByteBuffer *readbuffer;
+        ByteBuffer *writebuffer;
+        uint32_t baudrate;
+        uint32_t last_send_ms;
+        uint32_t last_size_tx;
+        uint32_t last_size_rx;
+        uint64_t last_recv_us;
+
+        HAL_Semaphore sem;
+    };
+
+    Port ports[AP_DRONECAN_SERIAL_NUM_PORTS];
+
+private:
+    AP_DroneCAN *dronecan;
+
+    Canard::Publisher<uavcan_tunnel_Targetted> *targetted;
+    static void handle_tunnel_targetted(AP_DroneCAN *dronecan,
+                                        const CanardRxTransfer& transfer,
+                                        const uavcan_tunnel_Targetted &msg);
+
+    static AP_DroneCAN_Serial *serial[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+};