diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp index 9ae349e753aea..b57a8f1cbef8b 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp +++ b/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) { diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h index 9557d28ace5da..65b94ef8ed212 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h +++ b/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);