posix: add mavlink shell for posix targets (#19800)

platforms/posix/src/px4/common/px4_daemon/pxh.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2015-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2015-2022 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
@@ -43,28 +43,32 @@
 #include <string>
 #include <sstream>
 #include <vector>
+#include <algorithm>
 #include <stdio.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <unistd.h>
 
 #include "pxh.h"
 
 namespace px4_daemon
 {
 
-Pxh *Pxh::_instance = nullptr;
-
 apps_map_type Pxh::_apps = {};
-
+Pxh *Pxh::_instance = nullptr;
 
 Pxh::Pxh()
 {
 	_history.try_to_add("commander takeoff"); // for convenience
 	_history.reset_to_end();
-	_instance = this;
 }
 
 Pxh::~Pxh()
 {
-	_instance = nullptr;
+	if (_local_terminal) {
+		tcsetattr(0, TCSANOW, &_orig_term);
+		_instance = nullptr;
+	}
 }
 
 int Pxh::process_line(const std::string &line, bool silently_fail)
@@ -133,11 +137,167 @@ int Pxh::process_line(const std::string &line, bool silently_fail)
 	}
 }
 
+void Pxh::_check_remote_uorb_command(std::string &line)
+{
+
+	if (line.empty()) {
+		return;
+	}
+
+	std::stringstream line_stream(line);
+	std::string word;
+
+	line_stream >> word;
+
+	if (word == "uorb") {
+		line += " -1";  // Run uorb command only once
+	}
+}
+
+void Pxh::run_remote_pxh(int remote_in_fd, int remote_out_fd)
+{
+	std::string mystr;
+	int p1[2], pipe_stdout;
+	int p2[2], pipe_stderr;
+	int backup_stdout_fd = dup(STDOUT_FILENO);
+	int backup_stderr_fd = dup(STDERR_FILENO);
+
+	if (pipe(p1) != 0) {
+		perror("Remote shell pipe creation failed");
+		return;
+	}
+
+	if (pipe(p2) != 0) {
+		perror("Remote shell pipe 2 creation failed");
+		close(p1[0]);
+		close(p1[1]);
+		return;
+	}
+
+	// Create pipe to receive stdout and stderr
+	dup2(p1[1], STDOUT_FILENO);
+	close(p1[1]);
+
+	dup2(p2[1], STDERR_FILENO);
+	close(p2[1]);
+
+	pipe_stdout = p1[0];
+	pipe_stderr = p2[0];
+
+	// Set fds for non-blocking operation
+	fcntl(pipe_stdout, F_SETFL, fcntl(pipe_stdout, F_GETFL) | O_NONBLOCK);
+	fcntl(pipe_stderr, F_SETFL, fcntl(pipe_stderr, F_GETFL) | O_NONBLOCK);
+	fcntl(remote_in_fd, F_SETFL, fcntl(remote_in_fd, F_GETFL) | O_NONBLOCK);
+
+	// Check for input on any pipe (i.e. stdout, stderr, or remote_in_fd
+	// stdout and stderr will be sent to the local terminal and a copy of the data
+	// will be sent over to the mavlink shell through the remote_out_fd.
+	//
+	// Any data from remote_in_fd will be process as shell commands when an '\n' is received
+	while (!_should_exit) {
+
+		struct pollfd fds[3] { {pipe_stderr, POLLIN}, {pipe_stdout, POLLIN}, {remote_in_fd, POLLIN}};
+
+		if (poll(fds, 3, -1) == -1) {
+			perror("Mavlink Shell Poll Error");
+			break;
+		}
+
+		if (fds[0].revents & POLLIN) {
+
+			uint8_t buffer[512];
+			size_t len;
+
+			if ((len = read(pipe_stderr, buffer, sizeof(buffer))) <= 0) {
+				break; //EOF or ERROR
+			}
+
+			// Send all the stderr data to the local terminal as well as the remote shell
+			if (write(backup_stderr_fd, buffer, len) <= 0) {
+				perror("Remote shell write stdout");
+				break;
+			}
+
+			if (write(remote_out_fd, buffer, len) <= 0) {
+				perror("Remote shell write");
+				break;
+			}
+
+			// Process all the stderr data first
+			continue;
+		}
+
+		if (fds[1].revents & POLLIN) {
+
+			uint8_t buffer[512];
+			size_t len;
+
+			if ((len = read(pipe_stdout, buffer, sizeof(buffer))) <= 0) {
+				break; //EOF or ERROR
+			}
+
+			// Send all the stdout data to the local terminal as well as the remote shell
+			if (write(backup_stdout_fd, buffer, len) <= 0) {
+				perror("Remote shell write stdout");
+				break;
+			}
+
+			if (write(remote_out_fd, buffer, len) <= 0) {
+				perror("Remote shell write");
+				break;
+			}
+		}
+
+		if (fds[2].revents & POLLIN) {
+
+			char c;
+
+			if (read(remote_in_fd, &c, 1) <= 0) {
+				break; // EOF or ERROR
+			}
+
+			switch (c) {
+
+			case '\n':	// user hit enter
+				printf("\n");
+				_check_remote_uorb_command(mystr);
+				process_line(mystr, false);
+				// reset string
+				mystr = "";
+
+				_print_prompt();
+
+				break;
+
+			default:	// any other input
+				if (c > 3) {
+					fprintf(stdout, "%c", c);
+					fflush(stdout);
+					mystr += (char)c;
+				}
+
+				break;
+			}
+		}
+	}
+
+	// Restore stdout and stderr
+	dup2(backup_stdout_fd, STDOUT_FILENO);
+	dup2(backup_stderr_fd, STDERR_FILENO);
+	close(backup_stdout_fd);
+	close(backup_stderr_fd);
+
+	close(pipe_stdout);
+	close(pipe_stderr);
+	close(remote_in_fd);
+	close(remote_out_fd);
+}
 
 void Pxh::run_pxh()
 {
-	_should_exit = false;
+	// Only the local_terminal needed for static calls
-
+	_instance = this;
+	_local_terminal = true;
 	_setup_term();
 
 	std::string mystr;
@@ -154,7 +314,10 @@ void Pxh::run_pxh()
 
 		switch (c) {
 		case EOF:
-			_should_exit = true;
+			break;
+
+		case '\t':
+			_tab_completion(mystr);
 			break;
 
 		case 127:	// backslash
@@ -230,7 +393,6 @@ void Pxh::run_pxh()
 			break;
 		}
 
-
 		if (update_prompt) {
 			// reprint prompt with mystr
 			mystr.insert(mystr.length() - cursor_position, add_string);
@@ -243,14 +405,11 @@ void Pxh::run_pxh()
 				_move_cursor(cursor_position);
 			}
 		}
