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

AP_DroneCAN: DNA_Server: move allocation operation to db 

Must be locked for the whole operation due to the find free/add
read-modify-write.

Preserves the previous behavior of sending back an ID of 0 in case of
allocation failure, for better or worse.
This commit is contained in:
Thomas Watson 2024-08-05 10:57:38 -05:00 committed by Andrew Tridgell
parent f406c62449
commit 4097e9fa42
2 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
View File

@ -504,18 +504,7 @@ void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer,
if (rcvd_unique_id_offset == 16) {
//We have received the full Unique ID, time to do allocation
uint8_t resp_node_id = db.getNodeIDForUniqueID((const uint8_t*)rcvd_unique_id, 16);
if (resp_node_id == 0) {
resp_node_id = db.findFreeNodeID(msg.node_id > MAX_NODE_ID ? 0 : msg.node_id);
if (resp_node_id != 0) {
db.addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16);
rsp.node_id = resp_node_id;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
}
} else {
rsp.node_id = resp_node_id;
}
rsp.node_id = db.handleAllocation(msg.node_id, (const uint8_t*)rcvd_unique_id);
//reset states as well
rcvd_unique_id_offset = 0;
memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
@ -524,6 +513,26 @@ void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer,
allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD
}
// handle the allocation message. returns the new node ID.
uint8_t AP_DroneCAN_DNA_Server::Database::handleAllocation(uint8_t node_id, const uint8_t unique_id[])
{
WITH_SEMAPHORE(sem);
uint8_t resp_node_id = getNodeIDForUniqueID(unique_id, 16);
if (resp_node_id == 0) {
resp_node_id = findFreeNodeID(node_id > MAX_NODE_ID ? 0 : node_id);
if (resp_node_id != 0) {
addNodeIDForUniqueID(resp_node_id, unique_id, 16);
return resp_node_id;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
return 0;
}
} else {
return resp_node_id;
}
}
//report the server state, along with failure message if any
bool AP_DroneCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
{

3
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
View File

@ -44,6 +44,9 @@ class AP_DroneCAN_DNA_Server
// handle processing the node info message. returns true if duplicate.
bool handleNodeInfo(uint8_t source_node_id, const uint8_t unique_id[]);
// handle the allocation message. returns the new node ID.
uint8_t handleAllocation(uint8_t node_id, const uint8_t unique_id[]);
//Generates 6Byte long hash from the specified unique_id
void getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const;