mavlink USB auto start/stop on boards with VBUS

- no longer start sercon or mavlink usb by default
 - on USB connection (VBUS) monitor serial USB at low rate and start Mavlink if there's a HEARTBEAT or nshterm on 3 consecutive carriage returns
 - the mavlink USB instance is automatically stopped and serdis executed if USB is disconnected
 - skipping Mavlink USB (and sercon) saves a considerable amount of memory on older boards

src/modules/commander/Commander.cpp

@@ -1911,31 +1911,6 @@ Commander::run()
 					_status_flags.condition_power_input_valid = true;
 				}
 
-#if defined(CONFIG_BOARDCTL_RESET)
-
-				if (!_status_flags.circuit_breaker_engaged_usb_check && _status_flags.usb_connected) {
-					/* if the USB hardware connection went away, reboot */
-					if (_system_power_usb_connected && !system_power.usb_connected) {
-						/*
-						 * Apparently the USB cable went away but we are still powered,
-						 * so we bring the system back to a nominal state for flight.
-						 * This is important to unload the USB stack of the OS which is
-						 * a relatively complex piece of software that is non-essential
-						 * for flight and continuing to run it would add a software risk
-						 * without a need. The clean approach to unload it is to reboot.
-						 */
-						if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
-							mavlink_log_critical(&_mavlink_log_pub, "USB disconnected, rebooting for flight safety\t");
-							events::send(events::ID("commander_reboot_usb_disconnect"), {events::Log::Critical, events::LogInternal::Info},
-								     "USB disconnected, rebooting for flight safety");
-
-							while (1) { px4_usleep(1); }
-						}
-					}
-				}
-
-#endif // CONFIG_BOARDCTL_RESET
-
 				_system_power_usb_connected = system_power.usb_connected;
 			}
 		}

src/modules/mavlink/mavlink_main.cpp

@@ -127,34 +127,53 @@ Mavlink::Mavlink() :
 	}
 
 	_vehicle_command_sub.subscribe();
+
+	if (orb_exists(ORB_ID(event), 0) == PX4_ERROR) {
+		orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH);
+	}
+
+	_event_sub.subscribe();
 }
 
 Mavlink::~Mavlink()
 {
-	if (_task_running) {
+	if (running()) {
 		/* task wakes up every 10ms or so at the longest */
-		_task_should_exit = true;
+		request_stop();
 
 		/* wait for a second for the task to quit at our request */
 		unsigned i = 0;
 
 		do {
-			/* wait 20ms */
-			px4_usleep(20000);
+			/* wait at least 1 second (10ms * 10) */
+			px4_usleep(10000);
 
 			/* if we have given up, kill it */
-			if (++i > 50) {
-				//TODO store main task handle in Mavlink instance to allow killing task
-				//task_delete(_mavlink_task);
+			if (++i > 100) {
+				PX4_ERR("mavlink didn't stop, killing task %d", _task_id);
+				px4_task_delete(_task_id);
 				break;
 			}
-		} while (_task_running);
+		} while (running());
 	}
 
 	if (_instance_id >= 0) {
 		mavlink_module_instances[_instance_id] = nullptr;
 	}
 
+	// if this instance was responsible for checking events then select a new mavlink instance
+	if (check_events()) {
+		check_events_disable();
+
+		// select next available instance
+		for (Mavlink *inst : mavlink_module_instances) {
+			if (inst) {
+				inst->check_events_enable();
+				break;
+			}
+		}
+	}
+
 	perf_free(_loop_perf);
 	perf_free(_loop_interval_perf);
 	perf_free(_send_byte_error_perf);
@@ -230,6 +249,20 @@ Mavlink::set_instance_id()
 {
 	LockGuard lg{mavlink_module_mutex};
 
+	// instance count
+	size_t inst_count = 0;
+
+	for (Mavlink *inst : mavlink_module_instances) {
+		if (inst != nullptr) {
+			inst_count++;
+		}
+	}
+
+	// if this is the first instance use it to check events
+	if (inst_count == 0) {
+		check_events_enable();
+	}
+
 	for (int instance_id = 0; instance_id < MAVLINK_COMM_NUM_BUFFERS; instance_id++) {
 		if (mavlink_module_instances[instance_id] == nullptr) {
 			mavlink_module_instances[instance_id] = this;
@@ -312,9 +345,9 @@ Mavlink::destroy_all_instances()
 	for (Mavlink *inst_to_del : mavlink_module_instances) {
 		if (inst_to_del != nullptr) {
 			/* set flag to stop thread and wait for all threads to finish */
-			inst_to_del->_task_should_exit = true;
+			inst_to_del->request_stop();
 
-			while (inst_to_del->_task_running) {
+			while (inst_to_del->running()) {
 				printf(".");
 				fflush(stdout);
 				px4_usleep(10000);
@@ -527,44 +560,9 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
 		return -EINVAL;
 	}
 
-	/* back off 1800 ms to avoid running into the USB setup timing */
-	while (_is_usb_uart && hrt_absolute_time() < 1800U * 1000U) {
-		px4_usleep(50000);
-	}
-
 	/* open uart */
 	_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
 
-	/* if this is a config link, stay here and wait for it to open */
-	if (_uart_fd < 0 && _is_usb_uart) {
-
-		uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
-
-		/* get the system arming state and abort on arming */
-		while (_uart_fd < 0 && !_task_should_exit) {
-
-			/* another task might have requested subscriptions: make sure we handle it */
-			check_requested_subscriptions();
-
-			/* abort if an arming topic is published and system is armed */
-			armed_sub.update();
-
-			/* the system is now providing arming status feedback.
-			 * instead of timing out, we resort to abort bringing
-			 * up the terminal.
-			 */
-			if (armed_sub.get().armed) {
-				/* this is not an error, but we are done */
-				return -1;
-			}
-
-			int errcode = errno;
-			/* ENOTCONN means that the USB device is not yet connected */
-			px4_usleep(errcode == ENOTCONN ? 1000000 :  100000);
-			_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
-		}
-	}
-
 	/*
 	 * Return here in the iridium mode since the iridium driver does not
 	 * support the subsequent function calls.
@@ -587,21 +585,11 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
 	/* Clear ONLCR flag (which appends a CR for every LF) */
 	uart_config.c_oflag &= ~ONLCR;
 
-	if (!_is_usb_uart) {
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
-			::close(_uart_fd);
-			return -1;
-		}
-
-	} else {
-
-		/* USB has no baudrate, but use a magic number for 'fast' */
-		_baudrate = 2000000;
-
-		set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
+		::close(_uart_fd);
+		return -1;
 	}
 
 #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
@@ -626,12 +614,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
 int
 Mavlink::setup_flow_control(enum FLOW_CONTROL_MODE mode)
 {
-	// We can't do this on USB - skip
-	if (_is_usb_uart) {
-		_flow_control_mode = FLOW_CONTROL_OFF;
-		return OK;
-	}
-
 	struct termios uart_config;
 
 	int ret = tcgetattr(_uart_fd, &uart_config);
@@ -1251,7 +1233,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 	/* orb subscription must be done from the main thread,
 	 * set _subscribe_to_stream and _subscribe_to_stream_rate fields
 	 * which polled in mavlink main loop */
-	if (!_task_should_exit) {
+	if (!should_exit()) {
 		/* wait for previous subscription completion */
 		while (_subscribe_to_stream != nullptr) {
 			px4_usleep(MAIN_LOOP_DELAY / 2);
@@ -2127,17 +2109,23 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	/* USB serial is indicated by /dev/ttyACMx */
-	if (strcmp(_device_name, "/dev/ttyACM0") == OK || strcmp(_device_name, "/dev/ttyACM1") == OK) {
+	if (strncmp(_device_name, "/dev/ttyACM", 11) == 0) {
 		if (_datarate == 0) {
 			_datarate = 800000;
 		}
 
+		/* USB has no baudrate, but use a magic number for 'fast' */
+		_baudrate = 2000000;
+
 		if (_mode == MAVLINK_MODE_COUNT) {
 			_mode = MAVLINK_MODE_CONFIG;
 		}
 
 		_ftp_on = true;
 		_is_usb_uart = true;
+		_flow_control_mode = FLOW_CONTROL_OFF;
+
+		set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
 	}
 
 	if (_mode == MAVLINK_MODE_COUNT) {
@@ -2183,6 +2171,12 @@ Mavlink::task_main(int argc, char *argv[])
 	if (!set_instance_id()) {
 		PX4_ERR("no instances available");
 		return PX4_ERROR;
+
+	} else {
+		// set thread name
+		char thread_name[13];
+		snprintf(thread_name, sizeof(thread_name), "mavlink_if%d", get_instance_id());
+		px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
 	}
 
 	set_channel();
@@ -2249,13 +2243,9 @@ Mavlink::task_main(int argc, char *argv[])
 	if (get_protocol() == Protocol::SERIAL) {
 		_uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control);
 
-		if (_uart_fd < 0 && !_is_usb_uart) {
+		if (_uart_fd < 0) {
 			PX4_ERR("could not open %s", _device_name);
 			return PX4_ERROR;
-
-		} else if (_uart_fd < 0 && _is_usb_uart) {
-			/* the config link is optional */
-			return PX4_OK;
 		}
 	}
 
@@ -2268,6 +2258,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 #endif // MAVLINK_UDP
 
+	_task_id = px4_getpid();
+
 	/* if the protocol is serial, we send the system version blindly */
 	if (get_protocol() == Protocol::SERIAL) {
 		send_autopilot_capabilities();
@@ -2275,17 +2267,11 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_receiver.start();
 
-	/* Events subscription: only the first MAVLink instance should check */
-	uORB::Subscription event_sub{ORB_ID(event)};
-	const bool should_check_events = _instance_id == 0;
 	uint16_t event_sequence_offset = 0; // offset to account for skipped events, not sent via MAVLink
-	// ensure topic exists, otherwise we might lose first queued events
-	orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH);
-	event_sub.subscribe();
 
 	_mavlink_start_time = hrt_absolute_time();
 
-	while (!_task_should_exit) {
+	while (!should_exit()) {
 		/* main loop */
 		px4_usleep(_main_loop_delay);
 
@@ -2504,23 +2490,27 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		/* handle new events */
-		if (should_check_events) {
-			event_s orb_event;
+		if (check_events()) {
+			if (_event_sub.updated()) {
+				LockGuard lg{mavlink_module_mutex};
 
-			while (event_sub.update(&orb_event)) {
-				if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) {
-					++event_sequence_offset; // skip this event
+				event_s orb_event;
 
-				} else {
-					events::Event e;
-					e.id = orb_event.id;
-					e.timestamp_ms = orb_event.timestamp / 1000;
-					e.sequence = orb_event.event_sequence - event_sequence_offset;
-					e.log_levels = orb_event.log_levels;
-					static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments),
-						      "uorb message event: arguments size mismatch");
-					memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments));
-					_event_buffer->insert_event(e);
+				while (_event_sub.update(&orb_event)) {
+					if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) {
+						++event_sequence_offset; // skip this event
+
+					} else {
+						events::Event e;
+						e.id = orb_event.id;
+						e.timestamp_ms = orb_event.timestamp / 1000;
+						e.sequence = orb_event.event_sequence - event_sequence_offset;
+						e.log_levels = orb_event.log_levels;
+						static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments),
+							      "uorb message event: arguments size mismatch");
+						memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments));
+						_event_buffer->insert_event(e);
+					}
 				}
 			}
 		}
@@ -2608,7 +2598,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* delete streams */
 	_streams.clear();
 
-	if (_uart_fd >= 0 && !_is_usb_uart) {
+	if (_uart_fd >= 0) {
 		/* discard all pending data, as close() might block otherwise on NuttX with flow control enabled */
 		tcflush(_uart_fd, TCIOFLUSH);
 		/* close UART */
@@ -2633,8 +2623,6 @@ Mavlink::task_main(int argc, char *argv[])
 	pthread_mutex_destroy(&_send_mutex);
 	pthread_mutex_destroy(&_radio_status_mutex);
 
-	_task_running = false;
-
 	PX4_INFO("exiting channel %i", (int)_channel);
 
 	return OK;
@@ -2791,16 +2779,15 @@ int Mavlink::start_helper(int argc, char *argv[])
 	int res;
 
 	if (!instance) {
-
 		/* out of memory */
 		res = -ENOMEM;
 		PX4_ERR("OUT OF MEM");
 
 	} else {
 		/* this will actually only return once MAVLink exits */
+		instance->_task_running.store(true);
 		res = instance->task_main(argc, argv);
-		instance->_task_running = false;
-
+		instance->_task_running.store(false);
 	}
 
 	return res;
@@ -2838,14 +2825,12 @@ Mavlink::start(int argc, char *argv[])
 	}
 
 	// Instantiate thread
-	char buf[24];
-	sprintf(buf, "mavlink_if%d", ic);
 
 	// This is where the control flow splits
 	// between the starting task and the spawned
 	// task - start_helper() only returns
 	// when the started task exits.
-	px4_task_spawn_cmd(buf,
+	px4_task_spawn_cmd("mavlink_main",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
 			   PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
@@ -2907,7 +2892,7 @@ Mavlink::display_status()
 			printf("\t  rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors);
 			printf("\t  fixed:\t%" PRIu16 "\n", _rstatus.fix);
 
-		} else if (_is_usb_uart) {
+		} else if (_tstatus.type == telemetry_status_s::LINK_TYPE_USB) {
 			printf("USB CDC\n");
 
 		} else {
@@ -3014,6 +2999,107 @@ Mavlink::display_status_streams()
 	}
 }
 
+int
+Mavlink::stop_command(int argc, char *argv[])
+{
+	const char *device_name = nullptr;
+
+#if defined(MAVLINK_UDP)
+	char *eptr;
+	int temp_int_arg;
+	unsigned short network_port = 0;
+#endif // MAVLINK_UDP
+
+	bool provided_device = false;
+	bool provided_network_port = false;
+
+	/*
+	 * Called via main with original argv
+	 *   mavlink start
+	 *
+	 *  Remove 2
+	 */
+	argc -= 2;
+	argv += 2;
+
+	/* don't exit from getopt loop to leave getopt global variables in consistent state,
+	 * set error flag instead */
+	bool err_flag = false;
+
+	int i = 0;
+
+	while (i < argc) {
+		if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
+			provided_device = true;
+			device_name = argv[i + 1];
+			i++;
+
+#if defined(MAVLINK_UDP)
+
+		} else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
+			provided_network_port = true;
+			temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
+
+			if (*eptr == '\0') {
+				network_port = temp_int_arg;
+
+			} else {
+				err_flag = true;
+			}
+
+			i++;
+#endif // MAVLINK_UDP
+
+		} else {
+			err_flag = true;
+		}
+
+		i++;
+	}
+
+	if (!err_flag) {
+		Mavlink *inst = nullptr;
+
+		if (provided_device && !provided_network_port) {
+			inst = get_instance_for_device(device_name);
+
+#if defined(MAVLINK_UDP)
+
+		} else if (provided_network_port && !provided_device) {
+			inst = get_instance_for_network_port(network_port);
+#endif // MAVLINK_UDP
+
+		} else if (provided_device && provided_network_port) {
+			PX4_WARN("please provide either a device name or a network port");
+			return PX4_ERROR;
+		}
+
+		if (inst != nullptr) {
+			/* set flag to stop thread and wait for all threads to finish */
+			if (inst->running() && !inst->should_exit()) {
+				inst->request_stop();
+
+				LockGuard lg{mavlink_module_mutex};
+
+				for (int mavlink_instance = 0; mavlink_instance < MAVLINK_COMM_NUM_BUFFERS; mavlink_instance++) {
+					if (mavlink_module_instances[mavlink_instance] == inst) {
+						delete mavlink_module_instances[mavlink_instance];
+						mavlink_module_instances[mavlink_instance] = nullptr;
+						return PX4_OK;
+					}
+				}
+			}
+
+			return PX4_ERROR;
+		}
+
+	} else {
+		usage();
+	}
+
+	return PX4_ERROR;
+}
+
 int
 Mavlink::stream_command(int argc, char *argv[])
 {
@@ -3215,6 +3301,13 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 
 	PRINT_MODULE_USAGE_COMMAND_DESCR("stop-all", "Stop all instances");
 
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop a running instance");
+#if defined(CONFIG_NET) || defined(__PX4_POSIX)
+	PRINT_MODULE_USAGE_PARAM_INT('u', -1, 0, 65536, "Select Mavlink instance via local Network Port", true);
+#endif
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Select Mavlink instance via Serial Device", true);
+
+
 	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status for all instances");
 	PRINT_MODULE_USAGE_ARG("streams", "Print all enabled streams", true);
 
@@ -3241,11 +3334,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 		return Mavlink::start(argc, argv);
 
-	} else if (!strcmp(argv[1], "stop")) {
-		PX4_WARN("mavlink stop is deprecated, use stop-all instead");
-		usage();
-		return 1;
-
 	} else if (!strcmp(argv[1], "stop-all")) {
 		return Mavlink::destroy_all_instances();
 
@@ -3253,6 +3341,9 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
 		bool show_streams_status = argc > 2 && strcmp(argv[2], "streams") == 0;
 		return Mavlink::get_status_all_instances(show_streams_status);
 
+	} else if (!strcmp(argv[1], "stop")) {
+		return Mavlink::stop_command(argc, argv);
+
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);
 

src/modules/mavlink/mavlink_main.h

@@ -125,6 +125,14 @@ public:
 	 */
 	static int		start(int argc, char *argv[]);
 
+	bool running() const { return _task_running.load(); }
+	bool should_exit() const { return _task_should_exit.load(); }
+	void request_stop()
+	{
+		_task_should_exit.store(true);
+		_receiver.request_stop();
+	}
+
 	/**
 	 * Display the mavlink status.
 	 */
@@ -135,6 +143,7 @@ public:
 	 */
 	void			display_status_streams();
 
+	static int		stop_command(int argc, char *argv[]);
 	static int		stream_command(int argc, char *argv[]);
 
 	static int		instance_count();
@@ -166,6 +175,10 @@ public:
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
 
+	bool			check_events() const { return _should_check_events.load(); }
+	void			check_events_enable() { _should_check_events.store(true); }
+	void			check_events_disable() { _should_check_events.store(false); }
+
 	int			get_uart_fd() const { return _uart_fd; }
 
 	/**
@@ -455,8 +468,6 @@ public:
 
 	int 			get_socket_fd() { return _socket_fd; };
 
-	bool			_task_should_exit{false};	/**< Mavlink task should exit iff true. */
-
 #if defined(MAVLINK_UDP)
 	unsigned short		get_network_port() { return _network_port; }
 
@@ -529,7 +540,12 @@ public:
 
 private:
 	MavlinkReceiver 	_receiver;
+
 	int			_instance_id{-1};
+	int			_task_id{-1};
+
+	px4::atomic_bool	_task_should_exit{false};
+	px4::atomic_bool	_task_running{false};
 
 	bool			_transmitting_enabled{true};
 	bool			_transmitting_enabled_commanded{false};
@@ -540,12 +556,12 @@ private:
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::PublicationMulti<telemetry_status_s> _telemetry_status_pub{ORB_ID(telemetry_status)};
 
+	uORB::Subscription _event_sub{ORB_ID(event)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	bool			_task_running{true};
 	static bool		_boot_complete;
 
 	static constexpr int	MAVLINK_MIN_INTERVAL{1500};
@@ -562,6 +578,8 @@ private:
 	bool			_wait_to_transmit{false};  	/**< Wait to transmit until received messages. */
 	bool			_received_messages{false};	/**< Whether we've received valid mavlink messages. */
 
+	px4::atomic_bool	_should_check_events{false};    /**< Events subscription: only one MAVLink instance should check */
+
 	unsigned		_main_loop_delay{1000};	/**< mainloop delay, depends on data rate */
 
 	List<MavlinkStream *>		_streams;

src/modules/mavlink/mavlink_receiver.cpp

@@ -3179,7 +3179,7 @@ MavlinkReceiver::run()
 	ssize_t nread = 0;
 	hrt_abstime last_send_update = 0;
 
-	while (!_mavlink->_task_should_exit) {
+	while (!_mavlink->should_exit()) {
 
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {

src/modules/mavlink/mavlink_receiver.h

@@ -131,6 +131,8 @@ public:
 	void enable_message_statistics() { _message_statistics_enabled = true; }
 	void print_detailed_rx_stats() const;
 
+	void request_stop() { _should_exit.store(true); }
+
 private:
 	static void *start_trampoline(void *context);
 	void run();