initial DroneCAN support

.gitignore

@@ -1 +1 @@
-generated/
+*generated/

.gitmodules

@@ -7,3 +7,12 @@
 [submodule "modules/mavlink"]
 	path = modules/mavlink
 	url = https://github.com/ArduPilot/mavlink.git
+[submodule "modules/DSDL"]
+	path = modules/DSDL
+	url = https://github.com/dronecan/DSDL.git
+[submodule "modules/libcanard"]
+	path = modules/libcanard
+	url = https://github.com/dronecan/libcanard.git
+[submodule "modules/dronecan_dsdlc"]
+	path = modules/dronecan_dsdlc
+	url = https://github.com/dronecan/dronecan_dsdlc.git

RemoteIDModule/CANDriver.cpp

@@ -0,0 +1,253 @@
+/*
+  CAN driver class for ESP32
+ */
+#include <Arduino.h>
+#include "CANDriver.h"
+
+#include <xtensa/hal.h>
+#include <freertos/FreeRTOS.h>
+#include "freertos/task.h"
+#include "freertos/queue.h"
+#include "freertos/semphr.h"
+#include "esp_err.h"
+#include "esp_log.h"
+#include "driver/twai.h"
+
+#define CAN1_TX_IRQ_Handler      ESP32_CAN1_TX_HANDLER
+#define CAN1_RX0_IRQ_Handler     ESP32_CAN1_RX0_HANDLER
+#define CAN1_RX1_IRQ_Handler     ESP32_CAN1_RX1_HANDLER
+#define CAN2_TX_IRQ_Handler      ESP32_CAN2_TX_HANDLER
+#define CAN2_RX0_IRQ_Handler     ESP32_CAN2_RX0_HANDLER
+#define CAN2_RX1_IRQ_Handler     ESP32_CAN2_RX1_HANDLER
+
+// from canard.h
+#define CANARD_CAN_FRAME_EFF                        (1UL << 31U)         ///< Extended frame format
+#define CANARD_CAN_FRAME_RTR                        (1UL << 30U)         ///< Remote transmission (not used by UAVCAN)
+#define CANARD_CAN_FRAME_ERR                        (1UL << 29U)         ///< Error frame (not used by UAVCAN)
+
+// constructor
+CANDriver::CANDriver()
+{}
+
+void CANDriver::init(uint32_t bitrate)
+{
+    init_bus(bitrate);
+}
+
+#define TX_GPIO_NUM           GPIO_NUM_47
+#define RX_GPIO_NUM           GPIO_NUM_38
+static const twai_general_config_t g_config =                      {.mode = TWAI_MODE_NORMAL, .tx_io = TX_GPIO_NUM, .rx_io = RX_GPIO_NUM,        \
+                                                                    .clkout_io = TWAI_IO_UNUSED, .bus_off_io = TWAI_IO_UNUSED,      \
+                                                                    .tx_queue_len = 5, .rx_queue_len = 5,                           \
+                                                                    .alerts_enabled = TWAI_ALERT_NONE,  .clkout_divider = 0,        \
+                                                                    .intr_flags = ESP_INTR_FLAG_LEVEL2};
+
+static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();
+static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
+
+void CANDriver::init_once(bool enable_irq)
+{
+    if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK)
+    {
+        Serial.printf("CAN/TWAI Driver installed\n");
+    }
+    else
+    {
+        Serial.printf("Failed to install CAN/TWAI driver\n");
+        return;
+    }
+
+    //Reconfigure alerts to detect rx-related stuff only...
+    uint32_t alerts_to_enable = TWAI_ALERT_RX_DATA | TWAI_ALERT_RX_QUEUE_FULL;
+    if (twai_reconfigure_alerts(alerts_to_enable, NULL) == ESP_OK) {
+        Serial.printf("CAN/TWAI Alerts reconfigured\n");
+    } else {
+        Serial.printf("Failed to reconfigure CAN/TWAI alerts");
+    }
+    
+     //Start TWAI driver
+    if (twai_start() == ESP_OK)
+    {
+        Serial.printf("CAN/TWAI Driver started\n");
+    }
+    else
+    {
+        Serial.printf("Failed to start CAN/TWAI driver\n");
+        return;
+    }
+}
+
+bool CANDriver::init_bus(const uint32_t _bitrate)
+{
+    bitrate = _bitrate;
+    init_once(true);
+
+    Timings timings;
+    if (!computeTimings(bitrate, timings)) {
+        return false;
+    }
+    Serial.printf("Timings: presc=%u sjw=%u bs1=%u bs2=%u",
+                  unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2));
+    return true;
+}
+
+bool CANDriver::computeTimings(uint32_t target_bitrate, Timings& out_timings)
+{
+    if (target_bitrate < 1) {
+        return false;
+    }
+
+    /*
+     * Hardware configuration
+     */
+    const uint32_t pclk = 100000;
+
+    static const int MaxBS1 = 16;
+    static const int MaxBS2 = 8;
+
+    /*
+     * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG
+     *      CAN in Automation, 2003
+     *
+     * According to the source, optimal quanta per bit are:
+     *   Bitrate        Optimal Maximum
+     *   1000 kbps      8       10
+     *   500  kbps      16      17
+     *   250  kbps      16      17
+     *   125  kbps      16      17
+     */
+    const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;
+
+    static const int MaxSamplePointLocation = 900;
+
+    /*
+     * Computing (prescaler * BS):
+     *   BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2))       -- See the Reference Manual
+     *   BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2))                 -- Simplified
+     * let:
+     *   BS = 1 + BS1 + BS2                                             -- Number of time quanta per bit
+     *   PRESCALER_BS = PRESCALER * BS
+     * ==>
+     *   PRESCALER_BS = PCLK / BITRATE
+     */
+    const uint32_t prescaler_bs = pclk / target_bitrate;
+
+    /*
+     * Searching for such prescaler value so that the number of quanta per bit is highest.
+     */
+    uint8_t bs1_bs2_sum = uint8_t(max_quanta_per_bit - 1);
+
+    while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) {
+        if (bs1_bs2_sum <= 2) {
+            return false;          // No solution
+        }
+        bs1_bs2_sum--;
+    }
+
+    const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
+    if ((prescaler < 1U) || (prescaler > 1024U)) {
+        return false;              // No solution
+    }
+
+    /*
+     * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum.
+     * We need to find the values so that the sample point is as close as possible to the optimal value.
+     *
+     *   Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2]  (* Where 7/8 is 0.875, the recommended sample point location *)
+     *   {{bs2 -> (1 + bs1)/7}}
+     *
+     * Hence:
+     *   bs2 = (1 + bs1) / 7
+     *   bs1 = (7 * bs1_bs2_sum - 1) / 8
+     *
+     * Sample point location can be computed as follows:
+     *   Sample point location = (1 + bs1) / (1 + bs1 + bs2)
+     *
+     * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one:
+     *   - With rounding to nearest
+     *   - With rounding to zero
+     */
+    struct BsPair {
+        uint8_t bs1;
+        uint8_t bs2;
+        uint16_t sample_point_permill;
+
+        BsPair() :
+            bs1(0),
+            bs2(0),
+            sample_point_permill(0)
+        { }
+
+        BsPair(uint8_t bs1_bs2_sum, uint8_t arg_bs1) :
+            bs1(arg_bs1),
+            bs2(uint8_t(bs1_bs2_sum - bs1)),
+            sample_point_permill(uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
+        {}
+
+        bool isValid() const
+        {
+            return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2);
+        }
+    };
+
+    // First attempt with rounding to nearest
+    BsPair solution(bs1_bs2_sum, uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8));
+
+    if (solution.sample_point_permill > MaxSamplePointLocation) {
+        // Second attempt with rounding to zero
+        solution = BsPair(bs1_bs2_sum, uint8_t((7 * bs1_bs2_sum - 1) / 8));
+    }
+
+    if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) {
+        return false;
+    }
+
+    Serial.printf("Timings: quanta/bit: %d, sample point location: %.1f%%",
+                  int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F);
+
+    out_timings.prescaler = uint16_t(prescaler - 1U);
+    out_timings.sjw = 0;                                        // Which means one
+    out_timings.bs1 = uint8_t(solution.bs1 - 1);
+    out_timings.bs2 = uint8_t(solution.bs2 - 1);
+    return true;
+}
+
+bool CANDriver::send(const CANFrame &frame)
+{
+    if (frame.isErrorFrame() || frame.dlc > 8) {
+        return false;
+    }
+
+    twai_message_t message {};
+    message.identifier = frame.id;
+    message.extd = frame.isExtended() ? 1 : 0;
+    message.data_length_code = frame.dlc;
+    memcpy(message.data, frame.data, 8);
+
+    esp_err_t sts = twai_transmit(&message, portMAX_DELAY);
+    ESP_ERROR_CHECK(sts);
+
+    return (sts == ESP_OK);
+}
+
+bool CANDriver::receive(CANFrame &out_frame)
+{
+#define MAX_RECV_MSGS_PER_SEC 200
+
+    twai_message_t message {};
+    esp_err_t recverr = twai_receive(&message, pdMS_TO_TICKS(1000/MAX_RECV_MSGS_PER_SEC));
+    if (recverr != ESP_OK) {
+        return false;
+    }
+
+    memcpy(out_frame.data, message.data, 8);// copy new data
+    out_frame.dlc = message.data_length_code;
+    out_frame.id = message.identifier;
+    if (message.extd) {
+        out_frame.id |= CANARD_CAN_FRAME_EFF;
+    }
+    if (out_frame.id & CANFrame::FlagERR) { // same as a message.isErrorFrame() if done later.
+        return false;
+    }
+    return true;
+}