-
 	}
 }
 
 void Pxh::stop()
 {
-	_restore_term();
-
 	if (_instance) {
 		_instance->_should_exit = true;
 	}
@@ -294,4 +453,101 @@ void Pxh::_move_cursor(int position)
 	printf("\033[%dD", position);
 }
 
+void Pxh::_tab_completion(std::string &mystr)
+{
+	// parse line and get command
+	std::stringstream line(mystr);
+	std::string cmd;
+	line >> cmd;
+
+	// cmd is empty or white space send a list of available commands
+	if (cmd.size() == 0) {
+
+		printf("\n");
+
+		for (auto it = _apps.begin(); it != _apps.end();  ++it) {
+			printf("%s ", it->first.c_str());
+		}
+
+		printf("\n");
+		mystr = "";
+
+	} else {
+
+		// find tab completion matches
+		std::vector<std::string> matches;
+
+		for (auto it = _apps.begin(); it != _apps.end();  ++it) {
+			if (it->first.compare(0, cmd.size(), cmd) == 0) {
+				matches.push_back(it->first);
+			}
+		}
+
+		if (matches.size() >= 1) {
+			// if more than one match print all matches
+			if (matches.size() != 1) {
+				printf("\n");
+
+				for (const auto &item : matches) {
+					printf("%s    ", item.c_str());
+				}
+
+				printf("\n");
+			}
+
+			// find minimum size match
+			size_t min_size = 0;
+
+			for (const auto &item : matches) {
+				if (min_size == 0) {
+					min_size = item.size();
+
+				} else if (item.size() < min_size) {
+					min_size = item.size();
+				}
+			}
+
+			// parse through elements to find longest match
+			std::string longest_match;
+			bool done = false;
+
+			for (int i = 0; i < (int)min_size ; ++i) {
+				bool first_time = true;
+
+				for (const auto &item : matches) {
+					if (first_time) {
+						longest_match += item[i];
+						first_time = false;
+
+					} else if (longest_match[i] != item[i]) {
+						done = true;
+						longest_match.pop_back();
+						break;
+					}
+				}
+
+				if (done) { break; }
+
+				mystr = longest_match;
+			}
+		}
+
+		std::string flags;
+
+		while (line >> cmd) {
+			flags += " " + cmd;
+		}
+
+		// add flags back in when there is a command match
+		if (matches.size() == 1) {
+			if (flags.empty()) {
+				mystr += " ";
+
+			} else {
+				mystr += flags;
+			}
+		}
+	}
+}
+
 } // namespace px4_daemon

platforms/posix/src/px4/common/px4_daemon/pxh.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2016-2022 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
@@ -72,7 +72,12 @@ public:
 	void run_pxh();
 
 	/**
-	 * Can be called to stop pxh.
+	 * Run the remote mavlink pxh shell.
+	 */
+	void run_remote_pxh(int remote_in_fd, int remote_out_fd);
+
+	/**
+	 * Can be called to stop all pxh shells.
 	 */
 	static void stop();
 
