From 0c1cf1fe12c0815879d7b3b229331d516ec7063d Mon Sep 17 00:00:00 2001
From: Thomas Watson <twatson52@icloud.com>
Date: Fri, 13 Sep 2024 21:12:48 -0500
Subject: [PATCH] AP_DroneCAN: DNAServer: remove preferred allocation support

Nothing is known to support it so it can't be tested. Removing it saves
flash and reduces complexity.
---
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp    | 46 ++++++-------------
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.h      |  7 +--
 2 files changed, 16 insertions(+), 37 deletions(-)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
index a0c09c27497b6..fe7eff7cba9f4 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
@@ -118,13 +118,21 @@ bool AP_DroneCAN_DNA_Server::Database::handle_node_info(uint8_t source_node_id,
 }
 
 // handle the allocation message. returns the allocated node ID, or 0 if allocation failed
-uint8_t AP_DroneCAN_DNA_Server::Database::handle_allocation(uint8_t node_id, const uint8_t unique_id[])
+uint8_t AP_DroneCAN_DNA_Server::Database::handle_allocation(const uint8_t unique_id[])
 {
     WITH_SEMAPHORE(sem);
 
     uint8_t resp_node_id = find_node_id(unique_id, 16);
     if (resp_node_id == 0) {
-        resp_node_id = find_free_node_id(node_id > MAX_NODE_ID ? 0 : node_id);
+        // find free node ID, starting at the max as prescribed by the standard
+        resp_node_id = MAX_NODE_ID;
+        while (resp_node_id > 0) {
+            if (!node_registered.get(resp_node_id)) {
+                break;
+            }
+            resp_node_id--;
+        }
+
         if (resp_node_id != 0) {
             create_registration(resp_node_id, unique_id, 16);
         }
@@ -132,34 +140,6 @@ uint8_t AP_DroneCAN_DNA_Server::Database::handle_allocation(uint8_t node_id, con
     return resp_node_id; // will be 0 if not found and not created
 }
 
-// search for a free node ID, starting at the preferred ID (which can be 0 if
-// none are preferred). returns 0 if none found. based on pseudocode in
-// uavcan/protocol/dynamic_node_id/1.Allocation.uavcan
-uint8_t AP_DroneCAN_DNA_Server::Database::find_free_node_id(uint8_t preferred)
-{
-    if (preferred == 0) {
-        preferred = MAX_NODE_ID;
-    }
-    // search for an ID >= preferred
-    uint8_t candidate = preferred;
-    while (candidate <= MAX_NODE_ID) {
-        if (!node_registered.get(candidate)) {
-            return candidate;
-        }
-        candidate++;
-    }
-    // search for an ID <= preferred
-    candidate = preferred;
-    while (candidate > 0) {
-        if (!node_registered.get(candidate)) {
-            return candidate;
-        }
-        candidate--;
-    }
-    // no IDs free
-    return 0;
-}
-
 // retrieve node ID that matches the given unique ID. returns 0 if not found
 uint8_t AP_DroneCAN_DNA_Server::Database::find_node_id(const uint8_t unique_id[], uint8_t size)
 {
@@ -491,7 +471,11 @@ void AP_DroneCAN_DNA_Server::handle_allocation(const CanardRxTransfer& transfer,
     rsp.unique_id.len = rcvd_unique_id_offset;
 
     if (rcvd_unique_id_offset == sizeof(rcvd_unique_id)) { // full unique ID received, allocate it!
-        rsp.node_id = db.handle_allocation(msg.node_id, rcvd_unique_id);
+        // we ignore the preferred node ID as it seems nobody uses the feature
+        // and we couldn't guarantee it anyway. we will always remember and
+        // re-assign node IDs consistently, so the node could send a status
+        // with a particular ID once then switch back to no preference for DNA
+        rsp.node_id = db.handle_allocation(rcvd_unique_id);
         rcvd_unique_id_offset = 0; // reset state for next allocation
         if (rsp.node_id == 0) { // allocation failed
             GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DroneCAN DNA allocation failed; database full");
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
index 80e36a318b51d..e08d3486c0cdc 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
@@ -60,14 +60,9 @@ class AP_DroneCAN_DNA_Server
         bool handle_node_info(uint8_t source_node_id, const uint8_t unique_id[]);
 
         // handle the allocation message. returns the allocated node ID, or 0 if allocation failed
-        uint8_t handle_allocation(uint8_t node_id, const uint8_t unique_id[]);
+        uint8_t handle_allocation(const uint8_t unique_id[]);
 
     private:
-        // search for a free node ID, starting at the preferred ID (which can be 0 if
-        // none are preferred). returns 0 if none found. based on pseudocode in
-        // uavcan/protocol/dynamic_node_id/1.Allocation.uavcan
-        uint8_t find_free_node_id(uint8_t preferred);
-
         // retrieve node ID that matches the given unique ID. returns 0 if not found
         uint8_t find_node_id(const uint8_t unique_id[], uint8_t size);