From 5f66d2b4dee5ba29e7eafc91713caf399cee333e Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Wed, 14 Feb 2024 14:13:13 +0000
Subject: [PATCH] AP_DroneCAN: DNA Server: log duplicate nodes in CAND

---
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp    | 23 +++++++++++++++----
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.h      |  1 +
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
index 1164d4f7346c5..39eba542f2786 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
@@ -357,6 +357,7 @@ void AP_DroneCAN_DNA_Server::verify_nodes()
     if (log_count != last_logging_count) {
         last_logging_count = log_count;
         logged.clearall();
+        logged_duplicate.clearall();
     }
 #endif
 
@@ -437,12 +438,27 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
     if (transfer.source_node_id > MAX_NODE_ID) {
         return;
     }
+
+    // Check if we have seen this node ID before
+    const bool seen_node_id = isNodeIDOccupied(transfer.source_node_id);
+
+    // Get node ID for this unique ID if it has been seen before
+    const uint8_t prev_node_id = getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16);
+
+    // Unique ID for this node is the same as the one registered
+    const bool unique_id_match = transfer.source_node_id == prev_node_id;
+
     /*
       if we haven't logged this node then log it now
      */
 #if HAL_LOGGING_ENABLED
-    if (!logged.get(transfer.source_node_id) && AP::logger().logging_started()) {
+    const bool duplicate_node = seen_node_id && !unique_id_match;
+    const bool log_duplicate = duplicate_node && !logged_duplicate.get(transfer.source_node_id);
+    if (AP::logger().logging_started() && (!logged.get(transfer.source_node_id) || log_duplicate)) {
         logged.set(transfer.source_node_id);
+        if (duplicate_node) {
+            logged_duplicate.set(transfer.source_node_id);
+        }
         uint64_t uid[2];
         memcpy(uid, rsp.hardware_version.unique_id, sizeof(rsp.hardware_version.unique_id));
         // @LoggerMessage: CAND
@@ -467,9 +483,9 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
     }
 #endif
 
-    if (isNodeIDOccupied(transfer.source_node_id)) {
+    if (seen_node_id) {
         //if node_id already registered, just verify if Unique ID matches as well
-        if (transfer.source_node_id == getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) {
+        if (unique_id_match) {
             if (transfer.source_node_id == curr_verifying_node) {
                 nodeInfo_resp_rcvd = true;
             }
@@ -484,7 +500,6 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
     } 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(rsp.hardware_version.unique_id, 16);
         if (prev_node_id != 255) {
             //yes we did, remove this registration
             freeNodeID(prev_node_id);
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
index ee58191e803c3..ed39edb78e3c6 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
@@ -40,6 +40,7 @@ class AP_DroneCAN_DNA_Server
     Bitmask<128> verified_mask;
     Bitmask<128> node_seen_mask;
     Bitmask<128> logged;
+    Bitmask<128> logged_duplicate;
     Bitmask<128> node_healthy_mask;
 
     uint8_t last_logging_count;