From 18a8d89fa47794bb17f5b1676d9420ac05acc861 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Mon, 1 Mar 2021 10:57:38 +0100 Subject: [PATCH] PNP fixed allocation scheme Fixed register scheme Incrased wq:uavcan stack since it overflowed when sending register response --- .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/drivers/uavcan_v1/NodeManager.cpp | 25 +++++++++---------- src/drivers/uavcan_v1/Uavcan.cpp | 3 ++- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index de096e127c..714408ef4c 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -75,7 +75,7 @@ static constexpr wq_config_t INS3{"wq:INS3", 6000, -17}; static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; -static constexpr wq_config_t uavcan{"wq:uavcan", 2576, -19}; +static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; static constexpr wq_config_t UART0{"wq:UART0", 1400, -21}; static constexpr wq_config_t UART1{"wq:UART1", 1400, -22}; diff --git a/src/drivers/uavcan_v1/NodeManager.cpp b/src/drivers/uavcan_v1/NodeManager.cpp index 841c09188e..8f11d7ed90 100644 --- a/src/drivers/uavcan_v1/NodeManager.cpp +++ b/src/drivers/uavcan_v1/NodeManager.cpp @@ -48,19 +48,18 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET; /* Search for an available NodeID to assign */ - if (msg.allocated_node_id.elements[0].value == CANARD_NODE_ID_UNSET) { - for (uint32_t i = 1; i < 16; i++) { // Note we're node ID 0 - if (nodeid_registry[i].node_id == 0) { // Unused - nodeid_registry[i].node_id = 1; - memcpy(&nodeid_registry[i].unique_id, &msg.unique_id_hash, 6); - break; + for (uint32_t i = 1; i < 16; i++) { + if (i == _canard_instance.node_id) { + continue; // Don't give our NodeID to a node - } else { - if (memcmp(&nodeid_registry[i].unique_id[0], &msg.unique_id_hash, 6) == 0) { - msg.allocated_node_id.elements[0].value = nodeid_registry[i].node_id; // Existing NodeID - break; - } - } + } else if (nodeid_registry[i].node_id == 0) { // Unused + nodeid_registry[i].node_id = i; + memcpy(&nodeid_registry[i].unique_id, &msg.unique_id_hash, 6); + break; + + } else if (memcmp(&nodeid_registry[i].unique_id[0], &msg.unique_id_hash, 6) == 0) { + msg.allocated_node_id.elements[0].value = nodeid_registry[i].node_id; // Existing NodeID + break; } } @@ -79,7 +78,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, - .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id, .payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 5d0dca0b9d..4abf99f7cf 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -572,10 +572,11 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive) } else { uavcan_register_Value_1_0 out_value; + _node_manager._node_register_last_received_index++; + _node_manager._uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); // Reset timer for next request if (_param_manager.GetParamByName(msg.name, out_value)) { _node_manager._node_register_setup = CANARD_NODE_ID_UNSET; - _node_manager._node_register_last_received_index++; uavcan_register_Access_Request_1_0 request_msg; memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));