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

uavcan_v1: Refactor RX handling into functions

Browse Source 
Cleans up the main transfer-handling loop
This commit is contained in:
JacobCrabill 2021-02-13 08:54:33 -08:00 committed by Lorenz Meier
parent 9fd7eb5944
commit e5cf92f20d
2 changed files with 161 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -339,143 +339,17 @@ void UavcanNode::Run()
PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
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,
};
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);
}
} if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
uavcan_register_List_Response_1_0 msg;
size_t register_in_size_bits = receive.payload_size;
uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
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;
PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id);
_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));
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = gps_port_id;
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
}
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;
PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
_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));
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = bms_port_id;
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
}
result = handlePnpNodeIDAllocationData(receive);
} else if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
result = handleRegisterList(receive);
} else if (receive.port_id == bms_port_id) {
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
//TODO deserialize
/*
battery_status_s battery_status{};
battery_status.id = bms_status.battery_id;
battery_status.voltage_v = bms_status.voltage;
//battery_status.remaining = bms_status.remaining_capacity;
battery_status.timestamp = hrt_absolute_time();
_battery_status_pub.publish(battery_status);*/
result = handleBMSStatus(receive);
} else if (receive.port_id == gps_port_id) {
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
result = handleUORBSensorGPS(receive);
sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
_sensor_gps_pub.publish(*gps_msg);
}
// Deallocate the dynamic memory afterwards.
@ -561,3 +435,154 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
print_usage();
return 1;
}
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;
size_t register_in_size_bits = receive.payload_size;
uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
int result;
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;
PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id);
_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));
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = gps_port_id;
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
}
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;
PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
_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));
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = bms_port_id;
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
}
return result;
}
int UavcanNode::handleBMSStatus(const CanardTransfer &receive)
{
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);
//TODO deserialize
/*
battery_status_s battery_status{};
battery_status.id = bms_status.battery_id;
battery_status.voltage_v = bms_status.voltage;
//battery_status.remaining = bms_status.remaining_capacity;
battery_status.timestamp = hrt_absolute_time();
_battery_status_pub.publish(battery_status);
*/
return 0;
}
int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
{
PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1;
}

5
src/drivers/uavcan_v1/Uavcan.hpp
View File

@ -108,6 +108,11 @@ private:
void Run() override;
void fill_node_info();
int handlePnpNodeIDAllocationData(const CanardTransfer &receive);
int handleRegisterList(const CanardTransfer &receive);
int handleBMSStatus(const CanardTransfer &receive);
int handleUORBSensorGPS(const CanardTransfer &receive);
void *_uavcan_heap{nullptr};
CanardInterface *const _can_interface;