drivers/uavcan: make firmware server available on both CAN1/CAN2 always (if enabled)

- uavcan firmware server no longer shuts down when arming (nodes might restart in flight)
 - always handle UAVCAN parameters with or without the FW server active
 - remove legacy ESC enumeration in FW server

ROMFS/px4fmu_common/init.d/rcS

@@ -291,15 +291,9 @@ else
 		# Start core UAVCAN module.
 		if uavcan start
 		then
-			if param greater UAVCAN_ENABLE 1
+			if param greater UAVCAN_ENABLE 2
 			then
-				# Start UAVCAN firmware update server and dynamic node ID allocation server.
+				set OUTPUT_MODE uavcan_esc
-				uavcan start fw
-
-				if param greater UAVCAN_ENABLE 2
-				then
-					set OUTPUT_MODE uavcan_esc
-				fi
 			fi
 		else
 			tune_control play error

src/drivers/uavcan/uavcan_main.cpp

@@ -84,15 +84,16 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_esc_controller(_node),
 	_servo_controller(_node),
 	_hardpoint_controller(_node),
-	_beep_controller(_node),
 	_safety_state_controller(_node),
 	_rgbled_controller(_node),
 	_time_sync_master(_node),
 	_time_sync_slave(_node),
 	_node_status_monitor(_node),
-	_cycle_perf(perf_alloc(PC_ELAPSED, "uavcan: cycle time")),
+	_node_info_retriever(_node),
-	_interval_perf(perf_alloc(PC_INTERVAL, "uavcan: cycle interval")),
+	_master_timer(_node),
-	_master_timer(_node)
+	_param_getset_client(_node),
+	_param_opcode_client(_node),
+	_param_restartnode_client(_node)
 {
 	int res = pthread_mutex_init(&_node_mutex, nullptr);
 
@@ -100,23 +101,16 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 		std::abort();
 	}
 
-	res = px4_sem_init(&_server_command_sem, 0, 0);
-
-	if (res < 0) {
-		std::abort();
-	}
-
-	/* _server_command_sem use case is a signal */
-	px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
-
 	_mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
 	_mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ);
-
 }
 
 UavcanNode::~UavcanNode()
 {
-	fw_server(Stop);
+	if (_servers != nullptr) {
+		delete _servers;
+		_servers = nullptr;
+	}
 
 	if (_instance) {
 
@@ -141,7 +135,6 @@ UavcanNode::~UavcanNode()
 	_sensor_bridges.clear();
 
 	pthread_mutex_destroy(&_node_mutex);
-	px4_sem_destroy(&_server_command_sem);
 
 	perf_free(_cycle_perf);
 	perf_free(_interval_perf);
@@ -192,15 +185,6 @@ UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
 	return -1;
 }
 
-void
-UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
-{
-	uavcan::protocol::param::ExecuteOpcode::Response resp;
-	_callback_success = result.isSuccessful();
-	resp = result.getResponse();
-	_callback_success &= resp.ok;
-}
-
 int
 UavcanNode::save_params(int remote_node_id)
 {
@@ -225,15 +209,6 @@ UavcanNode::save_params(int remote_node_id)
 	return 0;
 }
 
-void
-UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
-{
-	uavcan::protocol::RestartNode::Response resp;
-	_callback_success = result.isSuccessful();
-	resp = result.getResponse();
-	_callback_success &= resp.ok;
-}
-
 int
 UavcanNode::reset_node(int remote_node_id)
 {
@@ -422,99 +397,6 @@ UavcanNode::update_params()
 	_mixing_interface_servo.updateParams();
 }
 
-int
-UavcanNode::start_fw_server()
-{
-	int rv = -1;
-	_fw_server_action.store((int)Busy);
-	UavcanServers   *_servers = UavcanServers::instance();
-
-	if (_servers == nullptr) {
-
-		rv = UavcanServers::start(_node);
-
-		if (rv >= 0) {
-			/*
-			 * Set our pointer to to the injector
-			 *  This is a work around as
-			 *  main_node.getDispatcher().installRxFrameListener(driver.get());
-			 *  would require a dynamic cast and rtti is not enabled.
-			 */
-			UavcanServers::instance()->attachITxQueueInjector(&_tx_injector);
-		}
-	}
-
-	_fw_server_action.store((int)None);
-	px4_sem_post(&_server_command_sem);
-	return rv;
-}
-
-int
-UavcanNode::request_fw_check()
-{
-	int rv = -1;
-	_fw_server_action.store((int)Busy);
-	UavcanServers *_servers  = UavcanServers::instance();
-
-	if (_servers != nullptr) {
-		_servers->requestCheckAllNodesFirmwareAndUpdate();
-		rv = 0;
-	}
-
-	_fw_server_action.store((int)None);
-	px4_sem_post(&_server_command_sem);
-	return rv;
-}
-
-int
-UavcanNode::stop_fw_server()
-{
-	int rv = -1;
-	_fw_server_action.store((int)Busy);
-	UavcanServers *_servers = UavcanServers::instance();
-
-	if (_servers != nullptr) {
-		/*
-		 * Set our pointer to to the injector
-		 *  This is a work around as
-		 *  main_node.getDispatcher().remeveRxFrameListener();
-		 *  would require a dynamic cast and rtti is not enabled.
-		 */
-		_tx_injector = nullptr;
-
-		rv = _servers->stop();
-	}
-
-	_fw_server_action.store((int)None);
-	px4_sem_post(&_server_command_sem);
-	return rv;
-}
-
-int
-UavcanNode::fw_server(eServerAction action)
-{
-	int rv = -EAGAIN;
-
-	switch (action) {
-	case Start:
-	case Stop:
-	case CheckFW:
-		if (_fw_server_action.load() == (int)None) {
-			_fw_server_action.store((int)action);
-			px4_sem_wait(&_server_command_sem);
-			rv = _fw_server_status;
-		}
-
-		break;
-
-	default:
-		rv = -EINVAL;
-		break;
-	}
-
-	return rv;
-}
-
 int
 UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 {
@@ -637,12 +519,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return ret;
 	}
 
-	ret = _beep_controller.init();
-
-	if (ret < 0) {
-		return ret;
-	}
-
 	ret = _safety_state_controller.init();
 
 	if (ret < 0) {
@@ -655,6 +531,14 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return ret;
 	}
 
+	/* Start node info retriever to fetch node info from new nodes */
+	ret = _node_info_retriever.start();
+
+	if (ret < 0) {
+		PX4_ERR("NodeInfoRetriever init: %d", ret);
+		return ret;
+	}
+
 	// Sensor bridges
 	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
 
@@ -687,25 +571,31 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		enable_idle_throttle_when_armed(true);
 	}
 
-	/*  Start the Node   */
+	/* Set up shared service clients */
+	_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
+	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
+	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
+
+
+	int32_t uavcan_enable = 1;
+	(void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable);
+
+	if (uavcan_enable > 1) {
+		_servers = new UavcanServers(_node, _node_info_retriever);
+
+		if (_servers) {
+			int rv = _servers->init();
+
+			if (rv < 0) {
+				PX4_ERR("UavcanServers init: %d", ret);
+			}
+		}
+	}
+
+	// Start the Node
 	return _node.start();
 }
 
-void
-UavcanNode::node_spin_once()
-{
-	const int spin_res = _node.spinOnce();
-
-	if (spin_res < 0) {
-		PX4_ERR("node spin error %i", spin_res);
-	}
-
-
-	if (_tx_injector != nullptr) {
-		_tx_injector->injectTxFramesInto(_node);
-	}
-}
-
 void
 UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
 {
@@ -746,8 +636,6 @@ UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
 	_time_sync_master.publish();
 }
 
-
-
 void
 UavcanNode::Run()
 {
@@ -789,7 +677,12 @@ UavcanNode::Run()
 		br->update();
 	}
 
-	node_spin_once(); // expected to be non-blocking
+	if (_check_fw) {
+		_check_fw = false;
+		_node_info_retriever.invalidateAll();
+	}
+
+	_node.spinOnce(); // expected to be non-blocking
 
 	// Check arming state
 	const actuator_armed_s &armed = _mixing_interface_esc.mixingOutput().armed();
@@ -805,22 +698,206 @@ UavcanNode::Run()
 		update_params();
 	}
 
