diff --git a/libraries/AP_UAVCAN/AP_Canard_iface.cpp b/libraries/AP_UAVCAN/AP_Canard_iface.cpp
new file mode 100644
index 0000000000..e415fa4afe
--- /dev/null
+++ b/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
diff --git a/libraries/AP_UAVCAN/AP_Canard_iface.h b/libraries/AP_UAVCAN/AP_Canard_iface.h
new file mode 100644
index 0000000000..9c88df8826
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index 41f1ef3ac6..44a599e391 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
@@ -25,34 +25,13 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_CANManager/AP_CANManager.h>
 
-#include <uavcan/transport/can_acceptance_filter_configurator.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/esc/Status.hpp>
-#include <uavcan/equipment/indication/LightsCommand.hpp>
-#include <uavcan/equipment/indication/SingleLightCommand.hpp>
-#include <uavcan/equipment/indication/BeepCommand.hpp>
-#include <uavcan/equipment/indication/RGB565.hpp>
-#include <uavcan/equipment/safety/ArmingStatus.hpp>
-#include <ardupilot/indication/SafetyState.hpp>
-#include <ardupilot/indication/Button.hpp>
-#include <ardupilot/indication/NotifyState.hpp>
-#include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
-#if AP_DRONECAN_SEND_GPS
-#include <uavcan/equipment/gnss/Fix2.hpp>
-#include <uavcan/equipment/gnss/Auxiliary.hpp>
-#include <ardupilot/gnss/Heading.hpp>
-#include <ardupilot/gnss/Status.hpp>
-#endif
-#include <uavcan/equipment/gnss/RTCMStream.hpp>
-#include <uavcan/protocol/debug/LogMessage.hpp>
-
 #include <AP_Arming/AP_Arming.h>
+#include <AP_GPS/AP_GPS_UAVCAN.h>
+#include <AP_Compass/AP_Compass_UAVCAN.h>
 #include <AP_Baro/AP_Baro_UAVCAN.h>
+#include <AP_BattMonitor/AP_BattMonitor_UAVCAN.h>
+#include <AP_Airspeed/AP_Airspeed_UAVCAN.h>
+#include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
 #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
 #include <AP_EFI/AP_EFI_DroneCAN.h>
 #include <AP_GPS/AP_GPS_UAVCAN.h>
@@ -60,15 +39,14 @@
 #include <AP_BattMonitor/AP_BattMonitor_UAVCAN.h>
 #include <AP_Compass/AP_Compass_UAVCAN.h>
 #include <AP_Airspeed/AP_Airspeed_UAVCAN.h>
+#include <AP_Proximity/AP_Proximity_DroneCAN.h>
 #include <SRV_Channel/SRV_Channel.h>
-#include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>
 #include <AP_ADSB/AP_ADSB.h>
 #include "AP_UAVCAN_DNA_Server.h"
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_OpenDroneID/AP_OpenDroneID.h>
-#include "AP_UAVCAN_pool.h"
-#include <AP_Proximity/AP_Proximity_DroneCAN.h>
+#include <string.h>
 
 #define LED_DELAY_US 50000
 
@@ -111,7 +89,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
     // @Description: UAVCAN node should be set implicitly
     // @Range: 1 250
     // @User: Advanced
-    AP_GROUPINFO("NODE", 1, AP_UAVCAN, _uavcan_node, 10),
+    AP_GROUPINFO("NODE", 1, AP_UAVCAN, _dronecan_node, 10),
 
     // @Param: SRV_BM
     // @DisplayName: Output channels to be transmitted as servo over UAVCAN
@@ -172,60 +150,10 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
 // set this to 1 to minimise resend of stale msgs
 #define CAN_PERIODIC_TX_TIMEOUT_MS 2
 
