HAL_PX4: added implementation of NSH shell stream

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__

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 <AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include <stdio.h>
+#include <stdarg.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <apps/nsh.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#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, &param);
+    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

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
 

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;
 }
 
 /*

libraries/AP_HAL_PX4/Util.h

@@ -5,6 +5,29 @@
 #include <AP_HAL.h>
 #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__