uavcan_v1: Refactor RX handling into functions

Cleans up the main transfer-handling loop

src/drivers/uavcan_v1/Uavcan.cpp

@@ -339,13 +339,111 @@ void UavcanNode::Run()
 			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_) {
+				result = handleRegisterList(receive);
+
+			} else if (receive.port_id == bms_port_id) {
+				result = handleBMSStatus(receive);
+
+			} else if (receive.port_id == gps_port_id) {
+				result = handleUORBSensorGPS(receive);
+
+			}
+
+			// Deallocate the dynamic memory afterwards.
+			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
+
+		} else {
+			//PX4_INFO("RX canard %d\r\n", result);
+		}
+	}
+
+	perf_end(_cycle_perf);
+
+	if (_task_should_exit.load()) {
+		_can_interface->close();
+
+		ScheduleClear();
+		_instance = nullptr;
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+void UavcanNode::print_info()
+{
+	pthread_mutex_lock(&_node_mutex);
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+static void print_usage()
+{
+	PX4_INFO("usage: \n"
+		 "\tuavcannode {start|status|stop}");
+}
+
+extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		print_usage();
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (UavcanNode::instance()) {
+			PX4_ERR("already started");
+			return 1;
+		}
+
+		// CAN bitrate
+		int32_t bitrate = 0;
+		param_get(param_find("UAVCAN_V1_BAUD"), &bitrate);
+
+		// Node ID
+		int32_t node_id = 0;
+		param_get(param_find("UAVCAN_V1_ID"), &node_id);
+
+		// Start
+		PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate);
+		return UavcanNode::start(node_id, bitrate);
+	}
+
+	/* commands below require the app to be started */
+	UavcanNode *const inst = UavcanNode::instance();
+
+	if (!inst) {
+		PX4_ERR("application not running");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "status") || !strcmp(argv[1], "info")) {
+		inst->print_info();
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		delete inst;
+		return 0;
+	}
+
+	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
 
@@ -369,7 +467,7 @@ void UavcanNode::Run()
 		.payload        = &node_id_alloc_payload_buffer,
 	};
 
-				result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size);
+	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
@@ -377,13 +475,17 @@ void UavcanNode::Run()
 		result = canardTxPush(&_canard_instance, &transfer);
 	}
 
-			} if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
+	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
@@ -457,107 +559,30 @@ void UavcanNode::Run()
 		}
 	}
 
+	return result;
+}
 
-			} else if (receive.port_id == bms_port_id) {
+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);*/
-
-			} else if (receive.port_id == gps_port_id) {
-				PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
-
-				sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
-
-				_sensor_gps_pub.publish(*gps_msg);
-			}
-
-			// Deallocate the dynamic memory afterwards.
-			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
-
-		} else {
-			//PX4_INFO("RX canard %d\r\n", result);
-		}
-	}
+	_battery_status_pub.publish(battery_status);
+	*/
 
-	perf_end(_cycle_perf);
-
-	if (_task_should_exit.load()) {
-		_can_interface->close();
-
-		ScheduleClear();
-		_instance = nullptr;
-	}
-
-	pthread_mutex_unlock(&_node_mutex);
-}
-
-void UavcanNode::print_info()
-{
-	pthread_mutex_lock(&_node_mutex);
-
-	perf_print_counter(_cycle_perf);
-	perf_print_counter(_interval_perf);
-
-	pthread_mutex_unlock(&_node_mutex);
-}
-
-static void print_usage()
-{
-	PX4_INFO("usage: \n"
-		 "\tuavcannode {start|status|stop}");
+	return 0;
 }
 
-extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[])
+int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
 {
-	if (argc < 2) {
-		print_usage();
-		return 1;
-	}
-
-	if (!strcmp(argv[1], "start")) {
-		if (UavcanNode::instance()) {
-			PX4_ERR("already started");
-			return 1;
-		}
-
-		// CAN bitrate
-		int32_t bitrate = 0;
-		param_get(param_find("UAVCAN_V1_BAUD"), &bitrate);
-
-		// Node ID
-		int32_t node_id = 0;
-		param_get(param_find("UAVCAN_V1_ID"), &node_id);
-
-		// Start
-		PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate);
-		return UavcanNode::start(node_id, bitrate);
-	}
-
-	/* commands below require the app to be started */
-	UavcanNode *const inst = UavcanNode::instance();
-
-	if (!inst) {
-		PX4_ERR("application not running");
-		return 1;
-	}
-
-	if (!strcmp(argv[1], "status") || !strcmp(argv[1], "info")) {
-		inst->print_info();
-		return 0;
-	}
+	PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
 
-	if (!strcmp(argv[1], "stop")) {
-		delete inst;
-		return 0;
-	}
+	sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
 
-	print_usage();
-	return 1;
+	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;