Skip to content

Commit

Permalink
AP_DroneCAN: DNAServer: optimize allocation strategy
Browse files Browse the repository at this point in the history
Number of allocation messages with 3 nodes (6 trials):

before: 44, 36, 35, 92, 107, 41

after: 28, 28, 28, 28, 26, 28
  • Loading branch information
tpwrules committed Sep 14, 2024
1 parent cbba88f commit 0a471e8
Showing 1 changed file with 12 additions and 9 deletions.
21 changes: 12 additions & 9 deletions libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -450,15 +450,18 @@ void AP_DroneCAN_DNA_Server::handle_allocation(const CanardRxTransfer& transfer,
}
uint32_t now = AP_HAL::millis();

if (rcvd_unique_id_offset == 0 ||
(now - last_alloc_msg_ms) > UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) {
if (msg.first_part_of_unique_id) {
rcvd_unique_id_offset = 0;
} else {
return; // only accepting the first part
}
} else if (msg.first_part_of_unique_id) {
return; // only accepting follow up messages
if ((now - last_alloc_msg_ms) > UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) {
rcvd_unique_id_offset = 0; // reset state, timed out
}

if (msg.first_part_of_unique_id) {
// nodes waiting to send a follow up will instead send a first part
// again if they see another allocation message. therefore we should
// always reset and process a received first part, since any node we
// were allocating will never send its follow up and we'd just time out
rcvd_unique_id_offset = 0;
} else if (rcvd_unique_id_offset == 0) {
return; // not first part but we are expecting one, ignore
}

if (rcvd_unique_id_offset) {
Expand Down

0 comments on commit 0a471e8

Please sign in to comment.