Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: ignore first alloc messages while waiting for followup

Browse Source
This commit is contained in:
Siddharth Purohit 2020-05-31 18:17:45 +05:30 committed by Andrew Tridgell
parent bb460647a5
commit e473eed368
2 changed files with 29 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
View File

@ -585,14 +585,27 @@ void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t driver_index, uint8_t node_i
return;
}
if (cb.msg->first_part_of_unique_id) {
rcvd_unique_id_offset = 0;
memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
} else if (rcvd_unique_id_offset == 0) {
//we are only accepting first part
if (rcvd_unique_id_offset == 0 ||
(now - last_alloc_msg_ms) > 500) {
if (cb.msg->first_part_of_unique_id) {
rcvd_unique_id_offset = 0;
memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
} else {
//we are only accepting first part
return;
}
} else if (cb.msg->first_part_of_unique_id) {
// we are only accepting follow up messages
return;
}
if (rcvd_unique_id_offset) {
debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting Followup part! %u\n", uavcan::SystemClock::instance().getMonotonic().toUSec()/1000, (now - last_alloc_msg_ms));
} else {
debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting First part! %u\n", uavcan::SystemClock::instance().getMonotonic().toUSec()/1000, (now - last_alloc_msg_ms));
}
last_alloc_msg_ms = now;
if ((rcvd_unique_id_offset + cb.msg->unique_id.size()) > 16) {
//This request is malformed, Reset!
rcvd_unique_id_offset = 0;
@ -609,6 +622,12 @@ void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t driver_index, uint8_t node_i
//send follow up message
uavcan::protocol::dynamic_node_id::Allocation msg;
/* Respond with the message containing the received unique ID so far
or with node id if we successfully allocated one. */
for (uint8_t i = 0; i < rcvd_unique_id_offset; i++) {
msg.unique_id.push_back(rcvd_unique_id[i]);
}
if (rcvd_unique_id_offset == 16) {
//We have received the full Unique ID, time to do allocation
uint8_t resp_node_id = getNodeIDForUniqueID((const uint8_t*)rcvd_unique_id, 16);
@ -624,13 +643,11 @@ void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t driver_index, uint8_t node_i
} else {
msg.node_id = resp_node_id;
}
//reset states as well
rcvd_unique_id_offset = 0;
memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
}
/* Respond with the message containing the received unique ID so far
or with node id if we successfully allocated one. */
for (uint8_t i = 0; i < rcvd_unique_id_offset; i++) {
msg.unique_id.push_back(rcvd_unique_id[i]);
}
allocation_pub[driver_index]->broadcast(msg);
}

2
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
View File

@ -5,6 +5,7 @@
#include <uavcan/uavcan.hpp>
#include <AP_Common/Bitmask.h>
#include <StorageManager/StorageManager.h>
#include <AP_CANManager/AP_CANManager.h>
//Forward declaring classes
class AllocationCb;
@ -48,6 +49,7 @@ class AP_UAVCAN_DNA_Server
uint8_t rcvd_unique_id_offset;
uint8_t current_driver_index;
uint32_t last_activity_ms;
uint32_t last_alloc_msg_ms;
//Methods to handle and report Node IDs seen on the bus
void addToSeenNodeMask(uint8_t node_id);