@@ -80,11 +85,14 @@ private:
 	void _print_prompt();
 	void _move_cursor(int position);
 	void _clear_line();
+	void _tab_completion(std::string &prefix);
+	void _check_remote_uorb_command(std::string &line);
 
 	void _setup_term();
 	static void _restore_term();
 
 	bool _should_exit{false};
+	bool _local_terminal{false};
 	History _history;
 	struct termios _orig_term {};
 

platforms/posix/src/px4/common/px4_daemon/server.cpp

@@ -150,8 +150,12 @@ Server::_server_main()
 		int n_ready = poll(poll_fds.data(), poll_fds.size(), -1);
 
 		if (n_ready < 0) {
-			PX4_ERR("poll() failed: %s", strerror(errno));
+			// Reboot command causes System Interrupt to stop poll(). This is not an error
-			return;
+			if (errno != EINTR) {
+				PX4_ERR("poll() failed: %s", strerror(errno));
+			}
+
+			break;
 		}
 
 		_lock();

src/modules/mavlink/mavlink_shell.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2022 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
@@ -45,11 +45,14 @@
 #include <errno.h>
 #include <sys/ioctl.h>
 
-
 #ifdef __PX4_NUTTX
 #include <nshlib/nshlib.h>
 #endif /* __PX4_NUTTX */
 
+#ifdef __PX4_POSIX
+#include "../../../platforms/posix/src/px4/common/px4_daemon/pxh.h"
+#endif /* __PX4_POSIX */
+
 #ifdef __PX4_CYGWIN
 #include <asm/socket.h>
 #endif
@@ -69,11 +72,10 @@ MavlinkShell::~MavlinkShell()
 
 int MavlinkShell::start()
 {
-	//this currently only works for NuttX
+	//this currently only works for NuttX & POSIX
-#ifndef __PX4_NUTTX
+#if !defined(__PX4_NUTTX) && !defined(__PX4_POSIX)
 	return -1;
-#endif /* __PX4_NUTTX */
+#endif
-
 
 	PX4_INFO("Starting mavlink shell");
 
@@ -113,6 +115,17 @@ int MavlinkShell::start()
 	fflush(stdout);
 	fflush(stderr);
 
+#ifdef __PX4_POSIX
+	int remote_in_fd = dup(_shell_fds[0]);	// Input file descriptor for the remote shell
+	int remote_out_fd = dup(_shell_fds[1]); // Output file descriptor for the remote shell
+
+	char r_in[32];
+	char r_out[32];
+	sprintf(r_in, "%d", remote_in_fd);
+	sprintf(r_out, "%d", remote_out_fd);
+	char *const argv[3] = {r_in, r_out, nullptr};
+
+#else
 	int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task
 
 	for (int i = 0; i < 2; ++i) {
@@ -125,6 +138,7 @@ int MavlinkShell::start()
 
 	dup2(_shell_fds[0], 0);
 	dup2(_shell_fds[1], 1);
+#endif
 
 	if (ret == 0) {
 		_task = px4_task_spawn_cmd("mavlink_shell",
@@ -132,13 +146,19 @@ int MavlinkShell::start()
 					   SCHED_PRIORITY_DEFAULT,
 					   2048,
 					   &MavlinkShell::shell_start_thread,
+#ifdef __PX4_POSIX
+					   argv);
+#else
 					   nullptr);
+#endif
 
 		if (_task < 0) {
 			ret = -1;
 		}
 	}
 
+#if !defined(__PX4_POSIX)
+
 	//restore fd's
 	for (int i = 0; i < 2; ++i) {
 		if (dup2(fd_backups[i], i) == -1) {
@@ -148,6 +168,8 @@ int MavlinkShell::start()
 		close(fd_backups[i]);
 	}
 
+#endif
+
 	//close unused pipe fd's
 	close(_shell_fds[0]);
 	close(_shell_fds[1]);
@@ -161,12 +183,26 @@ int MavlinkShell::start()
 
 int MavlinkShell::shell_start_thread(int argc, char *argv[])
 {
+#ifdef __PX4_NUTTX
 	dup2(1, 2); //redirect stderror to stdout
 
-#ifdef __PX4_NUTTX
 	nsh_consolemain(0, NULL);
 #endif /* __PX4_NUTTX */
 
+#ifdef __PX4_POSIX
+
+	if (argc != 3) {
+		PX4_ERR("Mavlink shell bug");
+		return -1;
+	}
+
+	int remote_in_fd = atoi(argv[1]);
+	int remote_out_fd = atoi(argv[2]);
+
+	px4_daemon::Pxh pxh;
+	pxh.run_remote_pxh(remote_in_fd, remote_out_fd);
+#endif
+
 	return 0;
 }