From 3c27aa066ee9e041837f39d29f13dc70134dc8bc Mon Sep 17 00:00:00 2001
From: Siddharth Purohit <siddharthbharatpurohit@gmail.com>
Date: Thu, 21 Dec 2023 09:02:14 +0530
Subject: [PATCH] AP_DroneCAN: allow compilint AP_Canard_iface for AP_Periph

---
 libraries/AP_DroneCAN/AP_Canard_iface.cpp | 47 ++++++++++++++---------
 libraries/AP_DroneCAN/AP_Canard_iface.h   |  7 +++-
 2 files changed, 33 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
index 86bedca40e680..f6b7e46541685 100644
--- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp
+++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
@@ -1,7 +1,6 @@
 #include "AP_Canard_iface.h"
 #include <AP_HAL/AP_HAL.h>
 #include <AP_CANManager/AP_CANManager.h>
-#if HAL_ENABLE_DRONECAN_DRIVERS
 #include <canard/handler_list.h>
 #include <canard/transfer_object.h>
 #include <AP_Math/AP_Math.h>
@@ -21,6 +20,12 @@ DEFINE_HANDLER_LIST_SEMAPHORES();
 DEFINE_TRANSFER_OBJECT_HEADS();
 DEFINE_TRANSFER_OBJECT_SEMAPHORES();
 
+#if HAL_ENABLE_DRONECAN_DRIVERS
+#define CAN_LOG(...) AP::can().log_text(__VA_ARGS__)
+#else
+#define CAN_LOG(...) do {} while(0)
+#endif
+
 #if AP_TEST_DRONECAN_DRIVERS
 CanardInterface* CanardInterface::canard_ifaces[] = {nullptr, nullptr, nullptr};
 CanardInterface CanardInterface::test_iface{2};
@@ -93,11 +98,12 @@ bool CanardInterface::broadcast(const Canard::Transfer &bcast_transfer) {
 #endif
         .deadline_usec = AP_HAL::micros64() + (bcast_transfer.timeout_ms * 1000),
 #if CANARD_MULTI_IFACE
-        .iface_mask = uint8_t((1<<num_ifaces) - 1),
+        .iface_mask = bcast_transfer.iface_mask,
 #endif
     };
     // do canard broadcast
-    int16_t ret = canardBroadcastObj(&canard, &tx_transfer);
+    int16_t ret;
+    ret = canardBroadcastObj(&canard, &tx_transfer);
 #if AP_TEST_DRONECAN_DRIVERS
     if (this == &test_iface) {
         test_iface_sem.give();
@@ -130,7 +136,7 @@ bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfe
 #endif
         .deadline_usec = AP_HAL::micros64() + (req_transfer.timeout_ms * 1000),
 #if CANARD_MULTI_IFACE
-        .iface_mask = uint8_t((1<<num_ifaces) - 1),
+        .iface_mask = req_transfer.iface_mask,
 #endif
     };
     // do canard request
@@ -162,7 +168,7 @@ bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfe
 #endif
         .deadline_usec = AP_HAL::micros64() + (res_transfer.timeout_ms * 1000),
 #if CANARD_MULTI_IFACE
-        .iface_mask = uint8_t((1<<num_ifaces) - 1),
+        .iface_mask = res_transfer.iface_mask,
 #endif
     };
     // do canard respond
@@ -215,17 +221,15 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
         if (txq == nullptr) {
             return;
         }
-        // volatile as the value can change at any time during can interrupt
-        // we need to ensure that this is not optimized
-        volatile const auto *stats = ifaces[iface]->get_statistics();
-        uint64_t last_transmit_us = stats==nullptr?0:stats->last_transmit_us;
+        const auto *stats = ifaces[iface]->get_statistics();
         bool iface_down = true;
-        if (stats == nullptr || (AP_HAL::micros64() - last_transmit_us) < 200000UL) {
+        if (stats == nullptr || (AP_HAL::micros64() - stats->last_transmit_us) < 200000UL) {
             /*
             We were not able to queue the frame for
             sending. Only mark the send as failing if the
             interface is active. We consider an interface as
-            active if it has had successful transmits for some time.
+            active if it has had a successful transmit in the
+            last 500 milliseconds
             */
             iface_down = false;
         } 
@@ -420,31 +424,38 @@ void CanardInterface::process(uint32_t duration_ms) {
 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");
+        CAN_LOG(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");
+        CAN_LOG(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");
+        CAN_LOG(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");
+        CAN_LOG(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");
+        CAN_LOG(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));
+    CAN_LOG(AP_CANManager::LOG_INFO, LOG_TAG, "DroneCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces));
     num_ifaces++;
     return true;
 }
 
+uint16_t CanardInterface::pool_peak_percent()
+{
+    const CanardPoolAllocatorStatistics stats = canardGetPoolAllocatorStatistics(&canard);
+    const uint16_t peak_percent = (uint16_t)(100U * stats.peak_usage_blocks / stats.capacity_blocks);
+    return peak_percent;
+}
+
 // add an 11 bit auxillary driver
 bool CanardInterface::add_11bit_driver(CANSensor *sensor)
 {
@@ -468,5 +479,3 @@ bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_
     }
     return ret;
 }
-
-#endif // #if HAL_ENABLE_DRONECAN_DRIVERS
diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h
index ce9b75ee923d7..aa813974de15e 100644
--- a/libraries/AP_DroneCAN/AP_Canard_iface.h
+++ b/libraries/AP_DroneCAN/AP_Canard_iface.h
@@ -1,6 +1,5 @@
 #pragma once
 #include <AP_HAL/AP_HAL.h>
-#if HAL_ENABLE_DRONECAN_DRIVERS
 #include <canard/interface.h>
 #include <dronecan_msgs.h>
 
@@ -64,6 +63,11 @@ class CanardInterface : public Canard::Interface {
     void update_rx_protocol_stats(int16_t res);
 
     uint8_t get_node_id() const override { return canard.node_id; }
+    void set_node_id(uint8_t node_id) { canardSetLocalNodeID(&canard, node_id); }
+
+    uint16_t pool_peak_percent();
+    const dronecan_protocol_Stats& get_protocol_stats() const { return protocol_stats; }
+
 private:
     CanardInstance canard;
     AP_HAL::CANIface* ifaces[HAL_NUM_CAN_IFACES];
@@ -82,4 +86,3 @@ class CanardInterface : public Canard::Interface {
     // auxillary 11 bit CANSensor
     CANSensor *aux_11bit_driver;
 };
-#endif // HAL_ENABLE_DRONECAN_DRIVERS