From 49132f41a0e91ecfcab26762d9d05ae24e43c265 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 | 52 +++++++++++++++++++++++++++
 libraries/AP_DroneCAN/AP_DroneCAN.h   | 19 ++++++++++
 2 files changed, 71 insertions(+)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
index 49aace0952631..7342a8e84f5af 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -1503,6 +1503,58 @@ 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)
+{
+    // 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 7264fad9472fc..52817861a91bf 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -176,6 +176,10 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     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 +367,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     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 +418,16 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     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