-// publisher interfaces
-static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<uavcan::equipment::safety::ArmingStatus>* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-#if AP_DRONECAN_SEND_GPS
-static uavcan::Publisher<uavcan::equipment::gnss::Fix2>* gnss_fix2[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>* gnss_auxiliary[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<ardupilot::gnss::Heading>* gnss_heading[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<ardupilot::gnss::Status>* gnss_status[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-#endif
-static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::Publisher<ardupilot::indication::NotifyState>* notify_state[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-// Clients
-UC_CLIENT_CALL_REGISTRY_BINDER(ParamGetSetCb, uavcan::protocol::param::GetSet);
-static uavcan::ServiceClient<uavcan::protocol::param::GetSet, ParamGetSetCb>* param_get_set_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::protocol::param::GetSet::Request param_getset_req[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-UC_CLIENT_CALL_REGISTRY_BINDER(ParamExecuteOpcodeCb, uavcan::protocol::param::ExecuteOpcode);
-static uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ParamExecuteOpcodeCb>* param_execute_opcode_client[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-static uavcan::protocol::param::ExecuteOpcode::Request param_save_req[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-
-// subscribers
-
-// handler SafteyButton
-UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button);
-static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_button_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-// handler TrafficReport
-UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport);
-static uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb> *traffic_report_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-// handler actuator status
-UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status);
-static uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb> *actuator_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
-UC_REGISTRY_BINDER(ActuatorStatusVolzCb, com::volz::servo::ActuatorStatus);
-static uavcan::Subscriber<com::volz::servo::ActuatorStatus, ActuatorStatusVolzCb> *actuator_status_Volz_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-#endif
-
-// handler ESC status
-UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
-static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-// handler DEBUG
-UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage);
-static uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb> *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
-
-AP_UAVCAN::AP_UAVCAN()
+AP_UAVCAN::AP_UAVCAN(const int driver_index) :
+_driver_index(driver_index),
+canard_iface(driver_index),
+_dna_server(*this)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -250,120 +178,62 @@ AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index)
     return static_cast<AP_UAVCAN*>(AP::can().get_driver(driver_index));
 }
 
-bool AP_UAVCAN::add_interface(AP_HAL::CANIface* can_iface) {
-
-    if (_iface_mgr == nullptr) {
-        _iface_mgr = new uavcan::CanIfaceMgr();
-    }
-
-    if (_iface_mgr == nullptr) {
-        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't create UAVCAN interface manager\n\r");
-        return false;
-    }
-
-    if (!_iface_mgr->add_interface(can_iface)) {
+bool AP_UAVCAN::add_interface(AP_HAL::CANIface* can_iface)
+{
+    if (!canard_iface.add_interface(can_iface)) {
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't add UAVCAN interface\n\r");
         return false;   
     }
     return true;
 }
 
-#pragma GCC diagnostic push
-#pragma GCC diagnostic error "-Wframe-larger-than=2200"
 void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 {
-    _driver_index = driver_index;
-
+    if (driver_index != _driver_index) {
+        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: init called with wrong driver_index");
+        return;
+    }
     if (_initialized) {
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: init called more than once\n\r");
         return;
     }
 
-    if (_iface_mgr == nullptr) {
-        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't get UAVCAN interface driver\n\r");
-        return;
-    }
+    node_info_rsp.name.len = snprintf((char*)node_info_rsp.name.data, sizeof(node_info_rsp.name.data), "org.ardupilot:%u", driver_index);
 
-    _allocator = new AP_PoolAllocator(_pool_size);
+    node_info_rsp.software_version.major = AP_UAVCAN_SW_VERS_MAJOR;
+    node_info_rsp.software_version.minor = AP_UAVCAN_SW_VERS_MINOR;
+    node_info_rsp.hardware_version.major = AP_UAVCAN_HW_VERS_MAJOR;
+    node_info_rsp.hardware_version.minor = AP_UAVCAN_HW_VERS_MINOR;
 
-    if (_allocator == nullptr || !_allocator->init()) {
-        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node pool\n");
-        return;
-    }
-
-    _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), *_allocator);
-
-    if (_node == nullptr) {
-        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node\n\r");
-        return;
-    }
-
-    if (_node->isStarted()) {
-        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node was already started?\n\r");
-        return;
-    }
-    {
-        uavcan::NodeID self_node_id(_uavcan_node);
-        _node->setNodeID(self_node_id);
-
-        char ndname[20];
-        snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index);
-
-        uavcan::NodeStatusProvider::NodeName name(ndname);
-        _node->setName(name);
-    }
-    {
-        uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
-        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
-
-        hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
-        hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
-
-        const uint8_t uid_buf_len = hw_version.unique_id.capacity();
-        uint8_t uid_len = uid_buf_len;
-        uint8_t unique_id[uid_buf_len];
-
-
-        if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
-            //This is because we are maintaining a common Server Record for all UAVCAN Instances.
-            //In case the node IDs are different, and unique id same, it will create
-            //conflict in the Server Record.
-            unique_id[uid_len - 1] += _uavcan_node;
-            uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
-        }
-        _node->setHardwareVersion(hw_version);
-    }
-
-#if UAVCAN_SUPPORT_CANFD
+#if HAL_CANFD_SUPPORTED
     if (option_is_set(Options::CANFD_ENABLED)) {
-        _node->enableCanFd();
+        canard_iface.set_canfd(true);
     }
 #endif
 
-    int start_res = _node->start();
-    if (start_res < 0) {
-        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node start problem, error %d\n\r", start_res);
-        return;
-    }
+    uint8_t uid_len = sizeof(uavcan_protocol_HardwareVersion::unique_id);
+    uint8_t unique_id[sizeof(uavcan_protocol_HardwareVersion::unique_id)];
 
-    _dna_server = new AP_UAVCAN_DNA_Server(this, StorageAccess(StorageManager::StorageCANDNA));
-    if (_dna_server == nullptr) {
-        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate DNA server\n\r");
+    mem_pool = new uint32_t[_pool_size/sizeof(uint32_t)];
+    if (mem_pool == nullptr) {
+        debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: Failed to allocate memory pool\n\r");
         return;
     }
+    canard_iface.init(mem_pool, (_pool_size/sizeof(uint32_t))*sizeof(uint32_t), _dronecan_node);
+
+    if (!hal.util->get_system_id_unformatted(unique_id, uid_len)) {
+        return;
+    }
+    unique_id[uid_len - 1] += _dronecan_node;
+    memcpy(node_info_rsp.hardware_version.unique_id, unique_id, uid_len);
 
     //Start Servers
-    if (!_dna_server->init()) {
+    if (!_dna_server.init(unique_id, uid_len, _dronecan_node)) {
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: Failed to start DNA Server\n\r");
         return;
     }
 
     // Roundup all subscribers from supported drivers
-    AP_UAVCAN_DNA_Server::subscribe_msgs(this);
     AP_GPS_UAVCAN::subscribe_msgs(this);
 #if AP_COMPASS_UAVCAN_ENABLED
     AP_Compass_UAVCAN::subscribe_msgs(this);
@@ -389,107 +259,71 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     AP_Proximity_DroneCAN::subscribe_msgs(this);
 #endif
 
-    act_out_array[driver_index] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*_node);
-    act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
-    act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+    act_out_array.set_timeout_ms(2);
+    act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
 
-    esc_raw[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::RawCommand>(*_node);
-    esc_raw[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
-    esc_raw[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+    esc_raw.set_timeout_ms(2);
+    esc_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
 
-    rgb_led[driver_index] = new uavcan::Publisher<uavcan::equipment::indication::LightsCommand>(*_node);
-    rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    rgb_led.set_timeout_ms(20);
+    rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    buzzer[driver_index] = new uavcan::Publisher<uavcan::equipment::indication::BeepCommand>(*_node);
-    buzzer[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    buzzer[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    buzzer.set_timeout_ms(20);
+    buzzer.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    safety_state[driver_index] = new uavcan::Publisher<ardupilot::indication::SafetyState>(*_node);
-    safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    safety_state.set_timeout_ms(20);
+    safety_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    arming_status[driver_index] = new uavcan::Publisher<uavcan::equipment::safety::ArmingStatus>(*_node);
-    arming_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    arming_status[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    arming_status.set_timeout_ms(20);
+    arming_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
 #if AP_DRONECAN_SEND_GPS
-    gnss_fix2[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::Fix2>(*_node);
-    gnss_fix2[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    gnss_fix2[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+    gnss_fix2.set_timeout_ms(20);
+    gnss_fix2.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    gnss_auxiliary[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>(*_node);
-    gnss_auxiliary[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    gnss_auxiliary[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+    gnss_auxiliary.set_timeout_ms(20);
+    gnss_auxiliary.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    gnss_heading[driver_index] = new uavcan::Publisher<ardupilot::gnss::Heading>(*_node);
-    gnss_heading[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    gnss_heading[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+    gnss_heading.set_timeout_ms(20);
+    gnss_heading.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    gnss_status[driver_index] = new uavcan::Publisher<ardupilot::gnss::Status>(*_node);
-    gnss_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    gnss_status[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
+    gnss_status.set_timeout_ms(20);
+    gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 #endif
 
-    rtcm_stream[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>(*_node);
-    rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    rtcm_stream.set_timeout_ms(20);
+    rtcm_stream.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    notify_state[driver_index] = new uavcan::Publisher<ardupilot::indication::NotifyState>(*_node);
-    notify_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
-    notify_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    notify_state.set_timeout_ms(20);
+    notify_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    param_get_set_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::param::GetSet, ParamGetSetCb>(*_node, ParamGetSetCb(this, &AP_UAVCAN::handle_param_get_set_response));
+    param_save_client.set_timeout_ms(20);
+    param_save_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    param_execute_opcode_client[driver_index] = new uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ParamExecuteOpcodeCb>(*_node, ParamExecuteOpcodeCb(this, &AP_UAVCAN::handle_param_save_response));
+    param_get_set_client.set_timeout_ms(20);
+    param_get_set_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
-    safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
-    if (safety_button_listener[driver_index]) {
-        safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
-    }
+    node_status.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST);
+    node_status.set_timeout_ms(1000);
 
-    traffic_report_listener[driver_index] = new uavcan::Subscriber<ardupilot::equipment::trafficmonitor::TrafficReport, TrafficReportCb>(*_node);
-    if (traffic_report_listener[driver_index]) {
-        traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report));
-    }
+    node_info_server.set_timeout_ms(20);
 
-    actuator_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::actuator::Status, ActuatorStatusCb>(*_node);
-    if (actuator_status_listener[driver_index]) {
-        actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status));
-    }
-
-#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
-    actuator_status_Volz_listener[driver_index] = new uavcan::Subscriber<com::volz::servo::ActuatorStatus, ActuatorStatusVolzCb>(*_node);
-    if (actuator_status_Volz_listener[driver_index]) {
-        actuator_status_Volz_listener[driver_index]->start(ActuatorStatusVolzCb(this, &handle_actuator_status_Volz));
-    }
-#endif
-
-    esc_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb>(*_node);
-    if (esc_status_listener[driver_index]) {
-        esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status));
-    }
-
-    debug_listener[driver_index] = new uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb>(*_node);
-    if (debug_listener[driver_index]) {
-        debug_listener[driver_index]->start(DebugCb(this, &handle_debug));
-    }
-    
     _led_conf.devices_count = 0;
 
-    /*
-     * Informing other nodes that we're ready to work.
-     * Default mode is INITIALIZING.
-     */
-    _node->setModeOperational();
+    // setup node status
+    node_status_msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
+    node_status_msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
+    node_status_msg.sub_mode = 0;
 
     // Spin node for device discovery
-    _node->spin(uavcan::MonotonicDuration::fromMSec(5000));
+    for (uint8_t i = 0; i < 5; i++) {
+        send_node_status();
+        canard_iface.process(1000);
+    }
 
     snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index);
 
     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, UAVCAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
-        _node->setModeOfflineAndPublish();
         debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't create thread\n\r");
         return;
     }
@@ -497,7 +331,6 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     _initialized = true;
     debug_uavcan(AP_CANManager::LOG_INFO, "UAVCAN: init done\n\r");
 }
-#pragma GCC diagnostic pop
 
 void AP_UAVCAN::loop(void)
 {
@@ -507,12 +340,7 @@ void AP_UAVCAN::loop(void)
             continue;
         }
 
-        const int error = _node->spin(uavcan::MonotonicDuration::fromMSec(1));
-
-        if (error < 0) {
-            hal.scheduler->delay_microseconds(100);
-            continue;
-        }
+        canard_iface.process(1);
 
         if (_SRV_armed) {
             bool sent_servos = false;
@@ -548,7 +376,8 @@ void AP_UAVCAN::loop(void)
         notify_state_send();
         send_parameter_request();
         send_parameter_save_request();
-        _dna_server->verify_nodes();
+        send_node_status();
+        _dna_server.verify_nodes();
 #if AP_OPENDRONEID_ENABLED
         AP::opendroneid().dronecan_send(this);
 #endif
@@ -566,6 +395,26 @@ void AP_UAVCAN::loop(void)
 }
 
 
+void AP_UAVCAN::send_node_status(void)
+{
+    const uint32_t now = AP_HAL::native_millis();
+    if (now - _node_status_last_send_ms < 1000) {
+        return;
+    }
+    _node_status_last_send_ms = now;
+    node_status_msg.uptime_sec = now / 1000;
+    node_status.broadcast(node_status_msg);
+}
+
+void AP_UAVCAN::handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req)
+{
+    node_info_rsp.status = node_status_msg;
+    node_info_rsp.status.uptime_sec = AP_HAL::native_millis() / 1000;
+
+    node_info_server.respond(transfer, node_info_rsp);
+}
+
+
 ///// SRV output /////
 
 void AP_UAVCAN::SRV_send_actuator(void)
@@ -577,12 +426,12 @@ void AP_UAVCAN::SRV_send_actuator(void)
 
     do {
         repeat_send = false;
-        uavcan::equipment::actuator::ArrayCommand msg;
+        uavcan_equipment_actuator_ArrayCommand msg;
 
         uint8_t i;
         // UAVCAN can hold maximum of 15 commands in one frame
         for (i = 0; starting_servo < UAVCAN_SRV_NUMBER && i < 15; starting_servo++) {
-            uavcan::equipment::actuator::Command cmd;
+            uavcan_equipment_actuator_Command cmd;
 
             /*
              * Servo output uses a range of 1000-2000 PWM for scaling.
@@ -597,21 +446,21 @@ void AP_UAVCAN::SRV_send_actuator(void)
                 cmd.actuator_id = starting_servo + 1;
 
                 if (option_is_set(Options::USE_ACTUATOR_PWM)) {
-                    cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_PWM;
+                    cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_PWM;
                     cmd.command_value = _SRV_conf[starting_servo].pulse;
                 } else {
-                    cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
+                    cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS;
                     cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
                 }
 
-                msg.commands.push_back(cmd);
+                msg.commands.data[i] = cmd;
 
                 i++;
             }
         }
-
+        msg.commands.len = i;
         if (i > 0) {
-            if (act_out_array[_driver_index]->broadcast(msg) > 0) {
+            if (act_out_array.broadcast(msg) > 0) {
                 _srv_send_count++;
             } else {
                 _fail_send_count++;
@@ -626,8 +475,8 @@ void AP_UAVCAN::SRV_send_actuator(void)
 
 void AP_UAVCAN::SRV_send_esc(void)
 {
-    static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
-    uavcan::equipment::esc::RawCommand esc_msg;
+    static const int cmd_max = ((1<<13)-1);
+    uavcan_equipment_esc_RawCommand esc_msg;
 
     uint8_t active_esc_num = 0, max_esc_num = 0;
     uint8_t k = 0;
@@ -658,15 +507,16 @@ void AP_UAVCAN::SRV_send_esc(void)
 
                 scaled = constrain_float(scaled, 0, cmd_max);
 
-                esc_msg.cmd.push_back(static_cast<int>(scaled));
+                esc_msg.cmd.data[k] = static_cast<int>(scaled);
             } else {
-                esc_msg.cmd.push_back(static_cast<unsigned>(0));
+                esc_msg.cmd.data[k] = static_cast<unsigned>(0);
             }
 
             k++;
         }
+        esc_msg.cmd.len = k;
 
-        if (esc_raw[_driver_index]->broadcast(esc_msg) > 0) {
+        if (esc_raw.broadcast(esc_msg)) {
             _esc_send_count++;
         } else {
             _fail_send_count++;
@@ -701,7 +551,7 @@ void AP_UAVCAN::led_out_send()
         return;
     }
 
-    uavcan::equipment::indication::LightsCommand msg;
+    uavcan_equipment_indication_LightsCommand msg;
     {
         WITH_SEMAPHORE(_led_out_sem);
 
@@ -709,19 +559,16 @@ void AP_UAVCAN::led_out_send()
             return;
         }
 
-        uavcan::equipment::indication::SingleLightCommand cmd;
-
+        msg.commands.len = _led_conf.devices_count;
         for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
-            cmd.light_id =_led_conf.devices[i].led_index;
-            cmd.color.red = _led_conf.devices[i].red >> 3;
-            cmd.color.green = _led_conf.devices[i].green >> 2;
-            cmd.color.blue = _led_conf.devices[i].blue >> 3;
-
-            msg.commands.push_back(cmd);
+            msg.commands.data[i].light_id =_led_conf.devices[i].led_index;
+            msg.commands.data[i].color.red = _led_conf.devices[i].red >> 3;
+            msg.commands.data[i].color.green = _led_conf.devices[i].green >> 2;
+            msg.commands.data[i].color.blue = _led_conf.devices[i].blue >> 3;
         }
     }
 
-    rgb_led[_driver_index]->broadcast(msg);
+    rgb_led.broadcast(msg);
     _led_conf.last_update = now;
 }
 
@@ -761,7 +608,7 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t
 // buzzer send
 void AP_UAVCAN::buzzer_send()
 {
-    uavcan::equipment::indication::BeepCommand msg;
+    uavcan_equipment_indication_BeepCommand msg;
     WITH_SEMAPHORE(_buzzer.sem);
     uint8_t mask = (1U << _driver_index);
     if ((_buzzer.pending_mask & mask) == 0) {
@@ -770,7 +617,7 @@ void AP_UAVCAN::buzzer_send()
     _buzzer.pending_mask &= ~mask;
     msg.frequency = _buzzer.frequency;
     msg.duration = _buzzer.duration;
-    buzzer[_driver_index]->broadcast(msg);
+    buzzer.broadcast(msg);
 }
 
 // buzzer support
@@ -791,82 +638,83 @@ void AP_UAVCAN::notify_state_send()
         return;
     }
 
-    ardupilot::indication::NotifyState msg;
+    ardupilot_indication_NotifyState msg;
     msg.vehicle_state = 0;
     if (AP_Notify::flags.initialising) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_INITIALISING;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_INITIALISING;
     }
     if (AP_Notify::flags.armed) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_ARMED;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ARMED;
     }
     if (AP_Notify::flags.flying) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FLYING;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FLYING;
     }
     if (AP_Notify::flags.compass_cal_running) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_MAGCAL_RUN;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_MAGCAL_RUN;
     }
     if (AP_Notify::flags.ekf_bad) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_EKF_BAD;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_EKF_BAD;
     }
     if (AP_Notify::flags.esc_calibration) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_ESC_CALIBRATION;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ESC_CALIBRATION;
     }
     if (AP_Notify::flags.failsafe_battery) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_BATT;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_BATT;
     }
     if (AP_Notify::flags.failsafe_gcs) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_GCS;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_GCS;
     }
     if (AP_Notify::flags.failsafe_radio) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_RADIO;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_RADIO;
     }
     if (AP_Notify::flags.firmware_update) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FW_UPDATE;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FW_UPDATE;
     }
     if (AP_Notify::flags.gps_fusion) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_GPS_FUSION;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_FUSION;
     }
     if (AP_Notify::flags.gps_glitching) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_GPS_GLITCH;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_GLITCH;
     }
     if (AP_Notify::flags.have_pos_abs) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_POS_ABS_AVAIL;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POS_ABS_AVAIL;
     }
     if (AP_Notify::flags.leak_detected) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_LEAK_DET;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LEAK_DET;
     }
     if (AP_Notify::flags.parachute_release) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_CHUTE_RELEASED;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_CHUTE_RELEASED;
     }
     if (AP_Notify::flags.powering_off) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_POWERING_OFF;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POWERING_OFF;
     }
     if (AP_Notify::flags.pre_arm_check) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_PREARM;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM;
     }
     if (AP_Notify::flags.pre_arm_gps_check) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_PREARM_GPS;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM_GPS;
     }
     if (AP_Notify::flags.save_trim) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_SAVE_TRIM;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_SAVE_TRIM;
     }
     if (AP_Notify::flags.vehicle_lost) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_LOST;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LOST;
     }
     if (AP_Notify::flags.video_recording) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_VIDEO_RECORDING;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_VIDEO_RECORDING;
     }
     if (AP_Notify::flags.waiting_for_throw) {
-        msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_THROW_READY;
+        msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_THROW_READY;
     }
 
-    msg.aux_data_type = ardupilot::indication::NotifyState::VEHICLE_YAW_EARTH_CENTIDEGREES;
+    msg.aux_data_type = ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES;
     uint16_t yaw_cd = (uint16_t)(360.0f - degrees(AP::ahrs().get_yaw()))*100.0f;
     const uint8_t *data = (uint8_t *)&yaw_cd;
     for (uint8_t i=0; i<2; i++) {
-        msg.aux_data.push_back(data[i]);
+        msg.aux_data.data[i] = data[i];
     }
-    notify_state[_driver_index]->broadcast(msg);
+    msg.aux_data.len = 2;
+    notify_state.broadcast(msg);
     _last_notify_state_ms = AP_HAL::native_millis();
 }
 
@@ -885,16 +733,16 @@ void AP_UAVCAN::gnss_send_fix()
         send Fix2 packet
     */
 
-    uavcan::equipment::gnss::Fix2 pkt {};
+    uavcan_equipment_gnss_Fix2 pkt {};
     const Location &loc = gps.location();
     const Vector3f &vel = gps.velocity();
 
     pkt.timestamp.usec = AP_HAL::native_micros64();
     pkt.gnss_timestamp.usec = gps.time_epoch_usec();
     if (pkt.gnss_timestamp.usec == 0) {
-        pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_NONE;
+        pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_NONE;
     } else {
-        pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC;
+        pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC;
     }
     pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;
     pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;
@@ -907,53 +755,53 @@ void AP_UAVCAN::gnss_send_fix()
     switch (gps.status()) {
     case AP_GPS::GPS_Status::NO_GPS:
     case AP_GPS::GPS_Status::NO_FIX:
-        pkt.status = uavcan::equipment::gnss::Fix2::STATUS_NO_FIX;
-        pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
-        pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
+        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX;
+        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
+        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
         break;
     case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
-        pkt.status = uavcan::equipment::gnss::Fix2::STATUS_2D_FIX;
-        pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
-        pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
+        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX;
+        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
+        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
         break;
     case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
-        pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
-        pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
-        pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
+        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
+        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;
+        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;
         break;
     case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
-        pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
-        pkt.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS;
-        pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_SBAS;
+        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
+        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS;
+        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS;
         break;
     case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
-        pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
-        pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
-        pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT;
+        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
+        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
+        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT;
         break;
     case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
-        pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX;
-        pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
-        pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED;
+        pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;
+        pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;
+        pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED;
         break;
     }
 
-    pkt.covariance.resize(6);
+    pkt.covariance.len = 6;
     float hacc;
     if (gps.horizontal_accuracy(hacc)) {
-        pkt.covariance[0] = pkt.covariance[1] = sq(hacc);
+        pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc);
     }
     float vacc;
     if (gps.vertical_accuracy(vacc)) {
-        pkt.covariance[2] = sq(vacc);
+        pkt.covariance.data[2] = sq(vacc);
     }
     float sacc;
     if (gps.speed_accuracy(sacc)) {
         const float vc3 = sq(sacc);
-        pkt.covariance[3] = pkt.covariance[4] = pkt.covariance[5] = vc3;
+        pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3;
     }
 
-    gnss_fix2[_driver_index]->broadcast(pkt);
+    gnss_fix2.broadcast(pkt);
 
 
 
@@ -964,24 +812,24 @@ void AP_UAVCAN::gnss_send_fix()
         /*
         send aux packet
         */
-        uavcan::equipment::gnss::Auxiliary pkt_auxiliary {};
+        uavcan_equipment_gnss_Auxiliary pkt_auxiliary {};
         pkt_auxiliary.hdop = gps.get_hdop() * 0.01;
         pkt_auxiliary.vdop = gps.get_vdop() * 0.01;
 
-        gnss_auxiliary[_driver_index]->broadcast(pkt_auxiliary);
+        gnss_auxiliary.broadcast(pkt_auxiliary);
 
 
         /*
         send Status packet
         */
-        ardupilot::gnss::Status pkt_status {};
+        ardupilot_gnss_Status pkt_status {};
         pkt_status.healthy = gps.is_healthy();
         if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) {
-            pkt_status.status |= ardupilot::gnss::Status::STATUS_LOGGING;
+            pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING;
         }
         uint8_t idx; // unused
         if (pkt_status.healthy && !gps.first_unconfigured_gps(idx)) {
-            pkt_status.status |= ardupilot::gnss::Status::STATUS_ARMABLE;
+            pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE;
         }
 
         uint32_t error_codes;
@@ -989,7 +837,7 @@ void AP_UAVCAN::gnss_send_fix()
             pkt_status.error_codes = error_codes;
         }
 
-        gnss_status[_driver_index]->broadcast(pkt_status);
+        gnss_status.broadcast(pkt_status);
     }
 }
 
@@ -1009,13 +857,13 @@ void AP_UAVCAN::gnss_send_yaw()
 
     _gnss.last_lib_yaw_time_ms = yaw_time_ms;
 
-    ardupilot::gnss::Heading pkt_heading {};
+    ardupilot_gnss_Heading pkt_heading {};
     pkt_heading.heading_valid = true;
     pkt_heading.heading_accuracy_valid = is_positive(yaw_acc_deg);
     pkt_heading.heading_rad = radians(yaw_deg);
     pkt_heading.heading_accuracy_rad = radians(yaw_acc_deg);
 
-    gnss_heading[_driver_index]->broadcast(pkt_heading);
+    gnss_heading.broadcast(pkt_heading);
 }
 #endif // AP_DRONECAN_SEND_GPS
 
@@ -1034,20 +882,21 @@ void AP_UAVCAN::rtcm_stream_send()
         return;
     }
     _rtcm_stream.last_send_ms = now;
-    uavcan::equipment::gnss::RTCMStream msg;
+    uavcan_equipment_gnss_RTCMStream msg;
     uint32_t len = _rtcm_stream.buf->available();
     if (len > 128) {
         len = 128;
     }
-    msg.protocol_id = uavcan::equipment::gnss::RTCMStream::PROTOCOL_ID_RTCM3;
+    msg.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3;
     for (uint8_t i=0; i<len; i++) {
         uint8_t b;
         if (!_rtcm_stream.buf->read_byte(&b)) {
             return;
         }
-        msg.data.push_back(b);
+        msg.data.data[i] = b;
     }
-    rtcm_stream[_driver_index]->broadcast(msg);
+    msg.data.len = len;
+    rtcm_stream.broadcast(msg);
 }
 
 // SafetyState send
@@ -1061,26 +910,26 @@ void AP_UAVCAN::safety_state_send()
     _last_safety_state_ms = now;
 
     { // handle SafetyState
-        ardupilot::indication::SafetyState safety_msg;
+        ardupilot_indication_SafetyState safety_msg;
         switch (hal.util->safety_switch_state()) {
         case AP_HAL::Util::SAFETY_ARMED:
-            safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF;
+            safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_OFF;
             break;
         case AP_HAL::Util::SAFETY_DISARMED:
-            safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_ON;
+            safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON;
             break;
         default:
             // nothing to send
             break;
         }
-        safety_state[_driver_index]->broadcast(safety_msg);
+        safety_state.broadcast(safety_msg);
     }
 
     { // handle ArmingStatus
-        uavcan::equipment::safety::ArmingStatus arming_msg;
-        arming_msg.status = hal.util->get_soft_armed() ? uavcan::equipment::safety::ArmingStatus::STATUS_FULLY_ARMED :
-                                                      uavcan::equipment::safety::ArmingStatus::STATUS_DISARMED;
-        arming_status[_driver_index]->broadcast(arming_msg);
+        uavcan_equipment_safety_ArmingStatus arming_msg;
+        arming_msg.status = hal.util->get_soft_armed() ? UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED :
+                                                      UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED;
+        arming_status.broadcast(arming_msg);
     }
 }
 
