Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Thomas Watson 2024-09-13 21:12:48 -05:00 committed by Peter Barker
parent a9ea760cad
commit 20d04fa823
2 changed files with 16 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
View File

@ -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");

7
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
View File

@ -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);