-	switch ((eServerAction)_fw_server_action.load()) {
+	// Check for parameter requests (get/set/list)
-	case Start:
+	if (_param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-		_fw_server_status = start_fw_server();
+		uavcan_parameter_request_s request{};
-		break;
+		_param_request_sub.copy(&request);
 
-	case Stop:
+		if (_param_counts[request.node_id]) {
-		_fw_server_status = stop_fw_server();
+			/*
-		break;
+			 * We know how many parameters are exposed by this node, so
+			 * process the request.
+			 */
+			if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) {
+				uavcan::protocol::param::GetSet::Request req;
 
-	case CheckFW:
+				if (request.param_index >= 0) {
-		_fw_server_status = request_fw_check();
+					req.index = request.param_index;
-		break;
 
-	case None:
+				} else {
-	default:
+					req.name = (char *)request.param_id;
-		break;
+				}
+
+				int call_res = _param_getset_client.call(request.node_id, req);
+
+				if (call_res < 0) {
+					PX4_ERR("couldn't send GetSet: %d", call_res);
+
+				} else {
+					_param_in_progress = true;
+					_param_index = request.param_index;
+				}
+
+			} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
+				uavcan::protocol::param::GetSet::Request req;
+
+				if (request.param_index >= 0) {
+					req.index = request.param_index;
+
+				} else {
+					req.name = (char *)request.param_id;
+				}
+
+				if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_REAL32) {
+					req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
+
+				} else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) {
+					req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
+
+				} else {
+					req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
+				}
+
+				// Set the dirty bit for this node
+				set_node_params_dirty(request.node_id);
+
+				int call_res = _param_getset_client.call(request.node_id, req);
+
+				if (call_res < 0) {
+					PX4_ERR("couldn't send GetSet: %d", call_res);
+
+				} else {
+					_param_in_progress = true;
+					_param_index = request.param_index;
+				}
+
+			} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
+				// This triggers the _param_list_in_progress case below.
+				_param_index = 0;
+				_param_list_in_progress = true;
+				_param_list_node_id = request.node_id;
+				_param_list_all_nodes = false;
+
+				PX4_DEBUG("starting component-specific param list");
+			}
+
+		} else if (request.node_id == uavcan_parameter_request_s::NODE_ID_ALL) {
+			if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
+				/*
+				 * This triggers the _param_list_in_progress case below,
+				 * but additionally iterates over all active nodes.
+				 */
+				_param_index = 0;
+				_param_list_in_progress = true;
+				_param_list_node_id = get_next_active_node_id(0);
+				_param_list_all_nodes = true;
+
+				PX4_DEBUG("starting global param list with node %hhu", _param_list_node_id);
+
+				if (_param_counts[_param_list_node_id] == 0) {
+					param_count(_param_list_node_id);
+				}
+			}
+
+		} else {
+			/*
+			 * Need to know how many parameters this node has before we can
+			 * continue; count them now and then process the request.
+			 */
+			param_count(request.node_id);
+		}
+	}
+
+	// Handle parameter listing index/node ID advancement
+	if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
+		if (_param_index >= _param_counts[_param_list_node_id]) {
+			PX4_DEBUG("completed param list for node %hhu", _param_list_node_id);
+			// Reached the end of the current node's parameter set.
+			_param_list_in_progress = false;
+
+			if (_param_list_all_nodes) {
+				// We're listing all parameters for all nodes -- get the next node ID
+				uint8_t next_id = get_next_active_node_id(_param_list_node_id);
+
+				if (next_id < 128) {
+					_param_list_node_id = next_id;
+
+					/*
+					 * If there is a next node ID, check if that node's parameters
+					 * have been counted before. If not, do it now.
+					 */
+					if (_param_counts[_param_list_node_id] == 0) {
+						param_count(_param_list_node_id);
+					}
+
+					// Keep on listing.
+					_param_index = 0;
+					_param_list_in_progress = true;
+					PX4_DEBUG("started param list for node %hhu", _param_list_node_id);
+				}
+			}
+		}
+	}
+
+	// Check if we're still listing, and need to get the next parameter
+	if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
+		// Ready to request the next value -- _param_index is incremented
+		// after each successful fetch by cb_getset
+		uavcan::protocol::param::GetSet::Request req;
+		req.index = _param_index;
+
+		int call_res = _param_getset_client.call(_param_list_node_id, req);
+
+		if (call_res < 0) {
+			_param_list_in_progress = false;
+			PX4_ERR("couldn't send param list GetSet: %d", call_res);
+
+		} else {
+			_param_in_progress = true;
+		}
+	}
+
+	if (_vcmd_sub.updated() && !_cmd_in_progress) {
+		bool acknowledge = false;
+		vehicle_command_s cmd{};
+		_vcmd_sub.copy(&cmd);
+
+		uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+		if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
+			acknowledge = true;
+			int command_id = static_cast<int>(cmd.param1 + 0.5f);
+
+			PX4_DEBUG("received storage command ID %d", command_id);
+
+			switch (command_id) {
+			case 1: {
+					// Param save request
+					int node_id;
+					node_id = get_next_dirty_node_id(1);
+
+					if (node_id < 128) {
+						_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
+						param_opcode(node_id);
+					}
+
+					break;
+				}
+
+			case 2: {
+					// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
+					_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
+
+					for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
+						set_node_params_dirty(i);
+					}
+
+					param_opcode(get_next_dirty_node_id(1));
+					break;
+				}
+			}
+		}
+
+		if (acknowledge) {
+			// Acknowledge the received command
+			vehicle_command_ack_s ack{};
+			ack.command = cmd.command;
+			ack.result = cmd_ack_result;
+			ack.target_system = cmd.source_system;
+			ack.target_component = cmd.source_component;
+			ack.timestamp = hrt_absolute_time();
+			_command_ack_pub.publish(ack);
+		}
 	}
 
 	perf_end(_cycle_perf);
@@ -833,7 +910,6 @@ UavcanNode::Run()
 		_mixing_interface_servo.mixingOutput().unregister();
 		_mixing_interface_servo.ScheduleClear();
 		ScheduleClear();
-		teardown();
 		_instance = nullptr;
 	}
 }
@@ -851,13 +927,6 @@ UavcanNode::enable_idle_throttle_when_armed(bool value)
 	}
 }
 
-int
-UavcanNode::teardown()
-{
-	px4_sem_post(&_server_command_sem);
-	return 0;
-}
-
 int
 UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 {
@@ -884,33 +953,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 		}
 		break;
 
-
-	case UAVCAN_IOCS_HARDPOINT_SET: {
-			const auto &hp_cmd = *reinterpret_cast<uavcan::equipment::hardpoint::Command *>(arg);
-			_hardpoint_controller.set_command(hp_cmd.hardpoint_id, hp_cmd.command);
-		}
-		break;
-
-	case UAVCAN_IOCG_NODEID_INPROGRESS: {
-			UavcanServers   *_servers = UavcanServers::instance();
-
-			if (_servers == nullptr) {
-				// status unavailable
-				ret = -EINVAL;
-				break;
-
-			} else if (_servers->guessIfAllDynamicNodesAreAllocated()) {
-				// node discovery complete
-				ret = -ETIME;
-				break;
-
-			} else {
-				// node discovery in progress
-				ret = OK;
-				break;
-			}
-		}
-
 	default:
 		ret = -ENOTTY;
 		break;
@@ -925,7 +967,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 	return ret;
 }
 
-
 bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
@@ -1054,11 +1095,212 @@ UavcanNode::shrink()
 }
 
 void
-UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
+UavcanNode::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
 {
-	(void)pthread_mutex_lock(&_node_mutex);
+	if (_count_in_progress) {
-	_hardpoint_controller.set_command(hardpoint_id, command);
+		/*
-	(void)pthread_mutex_unlock(&_node_mutex);
+		 * Currently in parameter count mode:
+		 * Iterate over all parameters for the node to which the request was
+		 * originally sent, in order to find the maximum parameter ID. If a
+		 * request fails, set the node's parameter count to zero.
+		 */
+		uint8_t node_id = result.getCallID().server_node_id.get();
+
+		if (result.isSuccessful()) {
+			uavcan::protocol::param::GetSet::Response resp = result.getResponse();
+
+			if (resp.name.size()) {
+				_count_index++;
+				_param_counts[node_id] = _count_index;
+
+				uavcan::protocol::param::GetSet::Request req;
+				req.index = _count_index;
+
+				int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
+
+				if (call_res < 0) {
+					_count_in_progress = false;
+					_count_index = 0;
+					PX4_ERR("couldn't send GetSet during param count: %d", call_res);
+				}
+
+			} else {
+				_count_in_progress = false;
+				_count_index = 0;
+				PX4_DEBUG("completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
+			}
+
+		} else {
+			_param_counts[node_id] = 0;
+			_count_in_progress = false;
+			_count_index = 0;
+			PX4_ERR("GetSet error during param count");
+		}
+
+	} else {
+		/*
+		 * Currently in parameter get/set mode:
+		 * Publish a uORB uavcan_parameter_value message containing the current value
+		 * of the parameter.
+		 */
+		if (result.isSuccessful()) {
+			uavcan::protocol::param::GetSet::Response param = result.getResponse();
+
+			uavcan_parameter_value_s response{};
+			response.node_id = result.getCallID().server_node_id.get();
+			strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
+			response.param_id[16] = '\0';
+			response.param_index = _param_index;
+			response.param_count = _param_counts[response.node_id];
+
+			if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_INT64;
+				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
+
+			} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_REAL32;
+				response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
+
+			} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
+				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_UINT8;
+				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
+			}
+
+			_param_response_pub.publish(response);
+
+		} else {
+			PX4_ERR("GetSet error");
+		}
+
+		_param_in_progress = false;
+		_param_index++;
+	}
+}
+
+void
+UavcanNode::param_count(uavcan::NodeID node_id)
+{
+	uavcan::protocol::param::GetSet::Request req;
+	req.index = 0;
+	int call_res = _param_getset_client.call(node_id, req);
+
+	if (call_res < 0) {
+		PX4_ERR("couldn't start parameter count: %d", call_res);
+
+	} else {
+		_count_in_progress = true;
+		_count_index = 0;
+		PX4_DEBUG("starting param count");
+	}
+}
+
+void
+UavcanNode::param_opcode(uavcan::NodeID node_id)
+{
+	uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
+	opcode_req.opcode = _param_save_opcode;
+	int call_res = _param_opcode_client.call(node_id, opcode_req);
+
+	if (call_res < 0) {
+		PX4_ERR("couldn't send ExecuteOpcode: %d", call_res);
+
+	} else {
+		_cmd_in_progress = true;
+		PX4_INFO("sent ExecuteOpcode");
+	}
+}
+
+void
+UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
+{
+	bool success = result.isSuccessful();
+	uint8_t node_id = result.getCallID().server_node_id.get();
+	uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
+	success &= resp.ok;
+	_cmd_in_progress = false;
+
+	if (!result.isSuccessful()) {
+		PX4_ERR("save request for node %hhu timed out.", node_id);
+
+	} else if (!result.getResponse().ok) {
+		PX4_ERR("save request for node %hhu rejected.", node_id);
+
+	} else {
+		PX4_INFO("save request for node %hhu completed OK, restarting.", node_id);
+
+		uavcan::protocol::RestartNode::Request restart_req;
+		restart_req.magic_number = restart_req.MAGIC_NUMBER;
+		int call_res = _param_restartnode_client.call(node_id, restart_req);
+
+		if (call_res < 0) {
+			PX4_ERR("couldn't send RestartNode: %d", call_res);
+
+		} else {
+			PX4_ERR("sent RestartNode");
+			_cmd_in_progress = true;
+		}
+	}
+
+	if (!_cmd_in_progress) {
+		/*
+		 * Something went wrong, so cb_restart is never going to be called as a result of this request.
+		 * To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
+		 * ID and keep processing here. The dirty bit on the current node is still set, so the
+		 * save/erase attempt will occur when the next save/erase command is received over MAVLink.
+		 */
+		node_id = get_next_dirty_node_id(node_id);
+
+		if (node_id < 128) {
+			param_opcode(node_id);
+		}
+	}
+}
+
+void
+UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
+{
+	bool success = result.isSuccessful();
+	uint8_t node_id = result.getCallID().server_node_id.get();
+	uavcan::protocol::RestartNode::Response resp = result.getResponse();
+	success &= resp.ok;
+	_cmd_in_progress = false;
+
+	if (success) {
+		PX4_DEBUG("restart request for node %hhu completed OK.", node_id);
+
+		// Clear the dirty flag
+		clear_node_params_dirty(node_id);
+
+	} else {
+		PX4_ERR("restart request for node %hhu failed.", node_id);
+	}
+
+	// Get the next dirty node ID and send the same command to it
+	node_id = get_next_dirty_node_id(node_id);
+
+	if (node_id < 128) {
+		param_opcode(node_id);
+	}
+}
+
+uint8_t
+UavcanNode::get_next_active_node_id(uint8_t base)
+{
+	base++;
+
+	for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) || _node.getNodeID().get() == base); base++);
+
+	return base;
+}
+
+uint8_t
+UavcanNode::get_next_dirty_node_id(uint8_t base)
+{
+	base++;
+
+	for (; base < 128 && !are_node_params_dirty(base); base++);
+
+	return base;
 }
 
 /*
@@ -1067,35 +1309,19 @@ UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
 static void print_usage()
 {
 	PX4_INFO("usage: \n"
-		 "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|\n"
+		 "\tuavcan {start|status|stop|shrink|update}\n"
-		 "\t        param [set|get|list|save] <node-id> <name> <value>|reset <node-id>|\n"
+		 "\t        param [set|get|list|save] <node-id> <name> <value>|reset <node-id>");
-		 "\t        hardpoint set <id> <command>}");
 }
 
-extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
+extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
-
-int uavcan_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		print_usage();
 		::exit(1);
 	}
 
-	bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
-
 	if (!std::strcmp(argv[1], "start")) {
 		if (UavcanNode::instance()) {
-			if (fw && UavcanServers::instance() == nullptr) {
-				int rv = UavcanNode::instance()->fw_server(UavcanNode::Start);
-
-				if (rv < 0) {
-					PX4_ERR("Firmware Server Failed to Start %d", rv);
-					::exit(rv);
-				}
-
-				::exit(0);
-			}
-
 			// Already running, no error
 			PX4_INFO("already started");
 			::exit(0);
@@ -1126,17 +1352,12 @@ int uavcan_main(int argc, char *argv[])
 		errx(1, "application not running");
 	}
 
-	if (fw && !std::strcmp(argv[1], "update")) {
+	if (!std::strcmp(argv[1], "update")) {
-		if (UavcanServers::instance() == nullptr) {
+		if (UavcanNode::instance() == nullptr) {
 			errx(1, "firmware server is not running");
 		}
 
-		int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW);
+		UavcanNode::instance()->requestCheckAllNodesFirmwareAndUpdate();
-		::exit(rv);
-	}
-
-	if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
-		printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
 		::exit(0);
 	}
 
@@ -1204,47 +1425,9 @@ int uavcan_main(int argc, char *argv[])
 		}
 	}
 
-	if (!std::strcmp(argv[1], "hardpoint")) {
-		if (argc > 4 && !std::strcmp(argv[2], "set")) {
-			const int hardpoint_id = atoi(argv[3]);
-			const int command = atoi(argv[4]);
-
-			// Sanity check - weed out negative values, check against maximums
-			if (hardpoint_id >= 0 && hardpoint_id < 256 &&
-			    command >= 0 && command < 65536) {
-				inst->hardpoint_controller_set((uint8_t) hardpoint_id, (uint16_t) command);
-
-			} else {
-				errx(1, "Invalid argument");
-			}
-
-		} else {
-			errx(1, "Invalid hardpoint command");
-		}
-
-		::exit(0);
-	}
-
 	if (!std::strcmp(argv[1], "stop")) {
-		if (fw) {
+		delete inst;
-
+		::exit(0);
-			int rv = inst->fw_server(UavcanNode::Stop);
-
-			/* Let's recover any memory we can */
-
-			inst->shrink();
-
-			if (rv < 0) {
-				PX4_ERR("Firmware Server Failed to Stop %d", rv);
-				::exit(rv);
-			}
-
-			::exit(0);
-
-		} else {
-			delete inst;
-			::exit(0);
-		}
 	}
 
 	print_usage();

src/drivers/uavcan/uavcan_main.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,32 +46,37 @@
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
-#include "beep.hpp"
-#include "rgbled.hpp"
-#include "safety_state.hpp"
-#include "uavcan_driver.hpp"
-#include "uavcan_servers.hpp"
-#include "allocator.hpp"
 #include "actuators/esc.hpp"
 #include "actuators/hardpoint.hpp"
 #include "actuators/servo.hpp"
+#include "allocator.hpp"
+#include "beep.hpp"
+#include "rgbled.hpp"
+#include "safety_state.hpp"
 #include "sensors/sensor_bridge.hpp"
+#include "uavcan_driver.hpp"
+#include "uavcan_servers.hpp"
+
+#include <lib/drivers/device/Device.hpp>
+#include <lib/mixer_module/mixer_module.hpp>
+#include <lib/perf/perf_counter.h>
 
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
 #include <uavcan/protocol/global_time_sync_master.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
 #include <uavcan/protocol/node_status_monitor.hpp>
-#include <uavcan/protocol/param/GetSet.hpp>
 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
+#include <uavcan/protocol/param/GetSet.hpp>
 #include <uavcan/protocol/RestartNode.hpp>
 
-#include <lib/drivers/device/device.h>
+#include <uORB/Publication.hpp>
-#include <lib/mixer_module/mixer_module.hpp>
-#include <lib/perf/perf_counter.h>
-
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/uavcan_parameter_request.h>
+#include <uORB/topics/uavcan_parameter_value.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 using namespace time_literals;
 
@@ -178,18 +183,15 @@ public:
 
 	uavcan::Node<>	&get_node() { return _node; }
 
-	int		teardown();
-
 	void		print_info();
 
 	void		shrink();
 
-	void		hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
-
 	static UavcanNode	*instance() { return _instance; }
 	static int		 getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
-	int			 fw_server(eServerAction action);
+
-	void			 attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
+	void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
+
 	int			 list_params(int remote_node_id);
 	int			 save_params(int remote_node_id);
 	int			 set_param(int remote_node_id, const char *name, char *value);
@@ -198,16 +200,11 @@ public:
 
 	static void busevent_signal_trampoline();
 
-protected:
-	void Run() override;
 private:
+	void Run() override;
+
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
-	void		node_spin_once();
-
-	int		start_fw_server();
-	int		stop_fw_server();
-	int		request_fw_check();
 
 	int		print_params(uavcan::protocol::param::GetSet::Response &resp);
 	int		get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
@@ -219,10 +216,6 @@ private:
 	void enable_idle_throttle_when_armed(bool value);
 
 	px4::atomic_bool	_task_should_exit{false};	///< flag to indicate to tear down the CAN driver
-	px4::atomic<int>	_fw_server_action{None};
-	int			 _fw_server_status{-1};
-
-	bool			_is_armed{false};		///< the arming status of the actuators on the bus
 
 	unsigned		_output_count{0};		///< number of actuators currently available
 
@@ -232,30 +225,28 @@ private:
 
 	uavcan::Node<>			_node;				///< library instance
 	pthread_mutex_t			_node_mutex;
