From 25631291d79dc95c6fab9e8e7faae8f431f75c51 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 23 Oct 2024 10:44:56 +0900
Subject: [PATCH] AP_DroneCAN: support FlexDebug message

---
 libraries/AP_DroneCAN/AP_DroneCAN.cpp | 58 ++++++++++++++++++++++++++-
 libraries/AP_DroneCAN/AP_DroneCAN.h   | 20 +++++++++
 2 files changed, 77 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
index 49aace0952..21243e5d08 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -128,7 +128,7 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
     // @Param: OPTION
     // @DisplayName: DroneCAN options
     // @Description: Option flags
-    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats
+    // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug
     // @User: Advanced
     AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0),
     
@@ -1503,6 +1503,62 @@ bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
     return true;
 }
 
+#if AP_SCRIPTING_ENABLED
+/*
+  handle FlexDebug message, holding a copy locally for a lua script to access
+ */
+void AP_DroneCAN::handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug &msg)
+{
+    if (!option_is_set(Options::ENABLE_FLEX_DEBUG)) {
+        return;
+    }
+
+    // find an existing element in the list
+    const uint8_t source_node = transfer.source_node_id;
+    for (auto *p = flexDebug_list; p != nullptr; p = p->next) {
+        if (p->node_id == source_node && p->msg.id == msg.id) {
+            p->msg = msg;
+            p->timestamp_us = uint32_t(transfer.timestamp_usec);
+            return;
+        }
+    }
+
+    // new message ID, add to the list. Note that this gets called
+    // only from one thread, so no lock needed
+    auto *p = NEW_NOTHROW FlexDebug;
+    if (p == nullptr) {
+        return;
+    }
+    p->node_id = source_node;
+    p->msg = msg;
+    p->timestamp_us = uint32_t(transfer.timestamp_usec);
+    p->next = flexDebug_list;
+
+    // link into the list
+    flexDebug_list = p;
+}
+
+/*
+  get the last FlexDebug message from a node
+ */
+bool AP_DroneCAN::get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t &timestamp_us, dronecan_protocol_FlexDebug &msg) const
+{
+    for (const auto *p = flexDebug_list; p != nullptr; p = p->next) {
+        if (p->node_id == node_id && p->msg.id == msg_id) {
+            if (timestamp_us == p->timestamp_us) {
+                // stale message
+                return false;
+            }
+            timestamp_us = p->timestamp_us;
+            msg = p->msg;
+            return true;
+        }
+    }
+    return false;
+}
+
+#endif // AP_SCRIPTING_ENABLED
+
 /*
   handle LogMessage debug
  */
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
index 7264fad947..2d6e5ae2b7 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -148,6 +148,7 @@ public:
         USE_HIMARK_SERVO          = (1U<<6),
         USE_HOBBYWING_ESC         = (1U<<7),
         ENABLE_STATS              = (1U<<8),
+        ENABLE_FLEX_DEBUG         = (1U<<9),
     };
 
     // check if a option is set
@@ -176,6 +177,10 @@ public:
     Canard::Publisher<uavcan_equipment_hardpoint_Command> relay_hardpoint{canard_iface};
 #endif
 
+#if AP_SCRIPTING_ENABLED
+    bool get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t &timestamp_us, dronecan_protocol_FlexDebug &msg) const;
+#endif
+
 private:
     void loop(void);
 
@@ -363,6 +368,11 @@ private:
     Canard::Server<uavcan_protocol_GetNodeInfoRequest> node_info_server{canard_iface, node_info_req_cb};
     uavcan_protocol_GetNodeInfoResponse node_info_rsp;
 
+#if AP_SCRIPTING_ENABLED
+    Canard::ObjCallback<AP_DroneCAN, dronecan_protocol_FlexDebug> FlexDebug_cb{this, &AP_DroneCAN::handle_FlexDebug};
+    Canard::Subscriber<dronecan_protocol_FlexDebug> FlexDebug_listener{FlexDebug_cb, _driver_index};
+#endif
+    
 #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
     /*
       Hobbywing ESC support. Note that we need additional meta-data as
@@ -409,6 +419,16 @@ private:
     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);
+
+#if AP_SCRIPTING_ENABLED
+    void handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug& msg);
+    struct FlexDebug {
+        struct FlexDebug *next;
+        uint32_t timestamp_us;
+        uint8_t node_id;
+        dronecan_protocol_FlexDebug msg;
+    } *flexDebug_list;
+#endif
 };
 
 #endif // #if HAL_ENABLE_DRONECAN_DRIVERS