diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp index 82d8021a3a..c17bbda267 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp +++ b/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 #include #include +#include #include +#include +#include +#include #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 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 diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.h b/platforms/posix/src/px4/common/px4_daemon/pxh.h index 2139277530..45bd4b8633 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.h +++ b/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 {}; diff --git a/platforms/posix/src/px4/common/px4_daemon/server.cpp b/platforms/posix/src/px4/common/px4_daemon/server.cpp index 1f1762bdf3..dab591cb3a 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server.cpp +++ b/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)); - return; + // Reboot command causes System Interrupt to stop poll(). This is not an error + if (errno != EINTR) { + PX4_ERR("poll() failed: %s", strerror(errno)); + } + + break; } _lock(); diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp index f0feafa623..38bd12932e 100644 --- a/src/modules/mavlink/mavlink_shell.cpp +++ b/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 #include - #ifdef __PX4_NUTTX #include #endif /* __PX4_NUTTX */ +#ifdef __PX4_POSIX +#include "../../../platforms/posix/src/px4/common/px4_daemon/pxh.h" +#endif /* __PX4_POSIX */ + #ifdef __PX4_CYGWIN #include #endif @@ -69,11 +72,10 @@ MavlinkShell::~MavlinkShell() int MavlinkShell::start() { - //this currently only works for NuttX -#ifndef __PX4_NUTTX + //this currently only works for NuttX & POSIX +#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; }