forked from Archive/PX4-Autopilot
posix: add mavlink shell for posix targets (#19800)
This commit is contained in:
Bruce Meagher
GitHub
parent
4cc3e78558
commit
714234ca90
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 {};
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue