AP_UAVCAN: move libuavcan to libcanard driver

libraries/AP_UAVCAN/AP_Canard_iface.cpp

@@ -0,0 +1,309 @@
+#include "AP_Canard_iface.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_CANManager/AP_CANManager.h>
+#if HAL_ENABLE_LIBUAVCAN_DRIVERS
+#include <canard/handler_list.h>
+#include <canard/transfer_object.h>
+extern const AP_HAL::HAL& hal;
+#define LOG_TAG "DroneCANIface"
+
+#define DEBUG_PKTS 0
+
+DEFINE_HANDLER_LIST_HEADS();
+DEFINE_TRANSFER_OBJECT_HEADS();
+
+#if AP_TEST_DRONECAN_DRIVERS
+CanardInterface* CanardInterface::canard_ifaces[] = {nullptr, nullptr, nullptr};
+CanardInterface CanardInterface::test_iface{2};
+uint8_t test_node_mem_area[1024];
+HAL_Semaphore test_iface_sem;
+#endif
+
+CanardInterface::CanardInterface(uint8_t iface_index) :
+Interface(iface_index) {
+#if AP_TEST_DRONECAN_DRIVERS
+    if (iface_index < 3) {
+        canard_ifaces[iface_index] = this;
+    }
+    if (iface_index == 0) {
+        test_iface.init(test_node_mem_area, sizeof(test_node_mem_area), 125);
+    }
+#endif
+}
+
+void CanardInterface::init(void* mem_arena, size_t mem_arena_size, uint8_t node_id) {
+    canardInit(&canard, mem_arena, mem_arena_size, onTransferReception, shouldAcceptTransfer, this);
+    canardSetLocalNodeID(&canard, node_id);
+    initialized = true;
+}
+
+bool CanardInterface::broadcast(const Canard::Transfer &bcast_transfer) {
+    if (!initialized) {
+        return false;
+    }
+    WITH_SEMAPHORE(_sem);
+#if AP_TEST_DRONECAN_DRIVERS
+    if (this == &test_iface) {
+        test_iface_sem.take_blocking();
+    }
+#endif
+
+    // do canard broadcast
+    bool success = canardBroadcast(&canard,
+                            bcast_transfer.data_type_signature,
+                            bcast_transfer.data_type_id,
+                            bcast_transfer.inout_transfer_id,
+                            bcast_transfer.priority,
+                            bcast_transfer.payload,
+                            bcast_transfer.payload_len,
+                            AP_HAL::native_micros64() + (bcast_transfer.timeout_ms * 1000)
+#if CANARD_MULTI_IFACE
+                            , ((1<<num_ifaces) - 1) // send over all ifaces
+#endif
+#if HAL_CANFD_SUPPORTED
+                            , bcast_transfer.canfd
+#endif
+            ) > 0;
+#if AP_TEST_DRONECAN_DRIVERS
+    if (this == &test_iface) {
+        test_iface_sem.give();
+    }
+#endif
+    return success;
+}
+
+bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) {
+    if (!initialized) {
+        return false;
+    }
+    WITH_SEMAPHORE(_sem);
+    // do canard request
+    return canardRequestOrRespond(&canard,
+                                    destination_node_id,
+                                    req_transfer.data_type_signature,
+                                    req_transfer.data_type_id,
+                                    req_transfer.inout_transfer_id,
+                                    req_transfer.priority,
+                                    CanardRequest,
+                                    req_transfer.payload,
+                                    req_transfer.payload_len,
+                                    AP_HAL::native_micros64() + (req_transfer.timeout_ms * 1000)
+#if CANARD_MULTI_IFACE
+                    , ((1<<num_ifaces) - 1) // send over all ifaces
+#endif
+#if HAL_CANFD_SUPPORTED
+                    , false
+#endif
+            ) > 0;
+}
+
+bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) {
+    if (!initialized) {
+        return false;
+    }
+    WITH_SEMAPHORE(_sem);
+    // do canard respond
+    return canardRequestOrRespond(&canard,
+                                    destination_node_id,
+                                    res_transfer.data_type_signature,
+                                    res_transfer.data_type_id,
+                                    res_transfer.inout_transfer_id,
+                                    res_transfer.priority,
+                                    CanardResponse,
+                                    res_transfer.payload,
+                                    res_transfer.payload_len,
+                                    AP_HAL::native_micros64() + (res_transfer.timeout_ms * 1000)
+#if CANARD_MULTI_IFACE
+                    , ((1<<num_ifaces) - 1) // send over all ifaces
+#endif
+#if HAL_CANFD_SUPPORTED
+                    , false
+#endif
+            ) > 0;
+}
+
+void CanardInterface::onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer) {
+    CanardInterface* iface = (CanardInterface*) ins->user_reference;
+    iface->handle_message(*transfer);
+}
+
+bool CanardInterface::shouldAcceptTransfer(const CanardInstance* ins,
+                                           uint64_t* out_data_type_signature,
+                                           uint16_t data_type_id,
+                                           CanardTransferType transfer_type,
+                                           uint8_t source_node_id) {
+    CanardInterface* iface = (CanardInterface*) ins->user_reference;
+    return iface->accept_message(data_type_id, *out_data_type_signature);
+}
+
+#if AP_TEST_DRONECAN_DRIVERS
+void CanardInterface::processTestRx() {
+    if (!test_iface.initialized) {
+        return;
+    }
+    WITH_SEMAPHORE(test_iface_sem);
+    for (const CanardCANFrame* txf = canardPeekTxQueue(&test_iface.canard); txf != NULL; txf = canardPeekTxQueue(&test_iface.canard)) {
+        if (canard_ifaces[0]) {
+            canardHandleRxFrame(&canard_ifaces[0]->canard, txf, AP_HAL::native_micros64());   
+        }
+        canardPopTxQueue(&test_iface.canard);
+    }
+}
+#endif
+
+void CanardInterface::processTx() {
+    WITH_SEMAPHORE(_sem);
+
+    for (uint8_t iface = 0; iface < num_ifaces; iface++) {
+        if (ifaces[iface] == NULL) {
+            continue;
+        }
+        auto txq = canard.tx_queue;
+        if (txq == nullptr) {
+            return;
+        }
+        AP_HAL::CANFrame txmsg {};
+        // scan through list of pending transfers
+        while (true) {
+            auto txf = &txq->frame;
+            txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len);
+            memcpy(txmsg.data, txf->data, txf->data_len);
+            txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
+#if HAL_CANFD_SUPPORTED
+            txmsg.canfd = txf->canfd;
+#endif
+            bool write = true;
+            bool read = false;
+            ifaces[iface]->select(read, write, &txmsg, 0);
+            if ((AP_HAL::native_micros64() < txf->deadline_usec) && (txf->iface_mask & (1U<<iface)) && write) {
+                // try sending to interfaces, clearing the mask if we succeed
+                if (ifaces[iface]->send(txmsg, txf->deadline_usec, 0) > 0) {
+                    txf->iface_mask &= ~(1U<<iface);
+                } else {
+                    // if we fail to send then we try sending on next interface
+                    break;
+                }
+            }
+            // look at next transfer
+            txq = txq->next;
+            if (txq == nullptr) {
+                break;
+            }
+        }
+    }
+
+    // purge expired transfers
+    for (const CanardCANFrame* txf = canardPeekTxQueue(&canard); txf != NULL; txf = canardPeekTxQueue(&canard)) {
+        if ((AP_HAL::native_micros64() >= txf->deadline_usec) || (txf->iface_mask == 0)) {
+            canardPopTxQueue(&canard);
+        } else {
+            break;
+        }
+    }
+}
+
+void CanardInterface::processRx() {
+    AP_HAL::CANFrame rxmsg;
+    for (uint8_t i=0; i<num_ifaces; i++) {
+        while(true) {
+            if (ifaces[i] == NULL) {
+                break;
+            }
+            bool read_select = true;
+            bool write_select = false;
+            ifaces[i]->select(read_select, write_select, nullptr, 0);
+            if (!read_select) { // No data pending
+                break;
+            }
+            CanardCANFrame rx_frame {};
+
+            //palToggleLine(HAL_GPIO_PIN_LED);
+            uint64_t timestamp;
+            AP_HAL::CANIface::CanIOFlags flags;
+            if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) {
+                break;
+            }
+            rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc);
+            memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len);
+#if HAL_CANFD_SUPPORTED
+            rx_frame.canfd = rxmsg.canfd;
+#endif
+            rx_frame.id = rxmsg.id;
+#if CANARD_MULTI_IFACE
+            rx_frame.iface_id = i;
+#endif
+            {
+                WITH_SEMAPHORE(_sem);
+
+#if DEBUG_PKTS
+                const int16_t res = 
+#endif
+                canardHandleRxFrame(&canard, &rx_frame, timestamp);
+#if DEBUG_PKTS
+                // hal.console->printf("DTID: %u\n", extractDataType(rx_frame.id));
+                // hal.console->printf("Rx %d, IF%d %lx: ", res, i, rx_frame.id);
+                if (res < 0 &&
+                    res != -CANARD_ERROR_RX_NOT_WANTED &&
+                    res != -CANARD_ERROR_RX_WRONG_ADDRESS) {
+                    hal.console->printf("Rx error %d, IF%d %lx: \n", res, i, rx_frame.id);
+                    // for (uint8_t index = 0; index < rx_frame.data_len; index++) {
+                    //     hal.console->printf("%02x", rx_frame.data[index]);
+                    // }
+                    // hal.console->printf("\n");
+                }
+#endif
+            }
+        }
+    }
+}
+
+void CanardInterface::process(uint32_t duration_ms) {
+#if AP_TEST_DRONECAN_DRIVERS
+    const uint64_t deadline = AP_HAL::micros64() + duration_ms*1000;
+    while (AP_HAL::micros64() < deadline) {
+        processTestRx();
+        hal.scheduler->delay_microseconds(1000);
+    }
+#else
+    const uint64_t deadline = AP_HAL::native_micros64() + duration_ms*1000;
+    while (true) {
+        processRx();
+        processTx();
+        uint64_t now = AP_HAL::native_micros64();
+        if (now < deadline) {
+            _event_handle.wait(deadline - now);
+        } else {
+            break;
+        }
+    }
+#endif
+}
+
+bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface)
+{
+    if (num_ifaces > HAL_NUM_CAN_IFACES) {
+        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Num Ifaces Exceeded\n");
+        return false;
+    }
+    if (can_iface == nullptr) {
+        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface Null\n");
+        return false;
+    }
+    if (ifaces[num_ifaces] != nullptr) {
+        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface already added\n");
+        return false;
+    }
+    ifaces[num_ifaces] = can_iface;
+    if (ifaces[num_ifaces] == nullptr) {
+        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Can't alloc uavcan::iface\n");
+        return false;
+    }
+    if (!can_iface->set_event_handle(&_event_handle)) {
+        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Setting event handle failed\n");
+        return false;
+    }
+    AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "DroneCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces));
+    num_ifaces++;
+    return true;
+}
+#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS

libraries/AP_UAVCAN/AP_Canard_iface.h

@@ -0,0 +1,68 @@
+#pragma once
+#include <AP_HAL/AP_HAL.h>
+#if HAL_ENABLE_LIBUAVCAN_DRIVERS
+#include <canard/interface.h>
+
+class AP_UAVCAN;
+class CanardInterface : public Canard::Interface {
+    friend class AP_UAVCAN;
+public:
+
+    /// @brief delete copy constructor and assignment operator
+    CanardInterface(const CanardInterface&) = delete;
+    CanardInterface& operator=(const CanardInterface&) = delete;
+
+    CanardInterface(uint8_t driver_index);
+
+    void init(void* mem_arena, size_t mem_arena_size, uint8_t node_id);
+
+    /// @brief broadcast message to all listeners on Interface
+    /// @param bc_transfer
+    /// @return true if message was added to the queue
+    bool broadcast(const Canard::Transfer &bcast_transfer) override;
+
+    /// @brief request message from
+    /// @param destination_node_id
+    /// @param req_transfer
+    /// @return true if request was added to the queue
+    bool request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) override;
+
+    /// @brief respond to a request
+    /// @param destination_node_id
+    /// @param res_transfer
+    /// @return true if response was added to the queue
+    bool respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) override;
+
+    void processTx();
+    void processRx();
+
+    void process(uint32_t duration);
+
+    static void onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer);
+    static bool shouldAcceptTransfer(const CanardInstance* ins,
+                                     uint64_t* out_data_type_signature,
+                                     uint16_t data_type_id,
+                                     CanardTransferType transfer_type,
+                                     uint8_t source_node_id);
+
+    bool add_interface(AP_HAL::CANIface *can_drv);
+
+#if AP_TEST_DRONECAN_DRIVERS
+    static CanardInterface& get_test_iface() { return test_iface; }
+    static void processTestRx();
+#endif
+
+    uint8_t get_node_id() const override { return canard.node_id; }
+private:
+    CanardInstance canard;
+    AP_HAL::CANIface* ifaces[HAL_NUM_CAN_IFACES];
+#if AP_TEST_DRONECAN_DRIVERS
+    static CanardInterface* canard_ifaces[3];
+    static CanardInterface test_iface;
+#endif
+    uint8_t num_ifaces;
+    HAL_EventHandle _event_handle;
+    bool initialized;
+    HAL_Semaphore _sem;
+};
+#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -20,17 +20,20 @@
 
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
 
-#include <uavcan/uavcan.hpp>
+#include "AP_Canard_iface.h"
-#include "AP_UAVCAN_IfaceMgr.h"
-#include "AP_UAVCAN_Clock.h"
 #include <AP_CANManager/AP_CANManager.h>
 #include <AP_HAL/Semaphores.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
-#include <uavcan/protocol/param/GetSet.hpp>
-#include <uavcan/protocol/param/ExecuteOpcode.hpp>
 #include <SRV_Channel/SRV_Channel_config.h>
-
+#include <canard/publisher.h>
+#include <canard/subscriber.h>
+#include <canard/service_client.h>
+#include <canard/service_server.h>
+#include <stdio.h>
+#include "AP_UAVCAN_DNA_Server.h"
+#include <canard.h>
+#include <dronecan_msgs.h>
 
 #ifndef UAVCAN_SRV_NUMBER
 #define UAVCAN_SRV_NUMBER NUM_SERVO_CHANNELS
@@ -49,63 +52,12 @@
 #define AP_UAVCAN_MAX_LED_DEVICES 4
 
 // fwd-declare callback classes
-class ButtonCb;
-class TrafficReportCb;
-class ActuatorStatusCb;
-class ActuatorStatusVolzCb;
-class ESCStatusCb;
-class DebugCb;
-class ParamGetSetCb;
-class ParamExecuteOpcodeCb;
-class AP_PoolAllocator;
 class AP_UAVCAN_DNA_Server;
 
-#if defined(__GNUC__) && (__GNUC__ > 8)
-#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
-    _Pragma("GCC diagnostic push") \
-    _Pragma("GCC diagnostic ignored \"-Wcast-function-type\"")
-#define DISABLE_W_CAST_FUNCTION_TYPE_POP \
-    _Pragma("GCC diagnostic pop")
-#else
-#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH
-#define DISABLE_W_CAST_FUNCTION_TYPE_POP
-#endif
-#if defined(__GNUC__) && (__GNUC__ >= 11)
-#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID (void*)
-#else
-#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID
-#endif
-
-/*
-    Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
-    the Callback will invoke registry to register the node as separate backend.
-*/
-#define UC_REGISTRY_BINDER(ClassName_, DataType_) \
-    class ClassName_ : public AP_UAVCAN::RegistryBinder<DataType_> { \
-        typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
-        public: \
-            ClassName_() : RegistryBinder() {} \
-            DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
-            ClassName_(AP_UAVCAN* uc,  CN_Registry ffunc) : \
-                RegistryBinder(uc, (Registry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
-            DISABLE_W_CAST_FUNCTION_TYPE_POP \
-    }
-
-#define UC_CLIENT_CALL_REGISTRY_BINDER(ClassName_, DataType_) \
-    class ClassName_ : public AP_UAVCAN::ClientCallRegistryBinder<DataType_> { \
-        typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
-        public: \
-            ClassName_() : ClientCallRegistryBinder() {} \
-            DISABLE_W_CAST_FUNCTION_TYPE_PUSH \
-            ClassName_(AP_UAVCAN* uc,  CN_Registry ffunc) : \
-                ClientCallRegistryBinder(uc, (ClientCallRegistry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \
-            DISABLE_W_CAST_FUNCTION_TYPE_POP \
-    }
-
 class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     friend class AP_UAVCAN_DNA_Server;
 public:
-    AP_UAVCAN();
+    AP_UAVCAN(const int driver_index);
     ~AP_UAVCAN();
 
     static const struct AP_Param::GroupInfo var_info[];
@@ -117,13 +69,14 @@ public:
     void init(uint8_t driver_index, bool enable_filters) override;
     bool add_interface(AP_HAL::CANIface* can_iface) override;
 
-    uavcan::Node<0>* get_node() { return _node; }
     uint8_t get_driver_index() const { return _driver_index; }
 
     FUNCTOR_TYPEDEF(ParamGetSetIntCb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &);
     FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_UAVCAN*, const uint8_t, const char*, float &);
     FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_UAVCAN*,  const uint8_t, bool);
 
+    void send_node_status();
+
     ///// SRV output /////
     void SRV_push_servos(void);
 
@@ -138,7 +91,7 @@ public:
 
     // Send Reboot command
     // Note: Do not call this from outside UAVCAN thread context,
-    // you can call this from uavcan callbacks and handlers.
+    // you can call this from dronecan callbacks and handlers.
     // THIS IS NOT A THREAD SAFE API!
     void send_reboot_request(uint8_t node_id);
 
@@ -151,57 +104,6 @@ public:
     // Save parameters
     bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb);
 
-    template <typename DataType_>
-    class RegistryBinder {
-    protected:
-        typedef void (*Registry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const RegistryBinder& _cb);
-        AP_UAVCAN* _uc;
-        Registry _ffunc;
-
-    public:
-        RegistryBinder() :
-            _uc(),
-            _ffunc(),
-            msg() {}
-
-        RegistryBinder(AP_UAVCAN* uc, Registry ffunc) :
-            _uc(uc),
-            _ffunc(ffunc),
-            msg(nullptr) {}
-
-        void operator()(const uavcan::ReceivedDataStructure<DataType_>& _msg) {
-            msg = &_msg;
-            _ffunc(_uc, _msg.getSrcNodeID().get(), *this);
-        }
-
-        const uavcan::ReceivedDataStructure<DataType_> *msg;
-    };
-
-    // ClientCallRegistryBinder
-    template <typename DataType_>
-    class ClientCallRegistryBinder {
-    protected:
-        typedef void (*ClientCallRegistry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const ClientCallRegistryBinder& _cb);
-        AP_UAVCAN* _uc;
-        ClientCallRegistry _ffunc;
-    public:
-        ClientCallRegistryBinder() :
-            _uc(),
-            _ffunc(),
-            rsp() {}
-
-        ClientCallRegistryBinder(AP_UAVCAN* uc, ClientCallRegistry ffunc) :
-            _uc(uc),
-            _ffunc(ffunc),
-            rsp(nullptr) {}
-
-        void operator()(const uavcan::ServiceCallResult<DataType_>& _rsp) {
-            rsp = &_rsp;
-            _ffunc(_uc, _rsp.getCallID().server_node_id.get(), *this);
-        }
-        const uavcan::ServiceCallResult<DataType_> *rsp;
-    };
-
     // options bitmask
     enum class Options : uint16_t {
         DNA_CLEAR_DATABASE        = (1U<<0),
@@ -221,9 +123,7 @@ public:
     // 0. return true if it was set
     bool check_and_reset_option(Options option);
 
-    // This will be needed to implement if UAVCAN is used with multithreading
+    CanardInterface& get_canard_iface() { return canard_iface; }
-    // Such cases will be firmware update, etc.
-    class RaiiSynchronizer {};
 
 private:
     void loop(void);
@@ -270,7 +170,7 @@ private:
     uint8_t param_save_request_node_id;
 
     // UAVCAN parameters
-    AP_Int8 _uavcan_node;
+    AP_Int8 _dronecan_node;
     AP_Int32 _servo_bm;
     AP_Int32 _esc_bm;
     AP_Int8 _esc_offset;
@@ -279,14 +179,12 @@ private:
     AP_Int16 _notify_state_hz;
     AP_Int16 _pool_size;
 
-    AP_PoolAllocator *_allocator;
+    uint32_t *mem_pool;
-    AP_UAVCAN_DNA_Server *_dna_server;
 
-    uavcan::Node<0> *_node;
+    AP_UAVCAN_DNA_Server _dna_server;
 
     uint8_t _driver_index;
 
-    uavcan::CanIfaceMgr* _iface_mgr;
     char _thread_name[13];
     bool _initialized;
     ///// SRV output /////
@@ -355,22 +253,80 @@ private:
 
     static HAL_Semaphore _telem_sem;
 
+    // node status send
+    uint32_t _node_status_last_send_ms;
+
     // safety status send state
     uint32_t _last_safety_state_ms;
 
     // notify vehicle state
     uint32_t _last_notify_state_ms;
+    uavcan_protocol_NodeStatus node_status_msg;
+
+    CanardInterface canard_iface;
+    Canard::Publisher<uavcan_protocol_NodeStatus> node_status{canard_iface};
+    Canard::Publisher<uavcan_equipment_actuator_ArrayCommand> act_out_array{canard_iface};
+    Canard::Publisher<uavcan_equipment_esc_RawCommand> esc_raw{canard_iface};
+    Canard::Publisher<uavcan_equipment_indication_LightsCommand> rgb_led{canard_iface};
+    Canard::Publisher<uavcan_equipment_indication_BeepCommand> buzzer{canard_iface};
+    Canard::Publisher<ardupilot_indication_SafetyState> safety_state{canard_iface};
+    Canard::Publisher<uavcan_equipment_safety_ArmingStatus> arming_status{canard_iface};
+    Canard::Publisher<uavcan_equipment_gnss_RTCMStream> rtcm_stream{canard_iface};
+    Canard::Publisher<ardupilot_indication_NotifyState> notify_state{canard_iface};
+
+#if AP_DRONECAN_SEND_GPS
+    Canard::Publisher<uavcan_equipment_gnss_Fix2> gnss_fix2{canard_iface};
+    Canard::Publisher<uavcan_equipment_gnss_Auxiliary> gnss_auxiliary{canard_iface};
+    Canard::Publisher<ardupilot_gnss_Heading> gnss_heading{canard_iface};
+    Canard::Publisher<ardupilot_gnss_Status> gnss_status{canard_iface};
+#endif
+    // incoming messages
+    Canard::ObjCallback<AP_UAVCAN, ardupilot_indication_Button> safety_button_cb{this, &AP_UAVCAN::handle_button};
+    Canard::Subscriber<ardupilot_indication_Button> safety_button_listener{safety_button_cb, _driver_index};
+
+    Canard::ObjCallback<AP_UAVCAN, ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_cb{this, &AP_UAVCAN::handle_traffic_report};
+    Canard::Subscriber<ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_listener{traffic_report_cb, _driver_index};
+
+    Canard::ObjCallback<AP_UAVCAN, uavcan_equipment_actuator_Status> actuator_status_cb{this, &AP_UAVCAN::handle_actuator_status};
+    Canard::Subscriber<uavcan_equipment_actuator_Status> actuator_status_listener{actuator_status_cb, _driver_index};
+
+    Canard::ObjCallback<AP_UAVCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_UAVCAN::handle_ESC_status};
+    Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
+
+    Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_UAVCAN::handle_debug};
+    Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
+
+    // param client
+    Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_param_GetSetResponse> param_get_set_res_cb{this, &AP_UAVCAN::handle_param_get_set_response};
+    Canard::Client<uavcan_protocol_param_GetSetResponse> param_get_set_client{canard_iface, param_get_set_res_cb};
+
+    Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_param_ExecuteOpcodeResponse> param_save_res_cb{this, &AP_UAVCAN::handle_param_save_response};
+    Canard::Client<uavcan_protocol_param_ExecuteOpcodeResponse> param_save_client{canard_iface, param_save_res_cb};
+
+    // reboot client
+    void handle_restart_node_response(const CanardRxTransfer& transfer, const uavcan_protocol_RestartNodeResponse& msg) {}
+    Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_RestartNodeResponse> restart_node_res_cb{this, &AP_UAVCAN::handle_restart_node_response};
+    Canard::Client<uavcan_protocol_RestartNodeResponse> restart_node_client{canard_iface, restart_node_res_cb};
+
+    uavcan_protocol_param_ExecuteOpcodeRequest param_save_req;
+    uavcan_protocol_param_GetSetRequest param_getset_req;
+
+    // Node Info Server
+    Canard::ObjCallback<AP_UAVCAN, uavcan_protocol_GetNodeInfoRequest> node_info_req_cb{this, &AP_UAVCAN::handle_node_info_request};
+    Canard::Server<uavcan_protocol_GetNodeInfoRequest> node_info_server{canard_iface, node_info_req_cb};
+    uavcan_protocol_GetNodeInfoResponse node_info_rsp;
 
     // incoming button handling
-    static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
+    void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
-    static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
+    void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg);
-    static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
+    void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
-    static void handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb);
+    void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
-    static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
+    void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
     static bool is_esc_data_index_valid(const uint8_t index);
-    static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
+    void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
-    static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);
+    void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);
-    static void handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb);
+    void handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp);
+    void handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req);
 };
 
 #endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS

libraries/AP_UAVCAN/AP_UAVCAN_Clock.h

@@ -1,59 +0,0 @@
-/*
- * This file is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * Author: Siddharth Bharat Purohit
- */
-
-
-#pragma once
-
-#include <uavcan/uavcan.hpp>
-
-namespace uavcan
-{
-class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable
-{
-public:
-    SystemClock() = default;
-
-    void adjustUtc(uavcan::UtcDuration adjustment) override
-    {
-        utc_adjustment_usec = adjustment.toUSec();
-    }
-
-    uavcan::MonotonicTime getMonotonic() const override
-    {
-        return uavcan::MonotonicTime::fromUSec(AP_HAL::native_micros64());
-    }
-
-    uavcan::UtcTime getUtc() const override
-    {
-        return uavcan::UtcTime::fromUSec(AP_HAL::native_micros64() + utc_adjustment_usec);
-    }
-
-    int64_t getAdjustUsec() const
-    {
-        return utc_adjustment_usec;
-    }
-
-    static SystemClock& instance()
-    {
-        static SystemClock inst;
-        return inst;
-    }
-
-private:
-    int64_t utc_adjustment_usec;
-};
-}

libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp

@@ -24,11 +24,10 @@
 #include "AP_UAVCAN.h"
 #include <StorageManager/StorageManager.h>
 #include <AP_Math/AP_Math.h>
-#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
-#include "AP_UAVCAN_Clock.h"
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <stdio.h>
 extern const AP_HAL::HAL& hal;
 
 #define NODEDATA_MAGIC 0xAC01
@@ -37,56 +36,15 @@ extern const AP_HAL::HAL& hal;
 
 #define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0)
 
-//Callback Object Definitions
+AP_UAVCAN_DNA_Server::AP_UAVCAN_DNA_Server(AP_UAVCAN &ap_uavcan) :
-UC_REGISTRY_BINDER(AllocationCb, uavcan::protocol::dynamic_node_id::Allocation);
-UC_REGISTRY_BINDER(NodeStatusCb, uavcan::protocol::NodeStatus);
-UC_CLIENT_CALL_REGISTRY_BINDER(GetNodeInfoCb, uavcan::protocol::GetNodeInfo);
-
-static uavcan::ServiceClient<uavcan::protocol::GetNodeInfo, GetNodeInfoCb>* getNodeInfo_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-static uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>* allocation_pub[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-AP_UAVCAN_DNA_Server::AP_UAVCAN_DNA_Server(AP_UAVCAN *ap_uavcan, StorageAccess _storage) :
     _ap_uavcan(ap_uavcan),
-    storage(_storage)
+    _canard_iface(ap_uavcan.canard_iface),
+    storage(StorageManager::StorageCANDNA),
+    allocation_sub(allocation_cb, _ap_uavcan.get_driver_index()),
+    node_status_sub(node_status_cb, _ap_uavcan.get_driver_index()),
+    node_info_client(_canard_iface, node_info_cb)
 {}
 
-/* Subscribe to all the messages we are going to handle for
-Server registry and Node allocation. */
-void AP_UAVCAN_DNA_Server::subscribe_msgs(AP_UAVCAN* ap_uavcan)
-{
-    if (ap_uavcan == nullptr) {
-        return;
-    }
-
-    auto* node = ap_uavcan->get_node();
-    //Register Allocation Message Handler
-    uavcan::Subscriber<uavcan::protocol::dynamic_node_id::Allocation, AllocationCb> *AllocationListener;
-    AllocationListener = new uavcan::Subscriber<uavcan::protocol::dynamic_node_id::Allocation, AllocationCb>(*node);
-    if (AllocationListener == nullptr) {
-        AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: AllocationListener");
-    }
-    const int alloc_listener_res = AllocationListener->start(AllocationCb(ap_uavcan, &trampoline_handleAllocation));
-    if (alloc_listener_res < 0) {
-        AP_HAL::panic("Allocation Subscriber start problem\n\r");
-        return;
-    }
-    //We allow anonymous transfers, as they are specifically for Node Allocation
-    AllocationListener->allowAnonymousTransfers();
-
-    //Register Node Status Listener
-    uavcan::Subscriber<uavcan::protocol::NodeStatus, NodeStatusCb> *NodeStatusListener;
-    NodeStatusListener = new uavcan::Subscriber<uavcan::protocol::NodeStatus, NodeStatusCb>(*node);
-    if (NodeStatusListener == nullptr) {
-        AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: NodeStatusListener");
-    }
-    const int nodestatus_listener_res = NodeStatusListener->start(NodeStatusCb(ap_uavcan, &trampoline_handleNodeStatus));
-    if (nodestatus_listener_res < 0) {
-        AP_HAL::panic("NodeStatus Subscriber start problem\n\r");
-        return;
-    }
-}
-
 /* Method to generate 6byte hash from the Unique ID.
 We return it packed inside the referenced NodeData structure */
 void AP_UAVCAN_DNA_Server::getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const
@@ -255,43 +213,10 @@ bool AP_UAVCAN_DNA_Server::isValidNodeDataAvailable(uint8_t node_id)
 Also resets the Server Record in case there is a mismatch
 between specified node id and unique id against the existing
 Server Record. */
-bool AP_UAVCAN_DNA_Server::init()
+bool AP_UAVCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id)
 {
-    //Read the details from ap_uavcan
+    //Read the details from AP_UAVCAN
-    uavcan::Node<0>* _node = _ap_uavcan->get_node();
-    uint8_t node_id = _node->getNodeID().get();
-    uint8_t own_unique_id[16] = {0};
-    uint8_t own_unique_id_len = 16;
-
-    driver_index = _ap_uavcan->get_driver_index();
-
-    //copy unique id from node to uint8_t array
-    uavcan::copy(_node->getHardwareVersion().unique_id.begin(),
-                 _node->getHardwareVersion().unique_id.end(),
-                 own_unique_id);
-
     server_state = HEALTHY;
-
-    //Setup publisher for this driver index
-    allocation_pub[driver_index] = new uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation>(*_node, true);
-    if (allocation_pub[driver_index] == nullptr) {
-        AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: allocation_pub[%d]", driver_index);
-    }
-
-    int res = allocation_pub[driver_index]->init(uavcan::TransferPriority::Default);
-    if (res < 0) {
-        return false;
-    }
-    allocation_pub[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::dynamic_node_id::Allocation::FOLLOWUP_TIMEOUT_MS));
-
-
-    //Setup GetNodeInfo Client
-    getNodeInfo_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::GetNodeInfo, GetNodeInfoCb>(*_node, GetNodeInfoCb(_ap_uavcan, &trampoline_handleNodeInfo));
-    if (getNodeInfo_client[driver_index] == nullptr) {
-        AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: getNodeInfo_client[%d]", driver_index);
-    }
-
-
     /* Go through our records and look for valid NodeData, to initialise
     occupation mask */
     for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
@@ -310,7 +235,7 @@ bool AP_UAVCAN_DNA_Server::init()
         //Its not there a reset should write it in the Storage
         reset();
     }
-    if (_ap_uavcan->check_and_reset_option(AP_UAVCAN::Options::DNA_CLEAR_DATABASE)) {
+    if (_ap_uavcan.check_and_reset_option(AP_UAVCAN::Options::DNA_CLEAR_DATABASE)) {
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UC DNA database reset");
         reset();
     }
@@ -350,7 +275,7 @@ bool AP_UAVCAN_DNA_Server::init()
     addToSeenNodeMask(node_id);
     setVerificationMask(node_id);
     node_healthy_mask.set(node_id);
-    self_node_id[driver_index] = node_id;
+    self_node_id = node_id;
     return true;
 }
 
@@ -422,7 +347,7 @@ seen list, So that we can raise issue if there are duplicates
 on the bus. */
 void AP_UAVCAN_DNA_Server::verify_nodes()
 {
-    uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec();
+    uint32_t now = AP_HAL::millis();
     if ((now - last_verification_request) < 5000) {
         return;
     }
@@ -435,7 +360,7 @@ void AP_UAVCAN_DNA_Server::verify_nodes()
     
     //Check if we got acknowledgement from previous request
     //except for requests using our own node_id
-    if (curr_verifying_node == self_node_id[driver_index]) {
+    if (curr_verifying_node == self_node_id) {
         nodeInfo_resp_rcvd = true;
     }
 
@@ -456,13 +381,16 @@ void AP_UAVCAN_DNA_Server::verify_nodes()
     //Find the next registered Node ID to be verified.
     for (uint8_t i = 0; i <= MAX_NODE_ID; i++) {
         curr_verifying_node = (curr_verifying_node + 1) % (MAX_NODE_ID + 1);
+        if ((curr_verifying_node == self_node_id) || (curr_verifying_node == 0)) {
+            continue;
+        }
         if (isNodeSeen(curr_verifying_node)) {
             break;
         }
     }
-    if (getNodeInfo_client[driver_index] != nullptr && isNodeIDOccupied(curr_verifying_node)) {
+    if (isNodeIDOccupied(curr_verifying_node)) {
-        uavcan::protocol::GetNodeInfo::Request request;
+        uavcan_protocol_GetNodeInfoRequest request;
-        getNodeInfo_client[driver_index]->call(curr_verifying_node, request);
+        node_info_client.request(curr_verifying_node, request);
         nodeInfo_resp_rcvd = false;
     }
 }
@@ -470,63 +398,50 @@ void AP_UAVCAN_DNA_Server::verify_nodes()
 /* Handles Node Status Message, adds to the Seen Node list
 Also starts the Service call for Node Info to complete the
 Verification process. */
-void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb)
+void AP_UAVCAN_DNA_Server::handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg)
 {
-    if (node_id > MAX_NODE_ID) {
+    if (transfer.source_node_id > MAX_NODE_ID || transfer.source_node_id == 0) {
         return;
     }
-    if ((cb.msg->health != uavcan::protocol::NodeStatus::HEALTH_OK ||
+    if ((msg.health != UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK ||
-        cb.msg->mode != uavcan::protocol::NodeStatus::MODE_OPERATIONAL) &&
+        msg.mode != UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL) &&
-        !_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
+        !_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
         //if node is not healthy or operational, clear resp health mask, and set fault_node_id
-        fault_node_id = node_id;
+        fault_node_id = transfer.source_node_id;
         server_state = NODE_STATUS_UNHEALTHY;
-        node_healthy_mask.clear(node_id);
+        node_healthy_mask.clear(transfer.source_node_id);
     } else {
-        node_healthy_mask.set(node_id);
+        node_healthy_mask.set(transfer.source_node_id);
         if (node_healthy_mask == verified_mask) {
             server_state = HEALTHY;
         }
     }
-    if (!isNodeIDVerified(node_id)) {
+    if (!isNodeIDVerified(transfer.source_node_id)) {
         //immediately begin verification of the node_id
-        if (getNodeInfo_client[driver_index] != nullptr) {
+        uavcan_protocol_GetNodeInfoRequest request;
-            uavcan::protocol::GetNodeInfo::Request request;
+        node_info_client.request(transfer.source_node_id, request);
-            getNodeInfo_client[driver_index]->call(node_id, request);
-        }
     }
     //Add node to seen list if not seen before
-    addToSeenNodeMask(node_id);
+    addToSeenNodeMask(transfer.source_node_id);
 }
 
-//Trampoline call for handleNodeStatus member method
-void AP_UAVCAN_DNA_Server::trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb)
-{
-    if (ap_uavcan == nullptr) {
-        return;
-    }
-
-    ap_uavcan->_dna_server->handleNodeStatus(node_id, cb);
-}
-
-
 /* Node Info message handler
 Handle responses from GetNodeInfo Request. We verify the node info
 against our records. Marks Verification mask if already recorded,
 Or register if the node id is available and not recorded for the
 received Unique ID */
-void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit)
+void AP_UAVCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp)
 {
-    if (node_id > MAX_NODE_ID) {
+    if (transfer.source_node_id > MAX_NODE_ID) {
         return;
     }
     /*
       if we haven't logged this node then log it now
      */
-    if (!logged.get(node_id) && AP::logger().logging_started()) {
+    if (!logged.get(transfer.source_node_id) && AP::logger().logging_started()) {
-        logged.set(node_id);
+        logged.set(transfer.source_node_id);
         uint64_t uid[2];
-        memcpy(uid, unique_id, sizeof(uid));
+        memcpy(uid, rsp.hardware_version.unique_id, sizeof(rsp.hardware_version.unique_id));
         // @LoggerMessage: CAND
         // @Description: Info from GetNodeInfo request
         // @Field: TimeUS: Time since system startup
@@ -540,105 +455,82 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[],
         AP::logger().Write("CAND", "TimeUS,NodeId,UID1,UID2,Name,Major,Minor,Version",
                            "s#------", "F-------", "QBQQZBBI",
                            AP_HAL::micros64(),
-                           node_id,
+                           transfer.source_node_id,
                            uid[0], uid[1],
-                           name,
+                           rsp.name.data,
-                           major,
+                           rsp.software_version.major,
-                           minor,
+                           rsp.software_version.minor,
-                           vcs_commit);
+                           rsp.software_version.vcs_commit);
     }
 
-    if (isNodeIDOccupied(node_id)) {
+    if (isNodeIDOccupied(transfer.source_node_id)) {
         //if node_id already registered, just verify if Unique ID matches as well
-        if (node_id == getNodeIDForUniqueID(unique_id, 16)) {
+        if (transfer.source_node_id == getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) {
-            if (node_id == curr_verifying_node) {
+            if (transfer.source_node_id == curr_verifying_node) {
                 nodeInfo_resp_rcvd = true;
             }
-            setVerificationMask(node_id);
+            setVerificationMask(transfer.source_node_id);
-        } else if (!_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
+        } else if (!_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
             /* This is a device with node_id already registered
             for another device */
             server_state = DUPLICATE_NODES;
-            fault_node_id = node_id;
+            fault_node_id = transfer.source_node_id;
-            memcpy(fault_node_name, name, sizeof(fault_node_name));
+            memcpy(fault_node_name, rsp.name.data, sizeof(fault_node_name));
         }
     } else {
         /* Node Id was not allocated by us, or during this boot, let's register this in our records
         Check if we allocated this Node before */
-        uint8_t prev_node_id = getNodeIDForUniqueID(unique_id, 16);
+        uint8_t prev_node_id = getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16);
         if (prev_node_id != 255) {
             //yes we did, remove this registration
             freeNodeID(prev_node_id);
         }
         //add a new server record
-        addNodeIDForUniqueID(node_id, unique_id, 16);
+        addNodeIDForUniqueID(transfer.source_node_id, rsp.hardware_version.unique_id, 16);
         //Verify as well
-        setVerificationMask(node_id);
+        setVerificationMask(transfer.source_node_id);
-        if (node_id == curr_verifying_node) {
+        if (transfer.source_node_id == curr_verifying_node) {
             nodeInfo_resp_rcvd = true;
         }
     }
 }
 
-//Trampoline call for handleNodeInfo member call
-void AP_UAVCAN_DNA_Server::trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp)
-{
-    uint8_t unique_id[16] = {0};
-    char name[50] = {0};
-
-    //copy the unique id from message to uint8_t array
-    auto &r = resp.rsp->getResponse();
-    uavcan::copy(r.hardware_version.unique_id.begin(),
-                 r.hardware_version.unique_id.end(),
-                 unique_id);
-    strncpy_noterm(name, r.name.c_str(), sizeof(name)-1);
-
-    ap_uavcan->_dna_server->handleNodeInfo(node_id, unique_id, name,
-                              r.software_version.major,
-                              r.software_version.minor,
-                              r.software_version.vcs_commit);
-}
-
 /* Handle the allocation message from the devices supporting
 dynamic node allocation. */
-void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t node_id, const AllocationCb &cb)
+void AP_UAVCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg)
 {
-    if (allocation_pub[driver_index] == nullptr) {
+    if (transfer.source_node_id != 0) {
-        //init has not been called for this driver.
-        return;
-    }
-    if (!cb.msg->isAnonymousTransfer()) {
         //Ignore Allocation messages that are not DNA requests
         return;
     }
-    uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec();
+    uint32_t now = AP_HAL::millis();
 
     if (rcvd_unique_id_offset == 0 ||
         (now - last_alloc_msg_ms) > 500) {
-        if (cb.msg->first_part_of_unique_id) {
+        if (msg.first_part_of_unique_id) {
             rcvd_unique_id_offset = 0;
             memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
         } else {
             //we are only accepting first part
             return;
         }
-    } else if (cb.msg->first_part_of_unique_id) {
+    } else if (msg.first_part_of_unique_id) {
         // we are only accepting follow up messages
         return;
     }
 
     if (rcvd_unique_id_offset) {
         debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld  -- Accepting Followup part! %u\n",
-                     long(uavcan::SystemClock::instance().getMonotonic().toUSec()/1000),
+                     (long int)AP_HAL::millis(),
                      unsigned((now - last_alloc_msg_ms)));
     } else {
         debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld  -- Accepting First part! %u\n",
-                     long(uavcan::SystemClock::instance().getMonotonic().toUSec()/1000),
+                     (long int)AP_HAL::millis(),
                      unsigned((now - last_alloc_msg_ms)));
     }
 
     last_alloc_msg_ms = now;
-    if ((rcvd_unique_id_offset + cb.msg->unique_id.size()) > 16) {
+    if ((rcvd_unique_id_offset + msg.unique_id.len) > 16) {
         //This request is malformed, Reset!
         rcvd_unique_id_offset = 0;
         memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
@@ -646,50 +538,40 @@ void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t node_id, const AllocationCb
     }
 
     //copy over the unique_id
-    for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + cb.msg->unique_id.size()); i++) {
+    for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + msg.unique_id.len); i++) {
-        rcvd_unique_id[i] = cb.msg->unique_id[i - rcvd_unique_id_offset];
+        rcvd_unique_id[i] = msg.unique_id.data[i - rcvd_unique_id_offset];
     }
-    rcvd_unique_id_offset += cb.msg->unique_id.size();
+    rcvd_unique_id_offset += msg.unique_id.len;
 
     //send follow up message
-    uavcan::protocol::dynamic_node_id::Allocation msg;
+    uavcan_protocol_dynamic_node_id_Allocation rsp;
 
     /* Respond with the message containing the received unique ID so far
     or with node id if we successfully allocated one. */
-    for (uint8_t i = 0; i < rcvd_unique_id_offset; i++) {
+    memcpy(rsp.unique_id.data, rcvd_unique_id, rcvd_unique_id_offset);
-        msg.unique_id.push_back(rcvd_unique_id[i]);
+    rsp.unique_id.len = rcvd_unique_id_offset;
-    }
 
     if (rcvd_unique_id_offset == 16) {
         //We have received the full Unique ID, time to do allocation
         uint8_t resp_node_id = getNodeIDForUniqueID((const uint8_t*)rcvd_unique_id, 16);
         if (resp_node_id == 255) {
-            resp_node_id = findFreeNodeID(cb.msg->node_id);
+            resp_node_id = findFreeNodeID(msg.node_id);
             if (resp_node_id != 255) {
                 if (addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16)) {
-                    msg.node_id = resp_node_id;
+                    rsp.node_id = resp_node_id;
                 }
             } else {
                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
             }
         } else {
-            msg.node_id = resp_node_id;
+            rsp.node_id = resp_node_id;
         }
         //reset states as well        
         rcvd_unique_id_offset = 0;
         memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
     }
 
-    allocation_pub[driver_index]->broadcast(msg);
+    allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD
-}
-
-//Trampoline Call for handleAllocation member call
-void AP_UAVCAN_DNA_Server::trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb)
-{
-    if (ap_uavcan == nullptr) {
-        return;
-    }
-    ap_uavcan->_dna_server->handleAllocation(node_id, cb);
 }
 
 //report the server state, along with failure message if any
@@ -703,7 +585,7 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co
         return false;
     }
     case DUPLICATE_NODES: {
-        if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
+        if (_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
             // ignore error
             return true;
         }
@@ -715,7 +597,7 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co
         return false;
     }
     case NODE_STATUS_UNHEALTHY: {
-        if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
+        if (_ap_uavcan.option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
             // ignore error
             return true;
         }

libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h

@@ -3,18 +3,17 @@
 #include <AP_HAL/Semaphores.h>
 
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
-#include <uavcan/uavcan.hpp>
 #include <AP_Common/Bitmask.h>
 #include <StorageManager/StorageManager.h>
 #include <AP_CANManager/AP_CANManager.h>
+#include <canard/publisher.h>
+#include <canard/subscriber.h>
+#include <canard/service_client.h>
+#include "AP_Canard_iface.h"
+#include <dronecan_msgs.h>
 
-//Forward declaring classes
-class AllocationCb;
-class NodeStatusCb;
-class NodeInfoCb;
-class GetNodeInfoCb;
 class AP_UAVCAN;
-
+//Forward declaring classes
 class AP_UAVCAN_DNA_Server
 {
     StorageAccess storage;
@@ -34,7 +33,7 @@ class AP_UAVCAN_DNA_Server
 
     uint32_t last_verification_request;
     uint8_t curr_verifying_node;
-    uint8_t self_node_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+    uint8_t self_node_id;
     bool nodeInfo_resp_rcvd;
 
     Bitmask<128> occupation_mask;
@@ -89,24 +88,30 @@ class AP_UAVCAN_DNA_Server
     //Look in the storage and check if there's a valid Server Record there
     bool isValidNodeDataAvailable(uint8_t node_id);
 
-    static void trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp);
-    static void trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb);
-    static void trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb);
-
-
     HAL_Semaphore storage_sem;
-    AP_UAVCAN *_ap_uavcan;
+    AP_UAVCAN &_ap_uavcan;
-    uint8_t driver_index;
+    CanardInterface &_canard_iface;
+
+    Canard::Publisher<uavcan_protocol_dynamic_node_id_Allocation> allocation_pub{_canard_iface};
+
+    Canard::ObjCallback<AP_UAVCAN_DNA_Server, uavcan_protocol_dynamic_node_id_Allocation> allocation_cb{this, &AP_UAVCAN_DNA_Server::handleAllocation};
+    Canard::Subscriber<uavcan_protocol_dynamic_node_id_Allocation> allocation_sub;
+
+    Canard::ObjCallback<AP_UAVCAN_DNA_Server, uavcan_protocol_NodeStatus> node_status_cb{this, &AP_UAVCAN_DNA_Server::handleNodeStatus};
+    Canard::Subscriber<uavcan_protocol_NodeStatus> node_status_sub;
+
+    Canard::ObjCallback<AP_UAVCAN_DNA_Server, uavcan_protocol_GetNodeInfoResponse> node_info_cb{this, &AP_UAVCAN_DNA_Server::handleNodeInfo};
+    Canard::Client<uavcan_protocol_GetNodeInfoResponse> node_info_client;
 
 public:
-    AP_UAVCAN_DNA_Server(AP_UAVCAN *ap_uavcan, StorageAccess _storage);
+    AP_UAVCAN_DNA_Server(AP_UAVCAN &ap_uavcan);
 
 
     // Do not allow copies
     CLASS_NO_COPY(AP_UAVCAN_DNA_Server);
 
     //Initialises publisher and Server Record for specified uavcan driver
-    bool init();
+    bool init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id);
 
     //Reset the Server Record
     void reset();
@@ -125,9 +130,9 @@ public:
     bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
 
     //Callbacks
-    void handleAllocation(uint8_t node_id, const AllocationCb &cb);
+    void handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg);
-    void handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb);
+    void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg);
-    void handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit);
+    void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp);
 
     //Run through the list of seen node ids for verification
     void verify_nodes();

libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp

@@ -1,238 +0,0 @@
-
-/*
- * This file is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * Author: Siddharth Bharat Purohit
- */
-
-#include "AP_UAVCAN_IfaceMgr.h"
-
-#if HAL_ENABLE_LIBUAVCAN_DRIVERS
-#include "AP_UAVCAN_Clock.h"
-#include <AP_HAL/AP_HAL.h>
-#include <AP_CANManager/AP_CANManager.h>
-using namespace uavcan;
-extern const AP_HAL::HAL& hal;
-#define LOG_TAG "UAVCANIface"
-
-/*****************************************************
- *                                                   *
- *                    CAN Iface                      *
- *                                                   *
- * ***************************************************/
-
-/**
- * Non-blocking transmission.
- *
- * If the frame wasn't transmitted upon TX deadline, the driver should discard it.
- *
- * Note that it is LIKELY that the library will want to send the frames that were passed into the select()
- * method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new
- * frames between the calls.
- *
- * @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
- */
-int16_t CanIface::send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags)
-{
-    if (can_iface_ == UAVCAN_NULLPTR) {
-        return -1;
-    }
-    return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.isCanFDFrame()), tx_deadline.toUSec(), flags);
-}
-
-/**
- * Non-blocking reception.
- *
- * Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller.
- *
- * Monotonic timestamp is required and can be not precise since it is needed only for
- * protocol timing validation (transfer timeouts and inter-transfer intervals).
- *
- * UTC timestamp is optional, if available it will be used for precise time synchronization;
- * must be set to zero if not available.
- *
- * Refer to @ref ISystemClock to learn more about timestamps.
- *
- * @param [out] out_ts_monotonic Monotonic timestamp, mandatory.
- * @param [out] out_ts_utc       UTC timestamp, optional, zero if unknown.
- * @return 1 = one frame received, 0 = RX buffer empty, negative for error.
- */
-int16_t CanIface::receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc,
-                          CanIOFlags& out_flags)
-{
-
-    if (can_iface_ == UAVCAN_NULLPTR) {
-        return -1;
-    }
-    AP_HAL::CANFrame frame;
-    uint64_t rx_timestamp;
-    uint16_t flags;
-    out_ts_monotonic = SystemClock::instance().getMonotonic();
-    int16_t ret = can_iface_->receive(frame, rx_timestamp, flags);
-    if (ret < 0) {
-        return ret;
-    }
-    out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.canfd);
-    out_flags = flags;
-    if (rx_timestamp != 0) {
-        out_ts_utc = uavcan::UtcTime::fromUSec(SystemClock::instance().getAdjustUsec() + rx_timestamp);
-    } else {
-        out_ts_utc = uavcan::UtcTime::fromUSec(0);
-    }
-    return ret;
-}
-
-/**
- * Number of available hardware filters.
- */
-uint16_t CanIface::getNumFilters() const
-{
-    if (can_iface_ == UAVCAN_NULLPTR) {
-        return 0;
-    }
-    return can_iface_->getNumFilters();
-}
-
-/**
- * Continuously incrementing counter of hardware errors.
- * Arbitration lost should not be treated as a hardware error.
- */
-uint64_t CanIface::getErrorCount() const
-{
-    if (can_iface_ == UAVCAN_NULLPTR) {
-        return 0;
-    }
-    return can_iface_->getErrorCount();
-}
-
-/*****************************************************
- *                                                   *
- *                    CAN Driver                     *
- *                                                   *
- * ***************************************************/
-
-bool CanIfaceMgr::add_interface(AP_HAL::CANIface *can_iface)
-{
-    if (num_ifaces > HAL_NUM_CAN_IFACES) {
-        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Num Ifaces Exceeded\n");
-        return false;
-    }
-    if (can_iface == nullptr) {
-        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface Null\n");
-        return false;
-    }
-    if (ifaces[num_ifaces] != nullptr) {
-        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface already added\n");
-        return false;
-    }
-    ifaces[num_ifaces] = new CanIface(can_iface);
-    if (ifaces[num_ifaces] == nullptr) {
-        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Can't alloc uavcan::iface\n");
-        return false;
-    }
-    if (!ifaces[num_ifaces]->can_iface_->set_event_handle(&_event_handle)) {
-        AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Setting event handle failed\n");
-        return false;
-    }
-    AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "UAVCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces));
-    num_ifaces++;
-    return true;
-}
-/**
- * Returns an interface by index, or null pointer if the index is out of range.
- */
-ICanIface* CanIfaceMgr::getIface(uint8_t iface_index)
-{
-    if (iface_index >= num_ifaces) {
-        return UAVCAN_NULLPTR;
-    }
-    return ifaces[iface_index];
-}
-
-/**
- * Total number of available CAN interfaces.
- * This value shall not change after initialization.
- */
-uint8_t CanIfaceMgr::getNumIfaces() const
-{
-    return num_ifaces;
-}
-
-CanSelectMasks CanIfaceMgr::makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const
-{
-    CanSelectMasks msk;
-    for (uint8_t i = 0; i < num_ifaces; i++) {
-        bool read = in_mask.read & (1 << i);
-        bool write = in_mask.write & (1 << i);
-        CanIface* iface = ifaces[i];
-        if (iface == nullptr) {
-            continue;
-        }
-        if (pending_tx[i] == UAVCAN_NULLPTR) {
-            if (iface->can_iface_->select(read, write, nullptr, 0)) {
-                msk.read  |= (read ? 1 : 0) << i;
-                msk.write |= (write ? 1 : 0) << i;
-            }
-        } else {
-            AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, AP_HAL::CANFrame::dlcToDataLength(pending_tx[i]->dlc)};
-            if (iface->can_iface_->select(read, write, &frame, 0)) {
-                msk.read  |= (read ? 1 : 0) << i;
-                msk.write |= (write ? 1 : 0) << i;
-            }
-        }
-    }
-
-    return msk;
-}
-
-/**
- * Block until the deadline, or one of the specified interfaces becomes available for read or write.
- *
- * Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO.
- *
- * Bit position in the masks defines interface index.
- *
- * Note that it is allowed to return from this method even if no requested events actually happened, or if
- * there are events that were not requested by the library.
- *
- * The pending TX argument contains an array of pointers to CAN frames that the library wants to transmit
- * next, per interface. This is intended to allow the driver to properly prioritize transmissions; many
- * drivers will not need to use it. If a write flag for the given interface is set to one in the select mask
- * structure, then the corresponding pointer is guaranteed to be valid (not UAVCAN_NULLPTR).
- *
- * @param [in,out] inout_masks        Masks indicating which interfaces are needed/available for IO.
- * @param [in]     pending_tx         Array of frames, per interface, that are likely to be transmitted next.
- * @param [in]     blocking_deadline  Zero means non-blocking operation.
- * @return Positive number of ready interfaces or negative error code.
- */
-int16_t CanIfaceMgr::select(CanSelectMasks& inout_masks,
-                            const CanFrame* (& pending_tx)[MaxCanIfaces],
-                            const MonotonicTime blocking_deadline)
-{
-    const CanSelectMasks in_masks = inout_masks;
-    const uint64_t time = SystemClock::instance().getMonotonic().toUSec();
-
-    inout_masks = makeSelectMasks(in_masks, pending_tx);          // Check if we already have some of the requested events
-    if ((inout_masks.read  & in_masks.read)  != 0 ||
-        (inout_masks.write & in_masks.write) != 0) {
-        return 1;
-    }
-    if (time < blocking_deadline.toUSec()) {
-        _event_handle.wait(blocking_deadline.toUSec() - time); // Block until timeout expires or any iface updates
-    }
-
-    inout_masks = makeSelectMasks(in_masks, pending_tx);  // Return what we got even if none of the requested events are set
-    return 1;                                   // Return value doesn't matter as long as it is non-negative
-}
-#endif //HAL_ENABLE_LIBUAVCAN_DRIVERSs

libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h

@@ -1,75 +0,0 @@
-/*
- * This file is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * Author: Siddharth Bharat Purohit
- */
-#pragma once
-
-#include <AP_HAL/AP_HAL.h>
-#if HAL_ENABLE_LIBUAVCAN_DRIVERS
-
-#include <uavcan/uavcan.hpp>
-
-namespace uavcan
-{
-
-class CanDriver;
-
-class CanIface : public ICanIface, Noncopyable
-{
-    friend class CanIfaceMgr;
-    AP_HAL::CANIface* can_iface_;
-public:
-    CanIface(AP_HAL::CANIface *can_iface) : can_iface_(can_iface) {}
-
-    virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline,
-                         CanIOFlags flags) override;
-
-    virtual int16_t receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic,
-                            UtcTime& out_ts_utc, CanIOFlags& out_flags) override;
-
-    virtual int16_t configureFilters(const CanFilterConfig* filter_configs,
-                                     uint16_t num_configs) override {
-        return 0;
-    }
-    
-    uint16_t getNumFilters() const override;
-
-    uint64_t getErrorCount() const override;
-};
-
-/**
- * Generic CAN driver.
- */
-class CanIfaceMgr : public ICanDriver, Noncopyable
-{
-    CanIface* ifaces[HAL_NUM_CAN_IFACES];
-    uint8_t num_ifaces;
-    HAL_EventHandle _event_handle;
-public:
-    bool add_interface(AP_HAL::CANIface *can_drv);
-
-    ICanIface* getIface(uint8_t iface_index) override;
-
-    uint8_t getNumIfaces() const override;
-
-    CanSelectMasks makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const;
-
-    int16_t select(CanSelectMasks& inout_masks,
-                   const CanFrame* (& pending_tx)[MaxCanIfaces],
-                   const MonotonicTime blocking_deadline) override;
-};
-
-}
-#endif

libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp

@@ -1,90 +0,0 @@
-/*
- * This file is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-/*
-  based on dynamic_memory.hpp which is:
-  Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- */
-
-#include <AP_HAL/AP_HAL.h>
-
-#if HAL_ENABLE_LIBUAVCAN_DRIVERS
-
-#include "AP_UAVCAN.h"
-#include "AP_UAVCAN_pool.h"
-#include <AP_InternalError/AP_InternalError.h>
-
-AP_PoolAllocator::AP_PoolAllocator(uint16_t _pool_size) :
-    num_blocks(_pool_size / UAVCAN_NODE_POOL_BLOCK_SIZE)
-{
-}
-
-bool AP_PoolAllocator::init(void)
-{
-    WITH_SEMAPHORE(sem);
-    pool_nodes = (Node *)calloc(num_blocks, UAVCAN_NODE_POOL_BLOCK_SIZE);
-    if (pool_nodes == nullptr) {
-        return false;
-    }
-    for (uint16_t i=0; i<(num_blocks-1); i++) {
-        pool_nodes[i].next = &pool_nodes[i+1];
-    }
-    free_list = &pool_nodes[0];
-    return true;
-}
-
-void* AP_PoolAllocator::allocate(std::size_t size)
-{
-    WITH_SEMAPHORE(sem);
-    if (free_list == nullptr || size > UAVCAN_NODE_POOL_BLOCK_SIZE) {
-        return nullptr;
-    }
-    Node *ret = free_list;
-    const uint32_t blk = ret - pool_nodes;
-    if (blk >= num_blocks) {
-        INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
-        return nullptr;
-    }
-    free_list = free_list->next;
-
-    used++;
-    if (used > max_used) {
-        max_used = used;
-    }
-
-    return ret;
-}
-
-void AP_PoolAllocator::deallocate(const void* ptr)
-{
-    if (ptr == nullptr) {
-        return;
-    }
-    WITH_SEMAPHORE(sem);
-
-    Node *p = reinterpret_cast<Node*>(const_cast<void*>(ptr));
-    const uint32_t blk = p - pool_nodes;
-    if (blk >= num_blocks) {
-        INTERNAL_ERROR(AP_InternalError::error_t::mem_guard);
-        return;
-    }
-    p->next = free_list;
-    free_list = p;
-
-    used--;
-}
-
-#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS
-

libraries/AP_UAVCAN/AP_UAVCAN_pool.h

@@ -1,60 +0,0 @@
-/*
- * This file is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-/*
-  based on dynamic_memory.hpp which is:
-  Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
- */
-
-#pragma once
-
-#include "AP_UAVCAN.h"
-
-#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
-#if HAL_CANFD_SUPPORTED
-#define UAVCAN_NODE_POOL_BLOCK_SIZE 128
-#else
-#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
-#endif
-#endif
-
-class AP_PoolAllocator : public uavcan::IPoolAllocator
-{
-public:
-    AP_PoolAllocator(uint16_t _pool_size);
-
-    bool init(void);
-    void *allocate(std::size_t size) override;
-    void deallocate(const void* ptr) override;
-
-    uint16_t getBlockCapacity() const override {
-        return num_blocks;
-    }
-
-private:
-    const uint16_t num_blocks;
-
-    union Node {
-        uint8_t data[UAVCAN_NODE_POOL_BLOCK_SIZE];
-        Node* next;
-    };
-
-    Node *free_list;
-    Node *pool_nodes;
-    HAL_Semaphore sem;
-
-    uint16_t used;
-    uint16_t max_used;
-};

libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp

@@ -4,42 +4,18 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
 
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
+#if HAL_ENABLE_LIBUAVCAN_DRIVERS
 
 #include <AP_HAL/Semaphores.h>
 
 #include <AP_UAVCAN/AP_UAVCAN.h>
 
-#include <uavcan/uavcan.hpp>
-
-#include <uavcan/helpers/heap_based_pool_allocator.hpp>
-
-#include <uavcan/equipment/gnss/Fix2.hpp>
-#include <uavcan/equipment/gnss/Auxiliary.hpp>
-
-#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
-#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
-#include <uavcan/equipment/air_data/StaticPressure.hpp>
-#include <uavcan/equipment/air_data/StaticTemperature.hpp>
-
-#include <uavcan/equipment/actuator/ArrayCommand.hpp>
-#include <uavcan/equipment/actuator/Command.hpp>
-#include <uavcan/equipment/actuator/Status.hpp>
-
-#include <uavcan/equipment/esc/RawCommand.hpp>
-#include <uavcan/equipment/indication/LightsCommand.hpp>
-#include <uavcan/equipment/indication/SingleLightCommand.hpp>
-#include <uavcan/equipment/indication/RGB565.hpp>
-
-#include <uavcan/equipment/power/BatteryInfo.hpp>
-
-#include <com/hex/equipment/flow/Measurement.hpp>
-
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include <AP_HAL_Linux/CANSocketIface.h>
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <AP_HAL_SITL/CANSocketIface.h>
 #elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#include <hal.h>
 #include <AP_HAL_ChibiOS/CANIface.h>
 #endif
 
@@ -49,10 +25,8 @@ void loop();
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
 #define UAVCAN_NODE_POOL_SIZE 8192
-#ifdef UAVCAN_NODE_POOL_BLOCK_SIZE
+
-#undef UAVCAN_NODE_POOL_BLOCK_SIZE
+static uint8_t node_memory_pool[UAVCAN_NODE_POOL_SIZE];
-#endif
-#define UAVCAN_NODE_POOL_BLOCK_SIZE 256
 
 #define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)
 
@@ -67,22 +41,6 @@ public:
 
 private:
     uint8_t driver_index = 0;
-    uavcan::Node<0> *_node;
-
-    // This will be needed to implement if UAVCAN is used with multithreading
-    // Such cases will be firmware update, etc.
-    class RaiiSynchronizer {
-    public:
-        RaiiSynchronizer()
-        {
-        }
-
-        ~RaiiSynchronizer()
-        {
-        }
-    };
-
-    uavcan::HeapBasedPoolAllocator<UAVCAN_NODE_POOL_BLOCK_SIZE, UAVCAN_sniffer::RaiiSynchronizer> _node_allocator;
 
     AP_CANManager can_mgr;
 };
@@ -108,20 +66,35 @@ static void count_msg(const char *name)
 }
 
 #define MSG_CB(mtype, cbname)                                                  \
-    static void cb_ ## cbname(const uavcan::ReceivedDataStructure<mtype>& msg) { count_msg(msg.getDataTypeFullName()); }
+    static void cb_ ## cbname(const CanardRxTransfer &transfer, const mtype& msg) { count_msg(#mtype); }
 
-MSG_CB(uavcan::protocol::NodeStatus, NodeStatus)
+MSG_CB(uavcan_protocol_NodeStatus, NodeStatus)
-MSG_CB(uavcan::equipment::gnss::Fix2, Fix2)
+MSG_CB(uavcan_equipment_gnss_Fix2, Fix2)
-MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary)
+MSG_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary)
-MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength)
+MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength)
-MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2);
+MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);
-MSG_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure)
+MSG_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure)
-MSG_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature)
+MSG_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature)
-MSG_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo);
+MSG_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);
-MSG_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand)
+MSG_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand)
-MSG_CB(uavcan::equipment::esc::RawCommand, RawCommand)
+MSG_CB(uavcan_equipment_esc_RawCommand, RawCommand)
-MSG_CB(uavcan::equipment::indication::LightsCommand, LightsCommand);
+MSG_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);
-MSG_CB(com::hex::equipment::flow::Measurement, Measurement);
+MSG_CB(com_hex_equipment_flow_Measurement, Measurement);
+
+
+uavcan_protocol_NodeStatus node_status;
+uavcan_protocol_GetNodeInfoResponse node_info;
+CanardInterface *_uavcan_iface_mgr;
+Canard::Publisher<uavcan_protocol_NodeStatus> *node_status_pub;
+Canard::Server<uavcan_protocol_GetNodeInfoRequest> *node_info_srv;
+
+static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan_protocol_GetNodeInfoRequest& msg)
+{
+    if (node_info_srv == nullptr) {
+        return;
+    }
+    node_info_srv->respond(transfer, node_info);
+}
 
 void UAVCAN_sniffer::init(void)
 {
@@ -137,7 +110,7 @@ void UAVCAN_sniffer::init(void)
         debug_uavcan("Can not initialised\n");
         return;
     }
-    uavcan::CanIfaceMgr *_uavcan_iface_mgr = new uavcan::CanIfaceMgr;
+    _uavcan_iface_mgr = new CanardInterface{driver_index};
 
     if (_uavcan_iface_mgr == nullptr) {
         return;
@@ -148,74 +121,63 @@ void UAVCAN_sniffer::init(void)
         return;
     }
 
-    _node = new uavcan::Node<0>(*_uavcan_iface_mgr,  uavcan::SystemClock::instance(), _node_allocator);
+    _uavcan_iface_mgr->init(node_memory_pool, sizeof(node_memory_pool), 9);
 
-    if (_node == nullptr) {
+    node_status_pub = new Canard::Publisher<uavcan_protocol_NodeStatus>{*_uavcan_iface_mgr};
+    if (node_status_pub == nullptr) {
         return;
     }
 
-    if (_node->isStarted()) {
+    node_info_srv = new Canard::Server<uavcan_protocol_GetNodeInfoRequest>{*_uavcan_iface_mgr, *Canard::allocate_static_callback(cb_GetNodeInfoRequest)};
+    if (node_info_srv == nullptr) {
         return;
     }
 
-    uavcan::NodeID self_node_id(9);
+    node_info.name.len = snprintf((char*)node_info.name.data, sizeof(node_info.name.data), "org.ardupilot:%u", driver_index);
-    _node->setNodeID(self_node_id);
 
-    char ndname[20];
+    node_info.software_version.major = AP_UAVCAN_SW_VERS_MAJOR;
-    snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index);
+    node_info.software_version.minor = AP_UAVCAN_SW_VERS_MINOR;
 
-    uavcan::NodeStatusProvider::NodeName name(ndname);
+    node_info.hardware_version.major = AP_UAVCAN_HW_VERS_MAJOR;
-    _node->setName(name);
+    node_info.hardware_version.minor = AP_UAVCAN_HW_VERS_MINOR;
 
-    uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
+#define START_CB(mtype, cbname) Canard::allocate_sub_static_callback(cb_ ## cbname,driver_index)
-    sw_version.major = AP_UAVCAN_SW_VERS_MAJOR;
-    sw_version.minor = AP_UAVCAN_SW_VERS_MINOR;
-    _node->setSoftwareVersion(sw_version);
 
-    uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion
+    START_CB(uavcan_protocol_NodeStatus, NodeStatus);
-
+    START_CB(uavcan_equipment_gnss_Fix2, Fix2);
-    hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
+    START_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary);
-    hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
+    START_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength);
-    _node->setHardwareVersion(hw_version);
+    START_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);
-
+    START_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure);
-    int start_res = _node->start();
+    START_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature);
-    if (start_res < 0) {
+    START_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);
-        debug_uavcan("UAVCAN: node start problem\n\r");
+    START_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand);
-        return;
+    START_CB(uavcan_equipment_esc_RawCommand, RawCommand);
-    }
+    START_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);
-
+    START_CB(com_hex_equipment_flow_Measurement, Measurement);
-#define START_CB(mtype, cbname) (new uavcan::Subscriber<mtype>(*_node))->start(cb_ ## cbname)
-
-    START_CB(uavcan::protocol::NodeStatus, NodeStatus);
-    START_CB(uavcan::equipment::gnss::Fix2, Fix2);
-    START_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary);
-    START_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength);
-    START_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2);
-    START_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure);
-    START_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature);
-    START_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo);
-    START_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand);
-    START_CB(uavcan::equipment::esc::RawCommand, RawCommand);
-    START_CB(uavcan::equipment::indication::LightsCommand, LightsCommand);
-    START_CB(com::hex::equipment::flow::Measurement, Measurement);
-
-
-    /*
-     * Informing other nodes that we're ready to work.
-     * Default mode is INITIALIZING.
-     */
-    _node->setModeOperational();
 
     debug_uavcan("UAVCAN: init done\n\r");
 }
 
+static void send_node_status()
+{
+    uavcan_protocol_NodeStatus msg;
+    msg.uptime_sec = AP_HAL::millis() / 1000;
+    msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
+    msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
+    msg.sub_mode = 0;
+    msg.vendor_specific_status_code = 0;
+    node_status_pub->broadcast(msg);
+}
+
+uint32_t last_status_send = 0;
 void UAVCAN_sniffer::loop(void)
 {
-    if (_node == nullptr) {
+    if (AP_HAL::millis() - last_status_send > 1000) {
-        return;
+        last_status_send = AP_HAL::millis();
+        send_node_status();
     }
-
+    _uavcan_iface_mgr->process(1);
-    _node->spin(uavcan::MonotonicDuration::fromMSec(1));
 }
 
 void UAVCAN_sniffer::print_stats(void)
@@ -233,8 +195,7 @@ void UAVCAN_sniffer::print_stats(void)
 
 static UAVCAN_sniffer sniffer;
 
-UAVCAN_sniffer::UAVCAN_sniffer() :
+UAVCAN_sniffer::UAVCAN_sniffer()
-    _node_allocator(UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE)
 {}
 
 UAVCAN_sniffer::~UAVCAN_sniffer()

libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript

@@ -1,7 +1,21 @@
 #!/usr/bin/env python
 # encoding: utf-8
+from waflib.TaskGen import after_method, before_method, feature
 
 def build(bld):
-    bld.ap_example(
+    vehicle = bld.path.name
-        use='ap',
+    
+    bld.ap_stlib(
+        name=vehicle + '_libs',
+        ap_vehicle='UNKNOWN',
+        dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c',
+        ap_libraries=bld.ap_common_vehicle_libraries() + [
+            'AP_OSD',
+            'AP_RCMapper',
+            'AP_Arming'
+        ],
+    )
+    bld.ap_program(
+        program_groups=['tool'],
+        use=[vehicle + '_libs'],
     )