@@ -1104,12 +953,12 @@ void AP_UAVCAN::send_RTCMStream(const uint8_t *data, uint32_t len)
 /*
   handle Button message
  */
-void AP_UAVCAN::handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb)
+void AP_UAVCAN::handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg)
 {
-    switch (cb.msg->button) {
-    case ardupilot::indication::Button::BUTTON_SAFETY: {
+    switch (msg.button) {
+    case ARDUPILOT_INDICATION_BUTTON_BUTTON_SAFETY: {
         AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton();
-        if (brdconfig && brdconfig->safety_button_handle_pressed(cb.msg->press_time)) {
+        if (brdconfig && brdconfig->safety_button_handle_pressed(msg.press_time)) {
             AP_HAL::Util::safety_state state = hal.util->safety_switch_state();
             if (state == AP_HAL::Util::SAFETY_ARMED) {
                 hal.rcout->force_safety_on();
@@ -1125,7 +974,7 @@ void AP_UAVCAN::handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Butto
 /*
   handle traffic report
  */
-void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb)
+void AP_UAVCAN::handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg)
 {
 #if HAL_ADSB_ENABLED
     AP_ADSB *adsb = AP::ADSB();
@@ -1134,7 +983,6 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
         return;
     }
 
-    const ardupilot::equipment::trafficmonitor::TrafficReport &msg = cb.msg[0];
     AP_ADSB::adsb_vehicle_t vehicle;
     mavlink_adsb_vehicle_t &pkt = vehicle.info;
 
@@ -1152,10 +1000,10 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
     }
     pkt.emitter_type = msg.traffic_type;
 
-    if (msg.alt_type == ardupilot::equipment::trafficmonitor::TrafficReport::ALT_TYPE_PRESSURE_AMSL) {
+    if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL) {
         pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
         pkt.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
-    } else if (msg.alt_type == ardupilot::equipment::trafficmonitor::TrafficReport::ALT_TYPE_WGS84) {
+    } else if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84) {
         pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE;
         pkt.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
     }
@@ -1193,15 +1041,15 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con
 /*
   handle actuator status message
  */
-void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb)
+void AP_UAVCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg)
 {
     // log as CSRV message
     AP::logger().Write_ServoStatus(AP_HAL::native_micros64(),
-                                   cb.msg->actuator_id,
-                                   cb.msg->position,
-                                   cb.msg->force,
-                                   cb.msg->speed,
-                                   cb.msg->power_rating_pct);
+                                   msg.actuator_id,
+                                   msg.position,
+                                   msg.force,
+                                   msg.speed,
+                                   msg.power_rating_pct);
 }
 
 #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
@@ -1226,27 +1074,27 @@ void AP_UAVCAN::handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_i
 /*
   handle ESC status message
  */
-void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb)
+void AP_UAVCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg)
 {
 #if HAL_WITH_ESC_TELEM
-    const uint8_t esc_offset = constrain_int16(ap_uavcan->_esc_offset.get(), 0, UAVCAN_SRV_NUMBER);
-    const uint8_t esc_index = cb.msg->esc_index + esc_offset;
+    const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, UAVCAN_SRV_NUMBER);
+    const uint8_t esc_index = msg.esc_index + esc_offset;
 
     if (!is_esc_data_index_valid(esc_index)) {
         return;
     }
 
     TelemetryData t {
-        .temperature_cdeg = int16_t((KELVIN_TO_C(cb.msg->temperature)) * 100),
-        .voltage = cb.msg->voltage,
-        .current = cb.msg->current,
+        .temperature_cdeg = int16_t((KELVIN_TO_C(msg.temperature)) * 100),
+        .voltage = msg.voltage,
+        .current = msg.current,
     };
 