RemoteIDModule/CANDriver.h

@@ -0,0 +1,96 @@
+
+struct CANFrame;
+
+class CANDriver {
+public:
+    CANDriver();
+    void init(uint32_t bitrate);
+
+    bool send(const CANFrame &frame);
+    bool receive(CANFrame &out_frame);
+    
+private:
+    struct Timings {
+        uint16_t prescaler;
+        uint8_t sjw;
+        uint8_t bs1;
+        uint8_t bs2;
+
+        Timings()
+            : prescaler(0)
+            , sjw(0)
+            , bs1(0)
+            , bs2(0)
+        { }
+    };
+
+    bool init_bus(const uint32_t bitrate);
+    void init_once(bool enable_irq);
+    bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
+
+    uint32_t bitrate;
+};
+
+/**
+ * Raw CAN frame, as passed to/from the CAN driver.
+ */
+struct CANFrame {
+    static const uint32_t MaskStdID = 0x000007FFU;
+    static const uint32_t MaskExtID = 0x1FFFFFFFU;
+    static const uint32_t FlagEFF = 1U << 31;                  ///< Extended frame format
+    static const uint32_t FlagRTR = 1U << 30;                  ///< Remote transmission request
+    static const uint32_t FlagERR = 1U << 29;                  ///< Error frame
+
+    static const uint8_t NonFDCANMaxDataLen = 8;
+    static const uint8_t MaxDataLen = 8;
+    uint32_t id;                ///< CAN ID with flags (above)
+    union {
+        uint8_t data[MaxDataLen];
+        uint32_t data_32[MaxDataLen/4];
+    };
+    uint8_t dlc;                ///< Data Length Code
+
+CANFrame() :
+    id(0),
+        dlc(0)
+        {
+            memset(data,0, MaxDataLen);
+        }
+
+    CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false);
+
+    bool operator!=(const CANFrame& rhs) const
+    {
+        return !operator==(rhs);
+    }
+    bool operator==(const CANFrame& rhs) const
+    {
+        return (id == rhs.id) && (dlc == rhs.dlc) && (memcmp(data, rhs.data, dlc) == 0);
+    }
+
+    bool isExtended()                  const
+    {
+        return id & FlagEFF;
+    }
+    bool isRemoteTransmissionRequest() const
+    {
+        return id & FlagRTR;
+    }
+    bool isErrorFrame()                const
+    {
+        return id & FlagERR;
+    }
+
+    static uint8_t dlcToDataLength(uint8_t dlc);
+    static uint8_t dataLengthToDlc(uint8_t data_length);
+    /**
+     * CAN frame arbitration rules, particularly STD vs EXT:
+     *     Marco Di Natale - "Understanding and using the Controller Area Network"
+     *     http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf
+     */
+    bool priorityHigherThan(const CANFrame& rhs) const;
+    bool priorityLowerThan(const CANFrame& rhs) const
+    {
+        return rhs.priorityHigherThan(*this);
+    }
+};