-	px4_sem_t			_server_command_sem;
+
 	UavcanEscController		_esc_controller;
 	UavcanServoController		_servo_controller;
 	UavcanMixingInterfaceESC 	_mixing_interface_esc{_node_mutex, _esc_controller};
 	UavcanMixingInterfaceServo 	_mixing_interface_servo{_node_mutex, _servo_controller};
 	UavcanHardpointController	_hardpoint_controller;
-	UavcanBeep			_beep_controller;
 	UavcanSafetyState         	_safety_state_controller;
 	UavcanRGBController             _rgbled_controller;
+
 	uavcan::GlobalTimeSyncMaster	_time_sync_master;
 	uavcan::GlobalTimeSyncSlave	_time_sync_slave;
 	uavcan::NodeStatusMonitor	_node_status_monitor;
 
-	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges
+	uavcan::NodeInfoRetriever   _node_info_retriever;
 
-	ITxQueueInjector		*_tx_injector{nullptr};
+	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges
 
 	bool 				_idle_throttle_when_armed{false};
 	int32_t 			_idle_throttle_when_armed_param{0};
 
-	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	perf_counter_t			_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
-
+	perf_counter_t			_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
-	perf_counter_t			_cycle_perf;
-	perf_counter_t			_interval_perf;
 
 	void handle_time_sync(const uavcan::TimerEvent &);
 
@@ -274,7 +265,58 @@ private:
 		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
 
 	void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+
+	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)};
+
+	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
+	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	/*
+	 * The MAVLink parameter bridge needs to know the maximum parameter index
+	 * of each node so that clients can determine when parameter listings have
+	 * finished. We do that by querying a node's entire parameter set whenever
+	 * a parameter message is received for a node with a zero _param_count,
+	 * and storing the count here. If a node doesn't respond, or doesn't have
+	 * any parameters, its count will stay at zero and we'll try to query the
+	 * set again next time.
+	 *
+	 * The node's UAVCAN ID is used as the index into the _param_counts array.
+	 */
+	uint8_t _param_counts[128] {};
+	bool _count_in_progress{false};
+	uint8_t _count_index{0};
+
+	bool _param_in_progress{false};
+	uint8_t _param_index{0};
+	bool _param_list_in_progress{false};
+	bool _param_list_all_nodes{false};
+	uint8_t _param_list_node_id{1};
+
+	uint32_t _param_dirty_bitmap[4] {};
+	uint8_t _param_save_opcode{0};
+
+	bool _cmd_in_progress{false};
+
+	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
+	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
 	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
 	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
 
+	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
+	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
+	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
+	void param_count(uavcan::NodeID node_id);
+	void param_opcode(uavcan::NodeID node_id);
+
+	uint8_t get_next_active_node_id(uint8_t base);
+	uint8_t get_next_dirty_node_id(uint8_t base);
+	void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
+	void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
+	bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
+
+	bool _check_fw{false};
+
+	UavcanServers   *_servers{nullptr};
 };

src/drivers/uavcan/uavcan_module.hpp

@@ -66,18 +66,6 @@
 // ioctl interface
 #define _UAVCAN_IOC(_n)               (_IOC(_UAVCAN_IOCBASE, _n))
 #define _UAVCAN_IOCBASE               (0x4000)                        // IOCTL base for module UAVCAN
-/*
- * Query if node identification is in progress. Returns:
- *      EINVAL - not applicable in the current operating mode
- *      ETIME  - network discovery complete
- *      OK (0) - network discovery in progress
- */
-#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1)
-/*
- * Set hardpoint command. Accepts a pointer to uavcan::equipment::hardpoint::Command; returns nothing.
- * The pointer may be invalidated once the call returns.
- */
-#define UAVCAN_IOCS_HARDPOINT_SET       _UAVCAN_IOC(10)
 
 // public prototypes
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);

src/drivers/uavcan/uavcan_servers.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -51,16 +51,11 @@
 
 #include "uavcan_main.hpp"
 #include "uavcan_servers.hpp"
-#include "uavcan_virtual_can_driver.hpp"
 
 #include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
 #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
 #include <uavcan_posix/firmware_version_checker.hpp>
 
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/uavcan_parameter_request.h>
-
 /**
  * @file uavcan_servers.cpp
  *
@@ -70,147 +65,22 @@
  *         David Sidrane <david_s5@nscdg.com>
  */
 