-    ap_uavcan->update_rpm(esc_index, cb.msg->rpm, cb.msg->error_count);
-    ap_uavcan->update_telem_data(esc_index, t,
-        (isnan(cb.msg->current) ? 0 : AP_ESC_Telem_Backend::TelemetryType::CURRENT)
-            | (isnan(cb.msg->voltage) ? 0 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)
-            | (isnan(cb.msg->temperature) ? 0 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE));
+    update_rpm(esc_index, msg.rpm, msg.error_count);
+    update_telem_data(esc_index, t,
+        (isnan(msg.current) ? 0 : AP_ESC_Telem_Backend::TelemetryType::CURRENT)
+            | (isnan(msg.voltage) ? 0 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)
+            | (isnan(msg.temperature) ? 0 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE));
 #endif
 }
 
@@ -1261,16 +1109,15 @@ bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) {
 /*
   handle LogMessage debug
  */
-void AP_UAVCAN::handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb)
+void AP_UAVCAN::handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg)
 {
 #if HAL_LOGGING_ENABLED
-    const auto &msg = *cb.msg;
     if (AP::can().get_log_level() != AP_CANManager::LOG_NONE) {
         // log to onboard log and mavlink
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CAN[%u] %s", node_id, msg.text.c_str());
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CAN[%u] %s", transfer.source_node_id, msg.text.data);
     } else {
         // only log to onboard log
-        AP::logger().Write_MessageF("CAN[%u] %s", node_id, msg.text.c_str());
+        AP::logger().Write_MessageF("CAN[%u] %s", transfer.source_node_id, msg.text.data);
     }
 #endif
 }
@@ -1281,7 +1128,7 @@ void AP_UAVCAN::send_parameter_request()
     if (param_request_sent) {
         return;
     }
-    param_get_set_client[_driver_index]->call(param_request_node_id, param_getset_req[_driver_index]);
+    param_get_set_client.request(param_request_node_id, param_getset_req);
     param_request_sent = true;
 }
 
@@ -1293,9 +1140,10 @@ bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, float v
         //busy
         return false;
     }
-    param_getset_req[_driver_index].index = 0;
-    param_getset_req[_driver_index].name = name;
-    param_getset_req[_driver_index].value.to<uavcan::protocol::param::Value::Tag::real_value>() = value;
+    param_getset_req.index = 0;
+    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
+    param_getset_req.value.real_value = value;
+    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
     param_float_cb = cb;
     param_request_sent = false;
     param_request_node_id = node_id;
@@ -1310,9 +1158,10 @@ bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t
         //busy
         return false;
     }
-    param_getset_req[_driver_index].index = 0;
-    param_getset_req[_driver_index].name = name;
-    param_getset_req[_driver_index].value.to<uavcan::protocol::param::Value::Tag::integer_value>() = value;
+    param_getset_req.index = 0;
+    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1);
+    param_getset_req.value.integer_value = value;
+    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
     param_int_cb = cb;
     param_request_sent = false;
     param_request_node_id = node_id;
@@ -1327,9 +1176,9 @@ bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGe
         //busy
         return false;
     }
-    param_getset_req[_driver_index].index = 0;
-    param_getset_req[_driver_index].name = name;
-    param_getset_req[_driver_index].value.to<uavcan::protocol::param::Value::Tag::empty>();
+    param_getset_req.index = 0;
+    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
+    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
     param_float_cb = cb;
     param_request_sent = false;
     param_request_node_id = node_id;
@@ -1344,50 +1193,49 @@ bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGe
         //busy
         return false;
     }
-    param_getset_req[_driver_index].index = 0;
-    param_getset_req[_driver_index].name = name;
-    param_getset_req[_driver_index].value.to<uavcan::protocol::param::Value::Tag::empty>();
+    param_getset_req.index = 0;
+    param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data));
+    param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
     param_int_cb = cb;
     param_request_sent = false;
     param_request_node_id = node_id;
     return true;
 }
 
-void AP_UAVCAN::handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb)
+void AP_UAVCAN::handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp)
 {
-    WITH_SEMAPHORE(ap_uavcan->_param_sem);
-    if (!ap_uavcan->param_int_cb &&
-        !ap_uavcan->param_float_cb) {
+    WITH_SEMAPHORE(_param_sem);
+    if (!param_int_cb &&
+        !param_float_cb) {
         return;
     }
-    uavcan::protocol::param::GetSet::Response rsp = cb.rsp->getResponse();
-    if (rsp.value.is(uavcan::protocol::param::Value::Tag::integer_value) && ap_uavcan->param_int_cb) {
-        int32_t val = rsp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
-        if ((*ap_uavcan->param_int_cb)(ap_uavcan, node_id, rsp.name.c_str(), val)) {
+    if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) && param_int_cb) {
+        int32_t val = rsp.value.integer_value;
+        if ((*param_int_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
             // we want the parameter to be set with val
-            param_getset_req[ap_uavcan->_driver_index].index = 0;
-            param_getset_req[ap_uavcan->_driver_index].name = rsp.name;
-            param_getset_req[ap_uavcan->_driver_index].value.to<uavcan::protocol::param::Value::Tag::integer_value>() = val;
-            ap_uavcan->param_int_cb = ap_uavcan->param_int_cb;
-            ap_uavcan->param_request_sent = false;
-            ap_uavcan->param_request_node_id = node_id;
+            param_getset_req.index = 0;
+            memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
+            param_getset_req.value.integer_value = val;
+            param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+            param_request_sent = false;
+            param_request_node_id = transfer.source_node_id;
             return;
         }
-    } else if (rsp.value.is(uavcan::protocol::param::Value::Tag::real_value) && ap_uavcan->param_float_cb) {
-        float val = rsp.value.to<uavcan::protocol::param::Value::Tag::real_value>();
-        if ((*ap_uavcan->param_float_cb)(ap_uavcan, node_id, rsp.name.c_str(), val)) {
+    } else if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE) && param_float_cb) {
+        float val = rsp.value.real_value;
+        if ((*param_float_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) {
             // we want the parameter to be set with val
-            param_getset_req[ap_uavcan->_driver_index].index = 0;
-            param_getset_req[ap_uavcan->_driver_index].name = rsp.name;
-            param_getset_req[ap_uavcan->_driver_index].value.to<uavcan::protocol::param::Value::Tag::real_value>() = val;
-            ap_uavcan->param_float_cb = ap_uavcan->param_float_cb;
-            ap_uavcan->param_request_sent = false;
-            ap_uavcan->param_request_node_id = node_id;
+            param_getset_req.index = 0;
+            memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len);
+            param_getset_req.value.real_value = val;
+            param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
+            param_request_sent = false;
+            param_request_node_id = transfer.source_node_id;
             return;
         }
     }
-    ap_uavcan->param_int_cb = nullptr;
-    ap_uavcan->param_float_cb = nullptr;
+    param_int_cb = nullptr;
+    param_float_cb = nullptr;
 }
 
 
@@ -1397,7 +1245,7 @@ void AP_UAVCAN::send_parameter_save_request()
     if (param_save_request_sent) {
         return;
     }
-    param_execute_opcode_client[_driver_index]->call(param_save_request_node_id, param_save_req[_driver_index]);
+    param_save_client.request(param_save_request_node_id, param_save_req);
     param_save_request_sent = true;
 }
 
@@ -1409,7 +1257,7 @@ bool AP_UAVCAN::save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb)
         return false;
     }
 
-    param_save_req[_driver_index].opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
+    param_save_req.opcode = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE;
     param_save_request_sent = false;
     param_save_request_node_id = node_id;
     save_param_cb = cb;
@@ -1417,15 +1265,14 @@ bool AP_UAVCAN::save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb)
 }
 
 // handle parameter save request response
-void AP_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb)
+void AP_UAVCAN::handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp)
 {
-    WITH_SEMAPHORE(ap_uavcan->_param_save_sem);
-    if (!ap_uavcan->save_param_cb) {
+    WITH_SEMAPHORE(_param_save_sem);
+    if (!save_param_cb) {
         return;
     }
-    uavcan::protocol::param::ExecuteOpcode::Response rsp = cb.rsp->getResponse();
-    (*ap_uavcan->save_param_cb)(ap_uavcan, node_id, rsp.ok);
-    ap_uavcan->save_param_cb = nullptr;
+    (*save_param_cb)(this, transfer.source_node_id, rsp.ok);
+    save_param_cb = nullptr;
 }
 
 // Send Reboot command
@@ -1433,15 +1280,9 @@ void AP_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id
 // THIS IS NOT A THREAD SAFE API!
 void AP_UAVCAN::send_reboot_request(uint8_t node_id)
 {
-    if (_node == nullptr) {
-        return;
-    }
-    uavcan::protocol::RestartNode::Request request;
-    request.magic_number = uavcan::protocol::RestartNode::Request::MAGIC_NUMBER;
-    uavcan::ServiceClient<uavcan::protocol::RestartNode> client(*_node);
-    client.setCallback([](const uavcan::ServiceCallResult<uavcan::protocol::RestartNode>& call_result){});
-
-    client.call(node_id, request);
+    uavcan_protocol_RestartNodeRequest request;
+    request.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER;
+    restart_node_client.request(node_id, request);
 }
 
 // check if a option is set and if it is then reset it to 0.
@@ -1459,7 +1300,7 @@ bool AP_UAVCAN::check_and_reset_option(Options option)
 bool AP_UAVCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
 {
     // forward this to DNA_Server
-    return _dna_server->prearm_check(fail_msg, fail_msg_len);
+    return _dna_server.prearm_check(fail_msg, fail_msg_len);
 }
 
 /*
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
index cf11dcf773..ad2d268060 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.h
@@ -20,17 +20,20 @@
 
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
 
-#include <uavcan/uavcan.hpp>
-#include "AP_UAVCAN_IfaceMgr.h"
-#include "AP_UAVCAN_Clock.h"
+#include "AP_Canard_iface.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
-    // Such cases will be firmware update, etc.
-    class RaiiSynchronizer {};
+    CanardInterface& get_canard_iface() { return canard_iface; }
 
 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;
-    AP_UAVCAN_DNA_Server *_dna_server;
+    uint32_t *mem_pool;
 
-    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);
-    static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
-    static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb);
-    static void handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb);
-    static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
+    void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
+    void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg);
+    void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
+    void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
+    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);
-    static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);
-    static void handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb);
+    void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
+    void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);
+    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
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h b/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h
deleted file mode 100644
index eba4923a3c..0000000000
--- a/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h
+++ /dev/null
@@ -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;
-};
-}
\ No newline at end of file
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
index 0e1b50cd03..72c6019f83 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
+++ b/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
-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_DNA_Server::AP_UAVCAN_DNA_Server(AP_UAVCAN &ap_uavcan) :
     _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
-    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);
-
+    //Read the details from AP_UAVCAN
     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)) {
-        uavcan::protocol::GetNodeInfo::Request request;
-        getNodeInfo_client[driver_index]->call(curr_verifying_node, request);
+    if (isNodeIDOccupied(curr_verifying_node)) {
+        uavcan_protocol_GetNodeInfoRequest 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 ||
-        cb.msg->mode != uavcan::protocol::NodeStatus::MODE_OPERATIONAL) &&
-        !_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
+    if ((msg.health != UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK ||
+        msg.mode != UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL) &&
+        !_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::GetNodeInfo::Request request;
-            getNodeInfo_client[driver_index]->call(node_id, request);
-        }
+        uavcan_protocol_GetNodeInfoRequest request;
+        node_info_client.request(transfer.source_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()) {
-        logged.set(node_id);
+    if (!logged.get(transfer.source_node_id) && AP::logger().logging_started()) {
+        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,
-                           major,
-                           minor,
-                           vcs_commit);
+                           rsp.name.data,
+                           rsp.software_version.major,
+                           rsp.software_version.minor,
+                           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 (node_id == curr_verifying_node) {
+        if (transfer.source_node_id == getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) {
+            if (transfer.source_node_id == curr_verifying_node) {
                 nodeInfo_resp_rcvd = true;
             }
-            setVerificationMask(node_id);
-        } else if (!_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
+            setVerificationMask(transfer.source_node_id);
+        } 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;
-            memcpy(fault_node_name, name, sizeof(fault_node_name));
+            fault_node_id = transfer.source_node_id;
+            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);
-        if (node_id == curr_verifying_node) {
+        setVerificationMask(transfer.source_node_id);
+        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) {
-        //init has not been called for this driver.
-        return;
-    }
-    if (!cb.msg->isAnonymousTransfer()) {
+    if (transfer.source_node_id != 0) {
         //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++) {
-        rcvd_unique_id[i] = cb.msg->unique_id[i - rcvd_unique_id_offset];
+    for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + msg.unique_id.len); i++) {
+        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++) {
-        msg.unique_id.push_back(rcvd_unique_id[i]);
-    }
+    memcpy(rsp.unique_id.data, rcvd_unique_id, rcvd_unique_id_offset);
+    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);
-}
-
-//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);
+    allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD
 }
 
 //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;
         }
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
index 5443423d1e..0ba7a193f9 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
+++ b/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;
-    uint8_t driver_index;
+    AP_UAVCAN &_ap_uavcan;
+    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 handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb);
-    void handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit);
+    void handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg);
+    void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg);
+    void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp);
 
     //Run through the list of seen node ids for verification
     void verify_nodes();
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp
deleted file mode 100644
index bfa04b4084..0000000000
--- a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp
+++ /dev/null
@@ -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
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h
deleted file mode 100644
index 0c3b23ba18..0000000000
--- a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h
+++ /dev/null
@@ -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
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp
deleted file mode 100644
index 152381c3b8..0000000000
--- a/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp
+++ /dev/null
@@ -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
-
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_pool.h b/libraries/AP_UAVCAN/AP_UAVCAN_pool.h
deleted file mode 100644
index 2549526bb7..0000000000
--- a/libraries/AP_UAVCAN/AP_UAVCAN_pool.h
+++ /dev/null
@@ -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;
-};
diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp
index bf2c76257c..0c5d625f01 100644
--- a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp
+++ b/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
-#endif
-#define UAVCAN_NODE_POOL_BLOCK_SIZE 256
+
+static uint8_t node_memory_pool[UAVCAN_NODE_POOL_SIZE];
 
 #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::equipment::gnss::Fix2, Fix2)
-MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary)
-MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength)
-MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2);
-MSG_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure)
-MSG_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature)
-MSG_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo);
-MSG_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand)
-MSG_CB(uavcan::equipment::esc::RawCommand, RawCommand)
-MSG_CB(uavcan::equipment::indication::LightsCommand, LightsCommand);
-MSG_CB(com::hex::equipment::flow::Measurement, Measurement);
+MSG_CB(uavcan_protocol_NodeStatus, NodeStatus)
+MSG_CB(uavcan_equipment_gnss_Fix2, Fix2)
+MSG_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary)
+MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength)
+MSG_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2);
+MSG_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure)
+MSG_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature)
+MSG_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo);
+MSG_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand)
+MSG_CB(uavcan_equipment_esc_RawCommand, RawCommand)
+MSG_CB(uavcan_equipment_indication_LightsCommand, LightsCommand);
+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->setNodeID(self_node_id);
+    node_info.name.len = snprintf((char*)node_info.name.data, sizeof(node_info.name.data), "org.ardupilot:%u", driver_index);
 
-    char ndname[20];
-    snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index);
+    node_info.software_version.major = AP_UAVCAN_SW_VERS_MAJOR;
+    node_info.software_version.minor = AP_UAVCAN_SW_VERS_MINOR;
 
-    uavcan::NodeStatusProvider::NodeName name(ndname);
-    _node->setName(name);
+    node_info.hardware_version.major = AP_UAVCAN_HW_VERS_MAJOR;
+    node_info.hardware_version.minor = AP_UAVCAN_HW_VERS_MINOR;
 
-    uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
-    sw_version.major = AP_UAVCAN_SW_VERS_MAJOR;
-    sw_version.minor = AP_UAVCAN_SW_VERS_MINOR;
-    _node->setSoftwareVersion(sw_version);
+#define START_CB(mtype, cbname) Canard::allocate_sub_static_callback(cb_ ## cbname,driver_index)
 
-    uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion
-
-    hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
-    hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
-    _node->setHardwareVersion(hw_version);
-
-    int start_res = _node->start();
-    if (start_res < 0) {
-        debug_uavcan("UAVCAN: node start problem\n\r");
-        return;
-    }
-
-#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();
+    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);
 
     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) {
-        return;
+    if (AP_HAL::millis() - last_status_send > 1000) {
+        last_status_send = AP_HAL::millis();
+        send_node_status();
     }
-
-    _node->spin(uavcan::MonotonicDuration::fromMSec(1));
+    _uavcan_iface_mgr->process(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() :
-    _node_allocator(UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE)
+UAVCAN_sniffer::UAVCAN_sniffer()
 {}
 
 UAVCAN_sniffer::~UAVCAN_sniffer()
diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript
index 719ec151ba..3e81beeee1 100644
--- a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript
+++ b/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(
-        use='ap',
+    vehicle = bld.path.name
+    
+    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'],
     )