yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

PNP fixed allocation scheme

Browse Source 
Fixed register scheme
Incrased wq:uavcan stack since it overflowed when sending register response
This commit is contained in:
Peter van der Perk
2021-03-01 10:57:38 +01:00
committed by Lorenz Meier
parent 28c76663cd
commit 18a8d89fa4
3 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

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

25
src/drivers/uavcan_v1/NodeManager.cpp
View File

@@ -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_,

3
src/drivers/uavcan_v1/Uavcan.cpp
View File

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