-/*
+UavcanServers::UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever) :
- * UavcanNode
+	_server_instance(node, _storage_backend, _tracer),
- */
+	_fileserver_backend(node),
-
+	_fw_upgrade_trigger(node, _fw_version_checker),
-UavcanServers *UavcanServers::_instance;
+	_fw_server(node, _fileserver_backend),
-
+	_node_info_retriever(node_info_retriever)
-UavcanServers::UavcanServers(uavcan::INode &main_node) :
-	_vdriver(NumIfaces, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
-	_subnode(_vdriver, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator()),
-	_main_node(main_node),
-	_server_instance(_subnode, _storage_backend, _tracer),
-	_fileserver_backend(_subnode),
-	_node_info_retriever(_subnode),
-	_fw_upgrade_trigger(_subnode, _fw_version_checker),
-	_fw_server(_subnode, _fileserver_backend),
-	_param_getset_client(_subnode),
-	_param_opcode_client(_subnode),
-	_param_restartnode_client(_subnode),
-	_beep_pub(_subnode),
-	_enumeration_indication_sub(_subnode),
-	_enumeration_client(_subnode),
-	_enumeration_getset_client(_subnode),
-	_enumeration_save_client(_subnode)
 {
 }
 
-UavcanServers::~UavcanServers()
+int UavcanServers::init()
 {
-	if (_mutex_inited) {
-		(void)Lock::deinit(_subnode_mutex);
-	}
-
-	_main_node.getDispatcher().removeRxFrameListener();
-}
-
-int
-UavcanServers::stop()
-{
-	UavcanServers *server = instance();
-
-	if (server == nullptr) {
-		PX4_INFO("Already stopped");
-		return -1;
-	}
-
-	if (server->_subnode_thread) {
-		PX4_INFO("stopping fw srv thread...");
-		server->_subnode_thread_should_exit.store(true);
-		(void)pthread_join(server->_subnode_thread, NULL);
-	}
-
-	_instance = nullptr;
-
-	server->_main_node.getDispatcher().removeRxFrameListener();
-
-	delete server;
-	return 0;
-}
-
-int
-UavcanServers::start(uavcan::INode &main_node)
-{
-	if (_instance != nullptr) {
-		PX4_INFO("Already started");
-		return -1;
-	}
-
-	/*
-	 * Node init
-	 */
-	_instance = new UavcanServers(main_node);
-
-	if (_instance == nullptr) {
-		PX4_ERR("Out of memory");
-		return -2;
-	}
-
-	int rv = _instance->init();
-
-	if (rv < 0) {
-		PX4_ERR("Node init failed: %d", rv);
-		delete _instance;
-		_instance = nullptr;
-		return rv;
-	}
-
-	/*
-	 * Start the thread. Normally it should never exit.
-	 */
-	pthread_attr_t tattr;
-	struct sched_param param;
-
-	pthread_attr_init(&tattr);
-	(void)pthread_attr_getschedparam(&tattr, &param);
-	tattr.stacksize = PX4_STACK_ADJUSTED(StackSize);
-	param.sched_priority = Priority;
-
-	if (pthread_attr_setschedparam(&tattr, &param)) {
-		PX4_ERR("setting sched params failed");
-	}
-
-	static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
-
-	rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
-
-	if (rv != 0) {
-		rv = -rv;
-		PX4_ERR("pthread_create() failed: %d", rv);
-		delete _instance;
-		_instance = nullptr;
-	}
-
-	return rv;
-}
-
-int
-UavcanServers::init()
-{
-	errno = 0;
-
-	/*
-	 * Initialize the mutex.
-	 * giving it its path
-	 */
-	int ret = Lock::init(_subnode_mutex);
-
-	if (ret < 0) {
-		PX4_ERR("Lock init: %d", errno);
-		return ret;
-	}
-
-	_mutex_inited = true;
-
-	_subnode.setNodeID(_main_node.getNodeID());
-	_main_node.getDispatcher().installRxFrameListener(&_vdriver);
-
 	/*
 	 * Initialize the fw version checker.
 	 * giving it its path
 	 */
-	ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
+	int ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
 
 	if (ret < 0) {
 		PX4_ERR("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
@@ -218,7 +88,6 @@ UavcanServers::init()
 	}
 
 	/* Start fw file server back */
-
 	ret = _fw_server.start(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
 
 	if (ret < 0) {
@@ -227,7 +96,6 @@ UavcanServers::init()
 	}
 
 	/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
-
 	ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
 
 	if (ret < 0) {
@@ -236,7 +104,6 @@ UavcanServers::init()
 	}
 
 	/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
-
 	ret = _tracer.init(UAVCAN_LOG_FILE);
 
 	if (ret < 0) {
@@ -256,14 +123,6 @@ UavcanServers::init()
 		return ret;
 	}
 
-	/* Start node info retriever to fetch node info from new nodes */
-	ret = _node_info_retriever.start();
-
-	if (ret < 0) {
-		PX4_ERR("NodeInfoRetriever init: %d", ret);
-		return ret;
-	}
-
 	/* Start the fw version checker   */
 	ret = _fw_upgrade_trigger.start(_node_info_retriever);
 
@@ -272,689 +131,17 @@ UavcanServers::init()
 		return ret;
 	}
 
-	/*  Start the Node   */
-	return 0;
-}
-
-pthread_addr_t
-UavcanServers::run(pthread_addr_t)
-{
-	prctl(PR_SET_NAME, "uavcan_fw_srv", 0);
-
-	Lock lock(_subnode_mutex);
-
 	/*
 	Check for firmware in the root directory, move it to appropriate location on
 	the SD card, as defined by the APDesc.
 	*/
 	migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ROOT_PATH);
 
-	/* the subscribe call needs to happen in the same thread,
+	/*  Start the Node   */
-	 * so not in the constructor */
+	return 0;
-	uORB::Subscription armed_sub{ORB_ID(actuator_armed)};
-	uORB::Subscription vcmd_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription param_request_sub{ORB_ID(uavcan_parameter_request)};
-
-	/* Set up shared service clients */
-	_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
-	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
-	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
-	_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
-	_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
-	_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
-	_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));
-
-	_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
-	memset(_param_counts, 0, sizeof(_param_counts));
-
-	_esc_enumeration_active = false;
-	memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
-	_esc_enumeration_index = 0;
-
-	while (!_subnode_thread_should_exit.load()) {
-
-		if (_check_fw == true) {
-			_check_fw = false;
-			_node_info_retriever.invalidateAll();
-		}
-
-		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
-
-		if (spin_res < 0) {
-			PX4_ERR("node spin error %i", spin_res);
-		}
-
-		// Check for parameter requests (get/set/list)
-		if (param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-			uavcan_parameter_request_s request{};
-			param_request_sub.copy(&request);
-
-			if (_param_counts[request.node_id]) {
-				/*
-				 * We know how many parameters are exposed by this node, so
-				 * process the request.
-				 */
-				if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) {
-					uavcan::protocol::param::GetSet::Request req;
-
-					if (request.param_index >= 0) {
-						req.index = request.param_index;
-
-					} else {
-						req.name = (char *)request.param_id;
-					}
-
-					int call_res = _param_getset_client.call(request.node_id, req);
-
-					if (call_res < 0) {
-						PX4_ERR("couldn't send GetSet: %d", call_res);
-
-					} else {
-						_param_in_progress = true;
-						_param_index = request.param_index;
-					}
-
-				} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
-					uavcan::protocol::param::GetSet::Request req;
-
-					if (request.param_index >= 0) {
-						req.index = request.param_index;
-
-					} else {
-						req.name = (char *)request.param_id;
-					}
-
-					if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_REAL32) {
-						req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
-
-					} else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) {
-						req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
-
-					} else {
-						req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
-					}
-
-					// Set the dirty bit for this node
-					set_node_params_dirty(request.node_id);
-
-					int call_res = _param_getset_client.call(request.node_id, req);
-
-					if (call_res < 0) {
-						PX4_ERR("couldn't send GetSet: %d", call_res);
-
-					} else {
-						_param_in_progress = true;
-						_param_index = request.param_index;
-					}
-
-				} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
-					// This triggers the _param_list_in_progress case below.
-					_param_index = 0;
-					_param_list_in_progress = true;
-					_param_list_node_id = request.node_id;
-					_param_list_all_nodes = false;
-
-					PX4_DEBUG("starting component-specific param list");
-				}
-
-			} else if (request.node_id == uavcan_parameter_request_s::NODE_ID_ALL) {
-				if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) {
-					/*
-					 * This triggers the _param_list_in_progress case below,
-					 * but additionally iterates over all active nodes.
-					 */
-					_param_index = 0;
-					_param_list_in_progress = true;
-					_param_list_node_id = get_next_active_node_id(0);
-					_param_list_all_nodes = true;
-
-					PX4_DEBUG("starting global param list with node %hhu", _param_list_node_id);
-
-					if (_param_counts[_param_list_node_id] == 0) {
-						param_count(_param_list_node_id);
-					}
-				}
-
-			} else {
-				/*
-				 * Need to know how many parameters this node has before we can
-				 * continue; count them now and then process the request.
-				 */
-				param_count(request.node_id);
-			}
-		}
-
-		// Handle parameter listing index/node ID advancement
-		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-			if (_param_index >= _param_counts[_param_list_node_id]) {
-				PX4_DEBUG("completed param list for node %hhu", _param_list_node_id);
-				// Reached the end of the current node's parameter set.
-				_param_list_in_progress = false;
-
-				if (_param_list_all_nodes) {
-					// We're listing all parameters for all nodes -- get the next node ID
-					uint8_t next_id = get_next_active_node_id(_param_list_node_id);
-
-					if (next_id < 128) {
-						_param_list_node_id = next_id;
-
-						/*
-						 * If there is a next node ID, check if that node's parameters
-						 * have been counted before. If not, do it now.
-						 */
-						if (_param_counts[_param_list_node_id] == 0) {
-							param_count(_param_list_node_id);
-						}
-
-						// Keep on listing.
-						_param_index = 0;
-						_param_list_in_progress = true;
-						PX4_DEBUG("started param list for node %hhu", _param_list_node_id);
-					}
-				}
-			}
-		}
-
-		// Check if we're still listing, and need to get the next parameter
-		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-			// Ready to request the next value -- _param_index is incremented
-			// after each successful fetch by cb_getset
-			uavcan::protocol::param::GetSet::Request req;
-			req.index = _param_index;
-
-			int call_res = _param_getset_client.call(_param_list_node_id, req);
-
-			if (call_res < 0) {
-				_param_list_in_progress = false;
-				PX4_ERR("couldn't send param list GetSet: %d", call_res);
-
-			} else {
-				_param_in_progress = true;
-			}
-		}
-
-		// Check for ESC enumeration commands
-		if (vcmd_sub.updated() && !_cmd_in_progress) {
-			bool acknowledge = false;
-			vehicle_command_s cmd{};
-			vcmd_sub.copy(&cmd);
-
-			uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
-
-			if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
-				acknowledge = true;
-				int command_id = static_cast<int>(cmd.param1 + 0.5f);
-				int node_id = static_cast<int>(cmd.param2 + 0.5f);
-				int call_res;
-
-				PX4_DEBUG("received UAVCAN command ID %d, node ID %d", command_id, node_id);
-
-				switch (command_id) {
-				case 0:
-				case 1: {
-						_esc_enumeration_active = command_id;
-						_esc_enumeration_index = 0;
-						_esc_count = 0;
-						uavcan::protocol::enumeration::Begin::Request req;
-						// TODO: Incorrect implementation; the parameter name field should be left empty.
-						//       Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
-						req.parameter_name = "esc_index";
-						req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
-						call_res = _enumeration_client.call(get_next_active_node_id(0), req);
-
-						if (call_res < 0) {
-							PX4_ERR("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
-							beep(BeepFrequencyError);
-							cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
-
-						} else {
-							beep(BeepFrequencyGenericIndication);
-						}
-
-						break;
-					}
-
-				default: {
-						PX4_ERR("unknown command ID %d", command_id);
-						cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
-						break;
-					}
-				}
-
-			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
-				acknowledge = true;
-				int command_id = static_cast<int>(cmd.param1 + 0.5f);
-
-				PX4_DEBUG("received storage command ID %d", command_id);
-
-				switch (command_id) {
-				case 1: {
-						// Param save request
-						int node_id;
-						node_id = get_next_dirty_node_id(1);
-
-						if (node_id < 128) {
-							_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
-							param_opcode(node_id);
-						}
-
-						break;
-					}
-
-				case 2: {
-						// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
-						_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
-
-						for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
-							set_node_params_dirty(i);
-						}
-
-						param_opcode(get_next_dirty_node_id(1));
-						break;
-					}
-				}
-			}
-
-			if (acknowledge) {
-				// Acknowledge the received command
-				vehicle_command_ack_s ack{};
-				ack.command = cmd.command;
-				ack.result = cmd_ack_result;
-				ack.target_system = cmd.source_system;
-				ack.target_component = cmd.source_component;
-				ack.timestamp = hrt_absolute_time();
-				_command_ack_pub.publish(ack);
-			}
-		}
-
-		// Shut down once armed
-		// TODO (elsewhere): start up again once disarmed?
-		if (armed_sub.updated()) {
-			actuator_armed_s armed;
-
-			if (armed_sub.copy(&armed)) {
-				if (armed.armed) {
-					_subnode_thread_should_exit.store(true);
-				}
-			}
-		}
-	}
-
-	return (pthread_addr_t) 0;
 }
 