RemoteIDModule/DroneCAN.cpp

@@ -0,0 +1,403 @@
+/*
+  DroneCAN class for handling OpenDroneID messages
+ */
+#include <Arduino.h>
+#include <time.h>
+#include "DroneCAN.h"
+#include <canard.h>
+#include <uavcan.protocol.NodeStatus.h>
+#include <uavcan.protocol.GetNodeInfo.h>
+#include <uavcan.protocol.RestartNode.h>
+#include <uavcan.protocol.dynamic_node_id.Allocation.h>
+
+#define FW_VERSION_MAJOR 1
+#define FW_VERSION_MINOR 0
+#define BOARD_ID 10001
+#define CAN_APP_NODE_NAME "ArduPilot RemoteIDModule"
+#define CAN_DEFAULT_NODE_ID 0 // use DNA
+
+// constructor
+DroneCAN::DroneCAN()
+{}
+
+static void onTransferReceived_trampoline(CanardInstance* ins, CanardRxTransfer* transfer);
+static bool shouldAcceptTransfer_trampoline(const CanardInstance* ins, uint64_t* out_data_type_signature, uint16_t data_type_id,
+                                            CanardTransferType transfer_type,
+                                            uint8_t source_node_id);
+
+static uavcan_protocol_NodeStatus node_status;
+
+void DroneCAN::init(void)
+{
+    can_driver.init(1000000);
+
+    canardInit(&canard, (uint8_t *)canard_memory_pool, sizeof(canard_memory_pool),
+               onTransferReceived_trampoline, shouldAcceptTransfer_trampoline, NULL);
+#if CAN_DEFAULT_NODE_ID
+    canardSetLocalNodeID(&canard, CAN_DEFAULT_NODE_ID);
+#endif
+    canard.user_reference = (void*)this;
+}
+
+void DroneCAN::update(void)
+{
+    if (do_DNA()) {
+        const uint32_t now_ms = millis();
+        if (now_ms - last_node_status_ms >= 1000) {
+            last_node_status_ms = now_ms;
+            node_status_send();
+        }
+    }
+    processTx();
+    processRx();
+}
+
+void DroneCAN::node_status_send(void)
+{
+    uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
+    node_status.uptime_sec = millis() / 1000U;
+    node_status.vendor_specific_status_code = 0;
+    const uint16_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
+    static uint8_t tx_id;
+
+    Serial.printf("sending NodeStatus len=%u\n", unsigned(len));
+
+    canardBroadcast(&canard,
+                    UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
+                    UAVCAN_PROTOCOL_NODESTATUS_ID,
+                    &tx_id,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    (void*)buffer,
+                    len);
+}
+
+void DroneCAN::onTransferReceived(CanardInstance* ins,
+                                  CanardRxTransfer* transfer)
+{
+    if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) {
+        if (transfer->transfer_type == CanardTransferTypeBroadcast &&
+            transfer->data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
+            handle_allocation_response(ins, transfer);
+        }
+        return;
+    }
+
+    switch (transfer->data_type_id) {
+    case UAVCAN_PROTOCOL_GETNODEINFO_ID:
+        handle_get_node_info(ins, transfer);
+        break;
+    case UAVCAN_PROTOCOL_RESTARTNODE_ID:
+        Serial.printf("RestartNode\n");
+        delay(20);
+        esp_restart();
+        break;
+    defauult:
+        //Serial.printf("reject %u\n", transfer->data_type_id);
+        break;
+    }
+}
+
+bool DroneCAN::shouldAcceptTransfer(const CanardInstance* ins,
+                                    uint64_t* out_data_type_signature,
+                                    uint16_t data_type_id,
+                                    CanardTransferType transfer_type,
+                                    uint8_t source_node_id)
+{
+    if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID &&
+        data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID) {
+        *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE;
+        return true;
+    }
+
+    switch (data_type_id) {
+    case UAVCAN_PROTOCOL_GETNODEINFO_ID:
+        *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE;
+        return true;
+    case UAVCAN_PROTOCOL_RESTARTNODE_ID:
+        *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
+        return true;
+    }
+    //Serial.printf("%u: reject ID 0x%x\n", millis(), data_type_id);
+    return false;
+}
+
+static void onTransferReceived_trampoline(CanardInstance* ins,
+                                          CanardRxTransfer* transfer)
+{
+    DroneCAN *dc = (DroneCAN *)ins->user_reference;
+    dc->onTransferReceived(ins, transfer);
+}
+
+/*
+  see if we want to process this packet
+ */
+static bool shouldAcceptTransfer_trampoline(const CanardInstance* ins,
+                                            uint64_t* out_data_type_signature,
+                                            uint16_t data_type_id,
+                                            CanardTransferType transfer_type,
+                                            uint8_t source_node_id)
+{
+    DroneCAN *dc = (DroneCAN *)ins->user_reference;
+    return dc->shouldAcceptTransfer(ins, out_data_type_signature,
+                                    data_type_id,
+                                    transfer_type,
+                                    source_node_id);
+}
+
+void DroneCAN::processTx(void)
+{
+    for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) {
+        CANFrame txmsg {};
+        txmsg.dlc = CANFrame::dataLengthToDlc(txf->data_len);
+        memcpy(txmsg.data, txf->data, txf->data_len);
+        txmsg.id = (txf->id | CANFrame::FlagEFF);
+
+        // push message with 1s timeout
+        if (can_driver.send(txmsg)) {
+            canardPopTxQueue(&canard);
+            tx_fail_count = 0;
+        } else {
+            Serial.printf("can send fail\n");
+            if (tx_fail_count < 8) {
+                tx_fail_count++;
+            } else {
+                canardPopTxQueue(&canard);
+            }
+            break;
+        }
+    }
+}
+
+void DroneCAN::processRx(void)
+{
+    CANFrame rxmsg;
+    while (can_driver.receive(rxmsg)) {
+        CanardCANFrame rx_frame {};
+        uint64_t timestamp = micros64();
+        rx_frame.data_len = CANFrame::dlcToDataLength(rxmsg.dlc);
+        memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len);
+        rx_frame.id = rxmsg.id;
+        int err = canardHandleRxFrame(&canard, &rx_frame, timestamp);
+#if 0
+        Serial.printf("%u: FX %08x %02x %02x %02x %02x %02x %02x %02x %02x (%u) -> %d\n",
+                      millis(),
+                      rx_frame.id,
+                      rxmsg.data[0], rxmsg.data[1], rxmsg.data[2], rxmsg.data[3],
+                      rxmsg.data[4], rxmsg.data[5], rxmsg.data[6], rxmsg.data[7],
+                      rx_frame.data_len,
+                      err);
+#endif
+    }
+}
+
+CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) :
+        id(can_id)
+{
+    if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) {
+        return;
+    }
+    memcpy(this->data, can_data, data_len);
+    if (data_len <= 8) {
+        dlc = data_len;
+    } else {
+        dlc = 8;
+    }
+}
+
+uint8_t CANFrame::dataLengthToDlc(uint8_t data_length)
+{
+    if (data_length <= 8) {
+        return data_length;
+    } else if (data_length <= 12) {
+        return 9;
+    } else if (data_length <= 16) {
+        return 10;
+    } else if (data_length <= 20) {
+        return 11;
+    } else if (data_length <= 24) {
+        return 12;
+    } else if (data_length <= 32) {
+        return 13;
+    } else if (data_length <= 48) {
+        return 14;
+    }
+    return 15;
+}
+
+
+uint8_t CANFrame::dlcToDataLength(uint8_t dlc)
+{
+    /*
+    Data Length Code      9  10  11  12  13  14  15
+    Number of data bytes 12  16  20  24  32  48  64
+    */
+    if (dlc <= 8) {
+        return dlc;
+    } else if (dlc == 9) {
+        return 12;
+    } else if (dlc == 10) {
+        return 16;
+    } else if (dlc == 11) {
+        return 20;
+    } else if (dlc == 12) {
+        return 24;
+    } else if (dlc == 13) {
+        return 32;
+    } else if (dlc == 14) {
+        return 48;
+    }
+    return 64;
+}
+
+uint64_t DroneCAN::micros64(void)
+{
+    uint32_t us = micros();
+    if (us < last_micros32) {
+        base_micros64 += 0x100000000ULL;
+    }
+    last_micros32 = us;
+    return us + base_micros64;
+}
+
+/*
+  handle a GET_NODE_INFO request
+ */
+void DroneCAN::handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE] {};
+    uavcan_protocol_GetNodeInfoResponse pkt {};
+
+    node_status.uptime_sec = millis() / 1000U;
+
+    pkt.status = node_status;
+    pkt.software_version.major = FW_VERSION_MAJOR;
+    pkt.software_version.minor = FW_VERSION_MINOR;
+    pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT | UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_IMAGE_CRC;
+    pkt.software_version.vcs_commit = 0;
+
+    readUniqueID(pkt.hardware_version.unique_id);
+
+    pkt.hardware_version.major = BOARD_ID >> 8;
+    pkt.hardware_version.minor = BOARD_ID & 0xFF;
+    snprintf((char*)pkt.name.data, sizeof(pkt.name.data), "%s", CAN_APP_NODE_NAME);
+    pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data));
+
+    uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer);
+    canardRequestOrRespond(ins,
+                           transfer->source_node_id,
+                           UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
+                           UAVCAN_PROTOCOL_GETNODEINFO_ID,
+                           &transfer->transfer_id,
+                           transfer->priority,
+                           CanardResponse,
+                           &buffer[0],
+                           total_size);
+}
+
+void DroneCAN::handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    // Rule C - updating the randomized time interval
+    send_next_node_id_allocation_request_at_ms =
+        millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
+        random(1, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
+
+    if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) {
+        node_id_allocation_unique_id_offset = 0;
+        return;
+    }
+
+    // Copying the unique ID from the message
+    uavcan_protocol_dynamic_node_id_Allocation msg;
+
+    uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg);
+
+    // Obtaining the local unique ID
+    uint8_t my_unique_id[sizeof(msg.unique_id.data)] {};
+    readUniqueID(my_unique_id);
+
+    // Matching the received UID against the local one
+    if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) {
+        node_id_allocation_unique_id_offset = 0;
+        return;
+    }
+
+    if (msg.unique_id.len < sizeof(msg.unique_id.data)) {
+        // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout.
+        node_id_allocation_unique_id_offset = msg.unique_id.len;
+        send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS;
+    } else {
+        // Allocation complete - copying the allocated node ID from the message
+        canardSetLocalNodeID(ins, msg.node_id);
+        Serial.printf("Node ID allocated: %u\n", unsigned(msg.node_id));
+    }
+}
+
+bool DroneCAN::do_DNA(void)
+{
+    if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) {
+        return true;
+    }
+    const uint32_t now = millis();
+    if (now - last_DNA_start_ms < 1000 && node_id_allocation_unique_id_offset == 0) {
+        return false;
+    }
+    last_DNA_start_ms = now;
+
+    uint8_t node_id_allocation_transfer_id = 0;
+
+    send_next_node_id_allocation_request_at_ms =
+        now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS +
+        random(1, UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS);
+
+    uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1] {};
+    allocation_request[0] = 0;
+    if (node_id_allocation_unique_id_offset == 0) {
+        allocation_request[0] |= 1;
+    }
+
+    uint8_t my_unique_id[sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data)] {};
+    readUniqueID(my_unique_id);
+
+    static const uint8_t MaxLenOfUniqueIDInRequest = 6;
+    uint8_t uid_size = (uint8_t)(sizeof(uavcan_protocol_dynamic_node_id_Allocation::unique_id.data) - node_id_allocation_unique_id_offset);
+    
+    if (uid_size > MaxLenOfUniqueIDInRequest) {
+        uid_size = MaxLenOfUniqueIDInRequest;
+    }
+
+    memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size);
+
+    // Broadcasting the request
+    static uint8_t tx_id;
+    canardBroadcast(&canard,
+                    UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE,
+                    UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID,
+                    &tx_id,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    &allocation_request[0],
+                    (uint16_t) (uid_size + 1));
+    node_id_allocation_unique_id_offset = 0;
+    return false;
+}
+
+void DroneCAN::readUniqueID(uint8_t id[6])
+{
+    esp_efuse_mac_get_default(id);
+}
+
+#if 0
+// xprintf is useful when debugging in C code such as libcanard
+extern "C" {
+void xprintf(const char *fmt, ...);
+}
+
+void xprintf(const char *fmt, ...)
+{
+    char buffer[200] {};
+    va_list ap;
+    va_start(ap, fmt);
+    uint32_t n = vsnprintf(buffer, sizeof(buffer), fmt, ap);
+    va_end(ap);
+    Serial.printf("%s", buffer);
+}
+#endif

