fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-11 14:11:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCANv1 Move PNP from UAVCAN.cpp to NodeManager.cpp

Browse Source
This commit is contained in:
Peter van der Perk
2021-03-07 21:49:05 +01:00
committed by Lorenz Meier
parent d5e468a19e
commit 28c76663cd
6 changed files with 317 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Uavcan.cpp
+15 -69
View File
@@ -168,14 +168,6 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
// Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_pnp_v1_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
@@ -190,8 +182,6 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_list_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
bms_port_id,
@@ -256,14 +246,15 @@ void UavcanNode::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s &&
_node_register_setup != CANARD_NODE_ID_UNSET &&
_node_register_request_index == _node_register_last_received_index + 1) {
if (hrt_elapsed_time(&_node_manager._uavcan_pnp_nodeidallocation_last) >= 1_s &&
_node_manager._node_register_setup != CANARD_NODE_ID_UNSET &&
_node_manager._node_register_request_index == _node_manager._node_register_last_received_index + 1) {
PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index);
PX4_INFO("NodeID %i request register %i", _node_manager._node_register_setup,
_node_manager._node_register_request_index);
uavcan_register_List_Request_1_0 msg;
msg.index = _node_register_request_index;
msg.index = _node_manager._node_register_request_index;
uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
@@ -272,7 +263,7 @@ void UavcanNode::Run()
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = _node_register_setup,
.remote_node_id = _node_manager._node_register_setup,
.transfer_id = _uavcan_register_list_request_transfer_id,
.payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
@@ -285,7 +276,7 @@ void UavcanNode::Run()
++_uavcan_register_list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &request);
++_node_register_request_index;
++_node_manager._node_register_request_index;
}
}
@@ -349,10 +340,7 @@ void UavcanNode::Run()
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
result = handlePnpNodeIDAllocationData(receive);
} else if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
result = handleRegisterList(receive);
} else if (receive.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
@@ -504,48 +492,6 @@ void UavcanNode::sendHeartbeat()
}
}
int UavcanNode::handlePnpNodeIDAllocationData(const CanardTransfer &receive)
{
uavcan_pnp_NodeIDAllocationData_1_0 msg;
size_t pnp_in_size_bits = receive.payload_size;
uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &pnp_in_size_bits);
//TODO internal database with unique id to node ip mappings now we give an hardcoded ID back
msg.allocated_node_id.count = 1;
msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID
_uavcan_pnp_nodeidallocation_last = hrt_absolute_time();
_node_register_request_index = 0;
_node_register_last_received_index = -1;
_node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured
PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.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.
.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_,
.payload = &node_id_alloc_payload_buffer,
};
int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
return result;
}
int UavcanNode::handleRegisterList(const CanardTransfer &receive)
{
uavcan_register_List_Response_1_0 msg;
@@ -557,9 +503,9 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id",
msg.name.name.count) == 0) { //Demo GPS status publisher
_node_register_setup = CANARD_NODE_ID_UNSET;
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id);
_node_register_last_received_index++;
_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));
@@ -592,9 +538,9 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
} else if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id",
msg.name.name.count) == 0) { //Battery status publisher
_node_register_setup = CANARD_NODE_ID_UNSET;
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
_node_register_last_received_index++;
_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));
@@ -628,8 +574,8 @@ int UavcanNode::handleRegisterList(const CanardTransfer &receive)
uavcan_register_Value_1_0 out_value;
if (_param_manager.GetParamByName(msg.name, out_value)) {
_node_register_setup = CANARD_NODE_ID_UNSET;
_node_register_last_received_index++;
_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));