uavcan_v1: Refactor RX handling into functions

Cleans up the main transfer-handling loop

src/drivers/uavcan_v1/Uavcan.cpp

@@ -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;
+				result = handlePnpNodeIDAllocationData(receive);
-
-				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);
-					}
-				}
 
+			} 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);
+				result = handleBMSStatus(receive);
-				//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);*/
 
 			} 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;
+}

src/drivers/uavcan_v1/Uavcan.hpp

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