RemoteIDModule/DroneCAN.h

@@ -0,0 +1,48 @@
+
+#include "CANDriver.h"
+#include <canard.h>
+
+#define CAN_POOL_SIZE 4096
+
+class DroneCAN {
+public:
+    DroneCAN();
+    void init(void);
+    void update(void);
+
+private:
+    uint32_t last_node_status_ms;
+    CANDriver can_driver;
+    CanardInstance canard;
+    uint32_t canard_memory_pool[CAN_POOL_SIZE/sizeof(uint32_t)];
+
+    void node_status_send(void);
+
+    uint8_t tx_fail_count;
+
+    void processTx(void);
+    void processRx(void);
+
+    uint64_t micros64();
+    uint64_t base_micros64;
+    uint32_t last_micros32;
+
+    void handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer);
+
+    void readUniqueID(uint8_t id[6]);
+
+    bool do_DNA(void);
+    void handle_allocation_response(CanardInstance* ins, CanardRxTransfer* transfer);
+
+    uint32_t send_next_node_id_allocation_request_at_ms;
+    uint32_t node_id_allocation_unique_id_offset;
+    uint32_t last_DNA_start_ms;
+
+public:
+    void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);
+    bool shouldAcceptTransfer(const CanardInstance* ins,
+                              uint64_t* out_data_type_signature,
+                              uint16_t data_type_id,
+                              CanardTransferType transfer_type,
+                              uint8_t source_node_id);
+};