-void
+void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_path)
-UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
-{
-	if (_count_in_progress) {
-		/*
-		 * Currently in parameter count mode:
-		 * Iterate over all parameters for the node to which the request was
-		 * originally sent, in order to find the maximum parameter ID. If a
-		 * request fails, set the node's parameter count to zero.
-		 */
-		uint8_t node_id = result.getCallID().server_node_id.get();
-
-		if (result.isSuccessful()) {
-			uavcan::protocol::param::GetSet::Response resp = result.getResponse();
-
-			if (resp.name.size()) {
-				_count_index++;
-				_param_counts[node_id] = _count_index;
-
-				uavcan::protocol::param::GetSet::Request req;
-				req.index = _count_index;
-
-				int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
-
-				if (call_res < 0) {
-					_count_in_progress = false;
-					_count_index = 0;
-					PX4_ERR("couldn't send GetSet during param count: %d", call_res);
-					beep(BeepFrequencyError);
-				}
-
-			} else {
-				_count_in_progress = false;
-				_count_index = 0;
-				PX4_DEBUG("completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
-				beep(BeepFrequencyGenericIndication);
-			}
-
-		} else {
-			_param_counts[node_id] = 0;
-			_count_in_progress = false;
-			_count_index = 0;
-			PX4_ERR("GetSet error during param count");
-		}
-
-	} else {
-		/*
-		 * Currently in parameter get/set mode:
-		 * Publish a uORB uavcan_parameter_value message containing the current value
-		 * of the parameter.
-		 */
-		if (result.isSuccessful()) {
-			uavcan::protocol::param::GetSet::Response param = result.getResponse();
-
-			struct uavcan_parameter_value_s response;
-			response.node_id = result.getCallID().server_node_id.get();
-			strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
-			response.param_id[16] = '\0';
-			response.param_index = _param_index;
-			response.param_count = _param_counts[response.node_id];
-
-			if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
-				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_INT64;
-				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
-
-			} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
-				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_REAL32;
-				response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
-
-			} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
-				response.param_type = uavcan_parameter_request_s::PARAM_TYPE_UINT8;
-				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
-			}
-
-			_param_response_pub.publish(response);
-
-		} else {
-			PX4_ERR("GetSet error");
-		}
-
-		_param_in_progress = false;
-		_param_index++;
-	}
-}
-
-void
-UavcanServers::param_count(uavcan::NodeID node_id)
-{
-	uavcan::protocol::param::GetSet::Request req;
-	req.index = 0;
-	int call_res = _param_getset_client.call(node_id, req);
-
-	if (call_res < 0) {
-		PX4_ERR("couldn't start parameter count: %d", call_res);
-
-	} else {
-		_count_in_progress = true;
-		_count_index = 0;
-		PX4_DEBUG("starting param count");
-	}
-}
-
-void
-UavcanServers::param_opcode(uavcan::NodeID node_id)
-{
-	uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
-	opcode_req.opcode = _param_save_opcode;
-	int call_res = _param_opcode_client.call(node_id, opcode_req);
-
-	if (call_res < 0) {
-		PX4_ERR("couldn't send ExecuteOpcode: %d", call_res);
-
-	} else {
-		_cmd_in_progress = true;
-		PX4_INFO("sent ExecuteOpcode");
-	}
-}
-
-void
-UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
-{
-	bool success = result.isSuccessful();
-	uint8_t node_id = result.getCallID().server_node_id.get();
-	uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
-	success &= resp.ok;
-	_cmd_in_progress = false;
-
-	if (!result.isSuccessful()) {
-		PX4_ERR("save request for node %hhu timed out.", node_id);
-
-	} else if (!result.getResponse().ok) {
-		PX4_ERR("save request for node %hhu rejected.", node_id);
-
-	} else {
-		PX4_INFO("save request for node %hhu completed OK, restarting.", node_id);
-
-		uavcan::protocol::RestartNode::Request restart_req;
-		restart_req.magic_number = restart_req.MAGIC_NUMBER;
-		int call_res = _param_restartnode_client.call(node_id, restart_req);
-
-		if (call_res < 0) {
-			PX4_ERR("couldn't send RestartNode: %d", call_res);
-
-		} else {
-			PX4_ERR("sent RestartNode");
-			_cmd_in_progress = true;
-		}
-	}
-
-	if (!_cmd_in_progress) {
-		/*
-		 * Something went wrong, so cb_restart is never going to be called as a result of this request.
-		 * To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
-		 * ID and keep processing here. The dirty bit on the current node is still set, so the
-		 * save/erase attempt will occur when the next save/erase command is received over MAVLink.
-		 */
-		node_id = get_next_dirty_node_id(node_id);
-
-		if (node_id < 128) {
-			param_opcode(node_id);
-		}
-	}
-}
-
-void
-UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
-{
-	bool success = result.isSuccessful();
-	uint8_t node_id = result.getCallID().server_node_id.get();
-	uavcan::protocol::RestartNode::Response resp = result.getResponse();
-	success &= resp.ok;
-	_cmd_in_progress = false;
-
-	if (success) {
-		PX4_DEBUG("restart request for node %hhu completed OK.", node_id);
-
-		// Clear the dirty flag
-		clear_node_params_dirty(node_id);
-
-	} else {
-		PX4_ERR("restart request for node %hhu failed.", node_id);
-	}
-
-	// Get the next dirty node ID and send the same command to it
-	node_id = get_next_dirty_node_id(node_id);
-
-	if (node_id < 128) {
-		param_opcode(node_id);
-	}
-}
-
-uint8_t
-UavcanServers::get_next_active_node_id(uint8_t base)
-{
-	base++;
-
-	for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) ||
-			      _subnode.getNodeID().get() == base); base++);
-
-	return base;
-}
-
-uint8_t
-UavcanServers::get_next_dirty_node_id(uint8_t base)
-{
-	base++;
-
-	for (; base < 128 && !are_node_params_dirty(base); base++);
-
-	return base;
-}
-
-void
-UavcanServers::beep(float frequency)
-{
-	uavcan::equipment::indication::BeepCommand cmd;
-	cmd.frequency = frequency;
-	cmd.duration = 0.1F;              // We don't want to incapacitate ESC for longer time that this
-	(void)_beep_pub.broadcast(cmd);
-}
-
-void
-UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
-{
-	uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
-
-	if (!result.isSuccessful()) {
-		PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
-
-	} else if (result.getResponse().error) {
-		PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(),
-			result.getResponse().error);
-
-	} else {
-		_esc_count++;
-		PX4_INFO("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
-	}
-
-	if (next_id < 128) {
-		// Still other active nodes to send the request to
-		uavcan::protocol::enumeration::Begin::Request req;
-		// TODO: Incorrect implementation; the parameter name field should be left empty.
-		//       Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
-		req.parameter_name = "esc_index";
-		req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
-
-		int call_res = _enumeration_client.call(next_id, req);
-
-		if (call_res < 0) {
-			PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
-
-		} else {
-			PX4_INFO("UAVCAN ESC enumeration: sent Begin request");
-		}
-
-	} else {
-		PX4_INFO("UAVCAN ESC enumeration: begun enumeration on all nodes.");
-	}
-}
-
-void
-UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>
-		&msg)
-{
-	// Called whenever an ESC thinks it has received user input.
-	PX4_INFO("UAVCAN ESC enumeration: got indication");
-
-	if (!_esc_enumeration_active) {
-		// Ignore any messages received when we're not expecting them
-		return;
-	}
-
-	// First, check if we've already seen an indication from this ESC. If so, just ignore this indication.
-	int i = 0;
-
-	for (; i < _esc_enumeration_index; i++) {
-		if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
-			PX4_INFO("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
-			return;
-		}
-	}
-
-	uavcan::protocol::param::GetSet::Request req;
-	req.name = msg.parameter_name;	// 'esc_index' or something alike, the name is not standardized
-	req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
-
-	int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
-
-	if (call_res < 0) {
-		PX4_ERR("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
-
-	} else {
-		PX4_INFO("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
-	}
-}
-
-void
-UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
-{
-	if (!result.isSuccessful()) {
-		PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
-
-	} else {
-		PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
-
-		uavcan::protocol::param::GetSet::Response resp = result.getResponse();
-		uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
-		esc_index = math::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index);
-		_esc_enumeration_index = math::max(_esc_enumeration_index, (uint8_t)(esc_index + 1));
-
-		_esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get();
-
-		uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
-		opcode_req.opcode = opcode_req.OPCODE_SAVE;
-		int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
-
-		if (call_res < 0) {
-			PX4_ERR("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
-
-		} else {
-			PX4_INFO("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
-				 esc_index);
-		}
-	}
-}
-
-void
-UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
-{
-	const bool this_is_the_last_one =
-		(_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1) ||
-		(_esc_enumeration_index >= _esc_count);
-
-	if (!result.isSuccessful()) {
-		PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
-		beep(BeepFrequencyError);
-
-	} else if (!result.getResponse().ok) {
-		PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
-		beep(BeepFrequencyError);
-
-	} else {
-		PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
-		beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
-	}
-
-	PX4_INFO("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
-
-	if (this_is_the_last_one) {
-		_esc_enumeration_active = false;
-
-		// Tell all ESCs to stop enumerating
-		uavcan::protocol::enumeration::Begin::Request req;
-
-		// TODO: Incorrect implementation; the parameter name field should be left empty.
-		//       Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
-		req.parameter_name = "esc_index";
-		req.timeout_sec = 0;
-		int call_res = _enumeration_client.call(get_next_active_node_id(0), req);
-
-		if (call_res < 0) {
-			PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
-
-		} else {
-			PX4_INFO("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
-		}
-	}
-}
-
-
-void
-UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_path)
 {
 	/*
 	Copy Any bin files with APDes into appropriate location on SD card
@@ -1032,8 +219,7 @@ UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_path)
 	}
 }
 
-int
+int UavcanServers::copyFw(const char *dst, const char *src)
-UavcanServers::copyFw(const char *dst, const char *src)
 {
 	int rv = 0;
 	uint8_t buffer[512] {};

src/drivers/uavcan/uavcan_servers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,33 +34,18 @@
 #pragma once
 
 #include <px4_platform_common/px4_config.h>
-#include "uavcan_driver.hpp"
-#include <drivers/device/Device.hpp>
-#include <lib/perf/perf_counter.h>
-#include <uORB/Publication.hpp>
-#include <uORB/topics/uavcan_parameter_value.h>
-#include <uORB/topics/vehicle_command_ack.h>
-
-#include <uavcan/node/sub_node.hpp>
-#include <uavcan/protocol/node_status_monitor.hpp>
-#include <uavcan/protocol/param/GetSet.hpp>
-#include <uavcan/protocol/param/ExecuteOpcode.hpp>
 
 #include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
-#include <uavcan/protocol/node_info_retriever.hpp>
-#include <uavcan_posix/basic_file_server_backend.hpp>
-#include <uavcan/protocol/firmware_update_trigger.hpp>
 #include <uavcan/protocol/file_server.hpp>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan_posix/basic_file_server_backend.hpp>
 #include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
 #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
 #include <uavcan_posix/firmware_version_checker.hpp>
-#include <uavcan/equipment/esc/RawCommand.hpp>
-#include <uavcan/equipment/indication/BeepCommand.hpp>
-#include <uavcan/protocol/enumeration/Begin.hpp>
-#include <uavcan/protocol/enumeration/Indication.hpp>
 
 #include "uavcan_module.hpp"
-#include "uavcan_virtual_can_driver.hpp"
 
 /**
  * @file uavcan_servers.hpp
@@ -74,152 +59,29 @@
 /**
  * A UAVCAN Server Sub node.
  */
-class __EXPORT UavcanServers
+class UavcanServers
 {
-	static constexpr unsigned NumIfaces = 1;  // UAVCAN_STM32_NUM_IFACES
-
-	static constexpr unsigned StackSize  = 6000;
-	static constexpr unsigned Priority  =  120;
-
-	static constexpr unsigned VirtualIfaceBlockAllocationQuota =  80;
-
-	static constexpr float BeepFrequencyGenericIndication       = 1000.0F;
-	static constexpr float BeepFrequencySuccess                 = 2000.0F;
-	static constexpr float BeepFrequencyError                   = 100.0F;
-
 public:
-	UavcanServers(uavcan::INode &main_node);
+	UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever);
+	~UavcanServers() = default;
 
-	virtual		~UavcanServers();
+	int init();
-
-	static int      start(uavcan::INode &main_node);
-	static int      stop();
-
-	uavcan::SubNode<> &get_node() { return _subnode; }
-
-	static UavcanServers *instance() { return _instance; }
-
-	/*
-	 *  Set the main node's pointer to to the injector
-	 *  This is a work around as main_node.getDispatcher().remeveRxFrameListener();
-	 *  would require a dynamic cast and rtti is not enabled.
-	 */
-	void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
-
-	void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
 
 	bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
 
 private:
-	pthread_t         _subnode_thread{-1};
-	pthread_mutex_t   _subnode_mutex{};
-	px4::atomic_bool  _subnode_thread_should_exit{false};
 
-	int		init();
+	void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
-
+	void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
-	pthread_addr_t	run(pthread_addr_t);
+	int copyFw(const char *dst, const char *src);
-
-	static UavcanServers	*_instance;            ///< singleton pointer
-
-	VirtualCanDriver _vdriver;
-
-	uavcan::SubNode<>  _subnode;
-	uavcan::INode      &_main_node;
 
 	uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
 	uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
 	uavcan_posix::FirmwareVersionChecker _fw_version_checker;
 	uavcan::dynamic_node_id_server::CentralizedServer _server_instance;  ///< server singleton pointer
-	uavcan_posix::BasicFileServerBackend  _fileserver_backend;
+	uavcan_posix::BasicFileServerBackend _fileserver_backend;
-	uavcan::NodeInfoRetriever   _node_info_retriever;
+	uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
-	uavcan::FirmwareUpdateTrigger   _fw_upgrade_trigger;
+	uavcan::BasicFileServer _fw_server;
-	uavcan::BasicFileServer         _fw_server;
 
-	/*
+	uavcan::NodeInfoRetriever &_node_info_retriever;
-	 * The MAVLink parameter bridge needs to know the maximum parameter index
-	 * of each node so that clients can determine when parameter listings have
-	 * finished. We do that by querying a node's entire parameter set whenever
-	 * a parameter message is received for a node with a zero _param_count,
-	 * and storing the count here. If a node doesn't respond, or doesn't have
-	 * any parameters, its count will stay at zero and we'll try to query the
-	 * set again next time.
-	 *
-	 * The node's UAVCAN ID is used as the index into the _param_counts array.
-	 */
-	uint8_t _param_counts[128] = {};
-	bool _count_in_progress = false;
-	uint8_t _count_index = 0;
-
-	bool _param_in_progress = false;
-	uint8_t _param_index = 0;
-	bool _param_list_in_progress = false;
-	bool _param_list_all_nodes = false;
-	uint8_t _param_list_node_id = 1;
-
-	uint32_t _param_dirty_bitmap[4] = {};
-	uint8_t _param_save_opcode = 0;
-
-	bool _cmd_in_progress = false;
-
-	// uORB topic handle for MAVLink parameter responses
-	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
-	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
-
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)>
-		ExecuteOpcodeCallback;
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
-
-	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
-	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
-	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
-	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
-
-	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
-	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
-	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
-	void param_count(uavcan::NodeID node_id);
-	void param_opcode(uavcan::NodeID node_id);
-
-	uint8_t get_next_active_node_id(uint8_t base);
-	uint8_t get_next_dirty_node_id(uint8_t base);
-	void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
-	void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
-	bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
-
-	void beep(float frequency);
-
-	bool _mutex_inited = false;
-	volatile bool _check_fw = false;
-
-	// ESC enumeration
-	bool _esc_enumeration_active = false;
-	uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
-	uint8_t _esc_enumeration_index = 0;
-	uint8_t _esc_count = 0;
-
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)>
-		EnumerationBeginCallback;
-	typedef uavcan::MethodBinder<UavcanServers *,
-		void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
-		EnumerationIndicationCallback;
-	void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
-	void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
-	void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
-	void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
-
-	uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
-	uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback>
-	_enumeration_indication_sub;
-	uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
-	uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
-	uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
-
-	void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
-	void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
-	int copyFw(const char *dst, const char *src);
 };

src/drivers/uavcan/uavcan_virtual_can_driver.hpp

@@ -1,512 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
- *               Pavel Kirienko <pavel.kirienko@zubax.com>
- *               David Sidrane <david_s5@nscdg.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <nuttx/config.h>
-
-#include <cstdlib>
-#include <cstdint>
-#include <cstring>
-#include <cstdio>
-#include <fcntl.h>
-#include <pthread.h>
-#include <semaphore.h>
-#include <debug.h>
-
-#include <uavcan/node/sub_node.hpp>
-#include <uavcan/protocol/node_status_monitor.hpp>
-
-/*
- * General purpose wrapper around os's mutual exclusion
- * mechanism.
- *
- * It supports the
- *
- * Note the naming of thier_mutex_ implies that the underlying
- * mutex is owned by class using the Lock.
- * and this wrapper provides service to initialize and de initialize
- * the mutex.
- */
-class Lock
-{
-	pthread_mutex_t &thier_mutex_;
-
-public:
-
-	Lock(pthread_mutex_t &m) :
-		thier_mutex_(m)
-	{
-		(void)pthread_mutex_lock(&m);
-	}
-
-	~Lock()
-	{
-		(void)pthread_mutex_unlock(&thier_mutex_);
-	}
-
-	static int init(pthread_mutex_t &thier_mutex_)
-	{
-		return pthread_mutex_init(&thier_mutex_, NULL);
-	}
-
-	static int deinit(pthread_mutex_t &thier_mutex_)
-	{
-		return pthread_mutex_destroy(&thier_mutex_);
-	}
-
-};
-
-/**
- * Generic queue based on the linked list class defined in libuavcan.
- * This class does not use heap memory.
- */
-template <typename T>
-class Queue
-{
-	struct Item : public uavcan::LinkedListNode<Item> {
-		T payload;
-
-		template <typename... Args>
-		Item(Args... args) : payload(args...) { }
-	};
-
-	uavcan::LimitedPoolAllocator allocator_;
-	uavcan::LinkedListRoot<Item> list_;
-
-public:
-	Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
-		allocator_(arg_allocator, block_allocation_quota)
-	{
-		uavcan::IsDynamicallyAllocatable<Item>::check();
-	}
-
-	~Queue()
-	{
-		while (!isEmpty()) {
-			pop();
-		}
-	}
-
-	bool isEmpty() const { return list_.isEmpty(); }
-
-	/**
-	 * Creates one item in-place at the end of the list.
-	 * Returns true if the item was appended successfully, false if there's not enough memory.
-	 * Complexity is O(N) where N is queue length.
-	 */
-	template <typename... Args>
-	bool tryEmplace(Args... args)
-	{
-		// Allocating memory
-		void *const ptr = allocator_.allocate(sizeof(Item));
-
-		if (ptr == nullptr) {
-			return false;
-		}
-
-		// Constructing the new item
-		Item *const item = new (ptr) Item(args...);
-		assert(item != nullptr);
-
-		// Inserting the new item at the end of the list
-		Item *p = list_.get();
-
-		if (p == nullptr) {
-			list_.insert(item);
-
-		} else {
-			while (p->getNextListNode() != nullptr) {
-				p = p->getNextListNode();
-			}
-
-			assert(p->getNextListNode() == nullptr);
-			p->setNextListNode(item);
-			assert(p->getNextListNode()->getNextListNode() == nullptr);
-		}
-
-		return true;
-	}
-
-	/**
-	 * Accesses the first element.
-	 * Nullptr will be returned if the queue is empty.
-	 * Complexity is O(1).
-	 */
-	T       *peek()       { return isEmpty() ? nullptr : &list_.get()->payload; }
-	const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
-
-	/**
-	 * Removes the first element.
-	 * If the queue is empty, nothing will be done and assertion failure will be triggered.
-	 * Complexity is O(1).
-	 */
-	void pop()
-	{
-		Item *const item = list_.get();
-		assert(item != nullptr);
-
-		if (item != nullptr) {
-			list_.remove(item);
-			item->~Item();
-			allocator_.deallocate(item);
-		}
-	}
-};
-
-/**
- * Objects of this class are owned by the sub-node thread.
- * This class does not use heap memory.
- */
-class VirtualCanIface : public uavcan::ICanIface,
-	uavcan::Noncopyable
-{
-	/**
-	 * This class re-defines uavcan::RxCanFrame with flags.
-	 * Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
-	 * rendering this class unusable with Queue<>.
-	 */
-	struct RxItem: public uavcan::CanFrame {
-		const uavcan::MonotonicTime ts_mono;
-		const uavcan::UtcTime ts_utc;
-		const uavcan::CanIOFlags flags;
-		const uint8_t iface_index;
-
-		RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
-			uavcan::CanFrame(arg_frame),
-			ts_mono(arg_frame.ts_mono),
-			ts_utc(arg_frame.ts_utc),
-			flags(arg_flags),
-			iface_index(arg_frame.iface_index)
-		{
-			// Making sure it will fit into a pool block with a pointer prefix
-			static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
-		}
-	};
-
-	pthread_mutex_t &common_driver_mutex_;
-
-	uavcan::CanTxQueue prioritized_tx_queue_;
-	Queue<RxItem> rx_queue_;
-
-	int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
-	{
-		Lock lock(common_driver_mutex_);
-		prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
-		return 1;
-	}
-
-	int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
-			uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
-	{
-		Lock lock(common_driver_mutex_);
-
-		if (rx_queue_.isEmpty()) {
-			return 0;
-		}
-
-		const auto item = *rx_queue_.peek();
-		rx_queue_.pop();
-
-		out_frame = item;
-		out_ts_monotonic = item.ts_mono;
-		out_ts_utc = item.ts_utc;
-		out_flags = item.flags;
-
-		return 1;
-	}
-
-	int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
-	uint16_t getNumFilters() const override { return 0; }
-	uint64_t getErrorCount() const override { return 0; }
-
-public:
-	VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
-			pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
-		common_driver_mutex_(arg_mutex),
-		prioritized_tx_queue_(allocator, clock, quota_per_queue),
-		rx_queue_(allocator, quota_per_queue)
-	{
-	}
-
-	~VirtualCanIface()
-	{
-	}
-
-	/**
-	 * Note that RX queue overwrites oldest items when overflowed.
-	 * Call this from the main thread only.
-	 * No additional locking is required.
-	 */
-	void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
-	{
-		Lock lock(common_driver_mutex_);
-
-		if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
-			rx_queue_.pop();
-			(void)rx_queue_.tryEmplace(frame, flags);
-		}
-	}
-
-	/**
-	 * Call this from the main thread only.
-	 * No additional locking is required.
-	 */
-	void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
-	{
-		Lock lock(common_driver_mutex_);
-		const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
-
-		while (auto e = prioritized_tx_queue_.peek()) {
-			UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
-				     unsigned(iface_mask), e->toString().c_str());
-
-			const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
-								uavcan::CanTxQueue::Qos(e->qos), e->flags);
-
-			prioritized_tx_queue_.remove(e);
-
-			if (res <= 0) {
-				break;
-			}
-
-		}
-	}
-
-	/**
-	 * Call this from the sub-node thread only.
-	 * No additional locking is required.
-	 */
-	bool hasDataInRxQueue()
-	{
-		Lock lock(common_driver_mutex_);
-		return !rx_queue_.isEmpty();
-	}
-};
-
-/**
- * This interface defines one method that will be called by the main node thread periodically in order to
- * transfer contents of TX queue of the sub-node into the TX queue of the main node.
- */
-class ITxQueueInjector
-{
-public:
-	virtual ~ITxQueueInjector() { }
-
-	/**
-	 * Flush contents of TX queues into the main node.
-	 * @param main_node         Reference to the main node.
-	 */
-	virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
-};
-
-/**
- * Objects of this class are owned by the sub-node thread.
- * This class does not use heap memory.
- */
-class VirtualCanDriver : public uavcan::ICanDriver,
-	public uavcan::IRxFrameListener,
-	public ITxQueueInjector,
-	uavcan::Noncopyable
-{
-	class Event
-	{
-		FAR px4_sem_t sem;
-
-
-	public:
-
-		int init()
-		{
-			int rv = px4_sem_init(&sem, 0, 0);
-
-			if (rv == 0) {
-				px4_sem_setprotocol(&sem, SEM_PRIO_NONE);
-			}
-
-			return rv;
-		}
-
-		int deinit()
-		{
-			return px4_sem_destroy(&sem);
-		}
-
-
-		Event()
-		{
-		}
-
-		~Event()
-		{
-		}
-
-
-		/**
-		 */
-
-		void waitFor(uavcan::MonotonicDuration duration)
-		{
-			static const int NsPerSec = 1000000000;
-
-			if (duration.isPositive()) {
-				auto abstime = ::timespec();
-
-				if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
-					abstime.tv_nsec += duration.toUSec() * 1000;
-
-					if (abstime.tv_nsec >= NsPerSec) {
-						abstime.tv_sec++;
-						abstime.tv_nsec -= NsPerSec;
-					}
-
-					(void)px4_sem_timedwait(&sem, &abstime);
-				}
-			}
-		}
-
-		void signal()
-		{
-			int count;
-			int rv = px4_sem_getvalue(&sem, &count);
-
-			if (rv == 0 && count <= 0) {
-				px4_sem_post(&sem);
-			}
-		}
-	};
-
-	Event event_;                     ///< Used to unblock the select() call when IO happens.
-	pthread_mutex_t driver_mutex_;    ///< Shared across all ifaces
-	uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
-	const unsigned num_ifaces_;
-	uavcan::ISystemClock &clock_;
-
-	uavcan::ICanIface *getIface(uint8_t iface_index) override
-	{
-		return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
-	}
-
-	uint8_t getNumIfaces() const override { return num_ifaces_; }
-
-	/**
-	 * This and other methods of ICanDriver will be invoked by the sub-node thread.
-	 */
-	int16_t select(uavcan::CanSelectMasks &inout_masks,
-		       const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
-		       uavcan::MonotonicTime blocking_deadline) override
-	{
-		bool need_block = (inout_masks.write == 0);    // Write queue is infinite
-
-		for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
-			const bool need_read = inout_masks.read & (1U << i);
-
-			if (need_read && ifaces_[i]->hasDataInRxQueue()) {
-				need_block = false;
-			}
-		}
-
-		if (need_block) {
-			event_.waitFor(blocking_deadline - clock_.getMonotonic());
-		}
-
-		inout_masks = uavcan::CanSelectMasks();
-
-		for (unsigned i = 0; i < num_ifaces_; i++) {
-			const std::uint8_t iface_mask = 1U << i;
-			inout_masks.write |= iface_mask;           // Always ready to write
-
-			if (ifaces_[i]->hasDataInRxQueue()) {
-				inout_masks.read |= iface_mask;
-			}
-		}
-
-		return num_ifaces_;       // We're always ready to write, hence > 0.
-	}
-
-	/**
-	 * This handler will be invoked by the main node thread.
-	 */
-	void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
-	{
-		UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
-
-		if (frame.iface_index < num_ifaces_) {
-			ifaces_[frame.iface_index]->addRxFrame(frame, flags);
-			event_.signal();
-
-		}
-	}
-
-	/**
-	 * This method will be invoked by the main node thread.
-	 */
-	void injectTxFramesInto(uavcan::INode &main_node) override
-	{
-		for (unsigned i = 0; i < num_ifaces_; i++) {
-			ifaces_[i]->flushTxQueueTo(main_node, i);
-		}
-
-		event_.signal();
-	}
-
-public:
-	VirtualCanDriver(unsigned arg_num_ifaces,
-			 uavcan::ISystemClock &system_clock,
-			 uavcan::IPoolAllocator &allocator,
-			 unsigned virtual_iface_block_allocation_quota) :
-		num_ifaces_(arg_num_ifaces),
-		clock_(system_clock)
-	{
-		Lock::init(driver_mutex_);
-		event_.init();
-
-		assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
-
-		const unsigned quota_per_queue = virtual_iface_block_allocation_quota;             // 2x overcommit
-
-		for (unsigned i = 0; i < num_ifaces_; i++) {
-			ifaces_[i].construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &, pthread_mutex_t &,
-				unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
-		}
-	}
-
-	~VirtualCanDriver()
-	{
-		Lock::deinit(driver_mutex_);
-		event_.deinit();
-	}
-
-};