diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h index f016d5b38c..0e40813843 100644 --- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h +++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h @@ -13,6 +13,7 @@ namespace PX4 { class PX4Util; class PX4GPIO; class PX4DigitalSource; + class NSHShellStream; } #endif //__AP_HAL_PX4_NAMESPACE_H__ diff --git a/libraries/AP_HAL_PX4/NSHShellStream.cpp b/libraries/AP_HAL_PX4/NSHShellStream.cpp new file mode 100644 index 0000000000..9ed074a517 --- /dev/null +++ b/libraries/AP_HAL_PX4/NSHShellStream.cpp @@ -0,0 +1,142 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + implementation of NSH shell as a stream, for use in nsh over MAVLink + + See GCS_serial_control.cpp for usage + */ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include +#include +#include +#include +#include +#include +#include +#include +#include "Scheduler.h" + +extern const AP_HAL::HAL& hal; + +#include "Util.h" +using namespace PX4; + +void NSHShellStream::shell_thread(void) +{ + close(0); + close(1); + close(2); + dup2(child.in, 0); + dup2(child.out, 1); + dup2(child.out, 2); + + nsh_consolemain(0, NULL); + + shell_stdin = -1; + shell_stdout = -1; + child.in = -1; + child.out = -1; +} + +void NSHShellStream::start_shell(void) +{ + if (hal.util->available_memory() < 8192) { + if (!showed_memory_warning) { + showed_memory_warning = true; + hal.console->printf("Not enough memory for shell\n"); + } + return; + } + if (hal.util->get_soft_armed()) { + if (!showed_armed_warning) { + showed_armed_warning = true; + hal.console->printf("Disarm to start nsh shell\n"); + } + // don't allow shell start when armed + return; + } + + int p1[2], p2[2]; + + if (pipe(p1) != 0) { + return; + } + if (pipe(p2) != 0) { + return; + } + shell_stdin = p1[0]; + shell_stdout = p2[1]; + child.in = p2[0]; + child.out = p1[1]; + + pthread_attr_t thread_attr; + struct sched_param param; + + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 2048); + + param.sched_priority = APM_SHELL_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&shell_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::NSHShellStream::shell_thread, this); +} + +size_t NSHShellStream::write(uint8_t b) +{ + if (shell_stdout == -1) { + start_shell(); + } + if (shell_stdout != -1) { + return ::write(shell_stdout, &b, 1); + } + return 0; +} + +size_t NSHShellStream::write(const uint8_t *buffer, size_t size) +{ + size_t ret = 0; + while (size > 0) { + if (write(*buffer++) != 1) { + break; + } + ret++; + size--; + } + return ret; +} + +int16_t NSHShellStream::read() +{ + if (shell_stdin == -1) { + start_shell(); + } + if (shell_stdin != -1) { + uint8_t b; + if (::read(shell_stdin, &b, 1) == 1) { + return b; + } + } + return -1; +} + +int16_t NSHShellStream::available() +{ + int ret = 0; + if (ioctl(shell_stdin, FIONREAD, (unsigned long)&ret) == OK) { + return ret; + } + return 0; +} + +int16_t NSHShellStream::txspace() +{ + int ret = 0; + if (ioctl(shell_stdout, FIONWRITE, (unsigned long)&ret) == OK) { + return ret; + } + return 0; +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index 4a3eddf86a..06505ab8fc 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -18,6 +18,7 @@ #define APM_UART_PRIORITY 60 #define APM_STORAGE_PRIORITY 59 #define APM_IO_PRIORITY 58 +#define APM_SHELL_PRIORITY 57 #define APM_OVERTIME_PRIORITY 10 #define APM_STARTUP_PRIORITY 10 diff --git a/libraries/AP_HAL_PX4/Util.cpp b/libraries/AP_HAL_PX4/Util.cpp index 43055f3696..4a70cf5634 100644 --- a/libraries/AP_HAL_PX4/Util.cpp +++ b/libraries/AP_HAL_PX4/Util.cpp @@ -34,36 +34,36 @@ PX4Util::PX4Util(void) : Util() */ bool PX4Util::run_debug_shell(AP_HAL::BetterStream *stream) { - PX4UARTDriver *uart = (PX4UARTDriver *)stream; - int fd; + PX4UARTDriver *uart = (PX4UARTDriver *)stream; + int fd; - // trigger exit in the other threads. This stops use of the - // various driver handles, and especially the px4io handle, - // which otherwise would cause a crash if px4io is stopped in - // the shell - _px4_thread_should_exit = true; + // trigger exit in the other threads. This stops use of the + // various driver handles, and especially the px4io handle, + // which otherwise would cause a crash if px4io is stopped in + // the shell + _px4_thread_should_exit = true; + + // take control of stream fd + fd = uart->_get_fd(); - // take control of stream fd - fd = uart->_get_fd(); - - // mark it blocking (nsh expects a blocking fd) - unsigned v; - v = fcntl(fd, F_GETFL, 0); - fcntl(fd, F_SETFL, v & ~O_NONBLOCK); - - // setup the UART on stdin/stdout/stderr - close(0); - close(1); - close(2); - dup2(fd, 0); - dup2(fd, 1); - dup2(fd, 2); - - nsh_consolemain(0, NULL); - - // this shouldn't happen - hal.console->printf("shell exited\n"); - return true; + // mark it blocking (nsh expects a blocking fd) + unsigned v; + v = fcntl(fd, F_GETFL, 0); + fcntl(fd, F_SETFL, v & ~O_NONBLOCK); + + // setup the UART on stdin/stdout/stderr + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + // this shouldn't happen + hal.console->printf("shell exited\n"); + return true; } /* diff --git a/libraries/AP_HAL_PX4/Util.h b/libraries/AP_HAL_PX4/Util.h index 8f3e797d0b..e90fc5de5b 100644 --- a/libraries/AP_HAL_PX4/Util.h +++ b/libraries/AP_HAL_PX4/Util.h @@ -5,6 +5,29 @@ #include #include "AP_HAL_PX4_Namespace.h" +class PX4::NSHShellStream : public AP_HAL::Stream { +public: + size_t write(uint8_t); + size_t write(const uint8_t *buffer, size_t size); + int16_t read(); + int16_t available(); + int16_t txspace(); +private: + int shell_stdin = -1; + int shell_stdout = -1; + pthread_t shell_thread_ctx; + + struct { + int in = -1; + int out = -1; + } child; + bool showed_memory_warning = false; + bool showed_armed_warning = false; + + void start_shell(void); + void shell_thread(void); +}; + class PX4::PX4Util : public AP_HAL::Util { public: PX4Util(void); @@ -24,8 +47,14 @@ public: uint16_t available_memory(void); + /* + return a stream for access to nsh shell + */ + AP_HAL::Stream *get_shell_stream() { return &_shell_stream; } + private: int _safety_handle; + PX4::NSHShellStream _shell_stream; }; #endif // __AP_HAL_PX4_UTIL_H__