AP_DroneCAN: DNAServer: avoid resetting when server node ID changes

Avoids confusing the user and removes weirdness with multiple servers
sharing the same storage. Does leak the registration for the old ID but
in the unlikely event the table fills up the user can simply reset the
database.

We keep the check for an existing registration to avoid dirtying the
storage every boot unnecessarily. We also factor out the deletion of an
existing registration (which is very unlikely but technically possible)
to save some flash.

libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp

@@ -89,21 +89,15 @@ void AP_DroneCAN_DNA_Server::Database::reset()
 }
 
 // handle initializing the server with its own node ID and unique ID
-void AP_DroneCAN_DNA_Server::Database::init_server(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len)
+void AP_DroneCAN_DNA_Server::Database::init_server(uint8_t own_node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len)
 {
     WITH_SEMAPHORE(sem);
 
-    // ensure that its node ID and unique ID match in the database
-    const uint8_t stored_own_node_id = find_node_id(own_unique_id, own_unique_id_len);
-    static bool reset_done;
-    if (stored_own_node_id != node_id) { // cannot match if its unique ID was not found
-        // we have no record of its unique ID, do a reset
-        if (!reset_done) {
-            // only reset once per power cycle else we could wipe other servers' registrations
-            reset();
-            reset_done = true;
-        }
-        create_registration(node_id, own_unique_id, own_unique_id_len);
+    // register server node ID and unique ID if not correctly registered. note
+    // that ardupilot mixes the node ID into the unique ID so changing the node
+    // ID will "leak" the old node ID
+    if (own_node_id != find_node_id(own_unique_id, own_unique_id_len)) {
+        register_uid(own_node_id, own_unique_id, own_unique_id_len);
     }
 }
 
@@ -118,12 +112,7 @@ bool AP_DroneCAN_DNA_Server::Database::handle_node_info(uint8_t source_node_id,
             return true; // so raise as duplicate
         }
     } else {
-        // we don't know about this node ID, let's register it
-        uint8_t prev_node_id = find_node_id(unique_id, 16); // have we registered this unique ID under a different node ID?
-        if (prev_node_id != 0) {
-            delete_registration(prev_node_id); // yes, delete old registration
-        }
-        create_registration(source_node_id, unique_id, 16);
+        register_uid(source_node_id, unique_id, 16); // we don't know about this node ID, let's register it
     }
     return false; // not a duplicate
 }
@@ -205,6 +194,17 @@ void AP_DroneCAN_DNA_Server::Database::compute_uid_hash(NodeRecord &record, cons
     }
 }
 
+// register a given unique ID to a given node ID, deleting any existing registration for the unique ID
+void AP_DroneCAN_DNA_Server::Database::register_uid(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
+{
+    uint8_t prev_node_id = find_node_id(unique_id, size); // have we registered this unique ID under a different node ID?
+    if (prev_node_id != 0) {
+        delete_registration(prev_node_id); // yes, delete old node ID's registration
+    }
+    // overwrite an existing registration with this node ID, if any
+    create_registration(node_id, unique_id, size);
+}
+
 // create the registration for the given node ID and set its record's unique ID
 void AP_DroneCAN_DNA_Server::Database::create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
 {

libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h

@@ -54,7 +54,7 @@ class AP_DroneCAN_DNA_Server
         }
 
         // handle initializing the server with its own node ID and unique ID
-        void init_server(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len);
+        void init_server(uint8_t own_node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len);
 
         // handle processing the node info message. returns true if from a duplicate node
         bool handle_node_info(uint8_t source_node_id, const uint8_t unique_id[]);
@@ -74,6 +74,9 @@ class AP_DroneCAN_DNA_Server
         // fill the given record with the hash of the given unique ID
         void compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const;
 
+        // register a given unique ID to a given node ID, deleting any existing registration for the unique ID
+        void register_uid(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
+
         // create the registration for the given node ID and set its record's unique ID
         void create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size);