RemoteIDModule/RemoteIDModule.ino

@@ -14,8 +14,10 @@
 
 #include <id_open.h>
 #include "mavlink.h"
+#include "DroneCAN.h"
 
 static ID_OpenDrone          squitter;
+static DroneCAN              dronecan;
 
 static MAVLinkSerial mavlink{Serial1, MAVLINK_COMM_0};
 
@@ -43,6 +45,7 @@ void setup()
     Serial1.begin(MAVLINK_BAUDRATE, SERIAL_8N1, RX_PIN, TX_PIN);
 
     mavlink.init();
+    dronecan.init();
 }
 
 static void init_squitter(void)
@@ -72,6 +75,7 @@ void loop()
     static uint32_t last_update;
 
     mavlink.update();
+    dronecan.update();
 
     if (!mavlink.initialised()) {
         return;

add_libraries.sh

@@ -7,4 +7,8 @@ mkdir -p "$DEST"
 ln -sf $PWD/modules/uav_electronic_ids/id_open "$DEST"
 ln -sf $PWD/modules/uav_electronic_ids/utm "$DEST"
 ln -sf $PWD/modules/opendroneid-core-c/libopendroneid "$DEST"
-ln -sf $PWD/libraries/mavlink2 ~/Arduino/libraries/ "$DEST"
+ln -sf $PWD/modules/libcanard "$DEST"
+
+ln -sf $PWD/libraries/mavlink2 "$DEST"
+ln -sf $PWD/libraries/DroneCAN "$DEST"
+ln -sf $PWD/libraries/DroneCAN_generated "$DEST"

libraries/mavlink2/regen_mavlink.sh

@@ -1,6 +0,0 @@
-#!/bin/bash
-# re-generate mavlink headers, assumes pymavlink is installed
-
-mavgen.py --wire-protocol 2.0 --lang C ../../modules/mavlink/message_definitions/v1.0/all.xml -o generated
-
-

modules/DSDL

@@ -0,0 +1 @@
+Subproject commit 6070f45c922184c6a84e7d659529721cd2e0c27d

modules/dronecan_dsdlc

@@ -0,0 +1 @@
+Subproject commit 6a252d2e33e88144abda7e28e4ded49465dd9912

modules/libcanard

@@ -0,0 +1 @@
+Subproject commit f646cdd9345c79b2d144056dcce0fd342a3da12f

regen_headers.sh

@@ -0,0 +1,20 @@
+#!/bin/bash
+# re-generate mavlink headers, assumes pymavlink is installed
+
+echo "Generating mavlink2 headers"
+rm -rf libraries/mavlink2/generated
+mavgen.py --wire-protocol 2.0 --lang C modules/mavlink/message_definitions/v1.0/all.xml -o libraries/mavlink2/generated
+
+echo "Generating DroneCAN headers for libcanard"
+rm -rf libraries/DroneCAN_generated
+python3 modules/dronecan_dsdlc/dronecan_dsdlc.py -O libraries/DroneCAN_generated modules/DSDL/uavcan modules/DSDL/dronecan modules/DSDL/com
+
+# cope with horrible Arduino library handling
+PACKETS="NodeStatus GetNodeInfo HardwareVersion SoftwareVersion RestartNode dynamic_node_id"
+for p in $PACKETS; do
+    (
+        cd libraries/DroneCAN_generated
+        ln -s include/*"$p"*.h .
+        ln -s src/*"$p"*.c .
+    )
+done