diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h
index 8305edb311..0a70f761e9 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.h
+++ b/libraries/AP_HAL_PX4/UARTDriver.h
@@ -44,6 +44,10 @@ public:
 
     void _timer_tick(void);
 
+    int _get_fd(void) {
+	    return _fd;
+    }
+
 private:
     const char *_devpath;
     int _fd;
diff --git a/libraries/AP_HAL_PX4/Util.cpp b/libraries/AP_HAL_PX4/Util.cpp
index f3ce46690c..c3c4083888 100644
--- a/libraries/AP_HAL_PX4/Util.cpp
+++ b/libraries/AP_HAL_PX4/Util.cpp
@@ -3,6 +3,14 @@
 #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 "UARTDriver.h"
+
+extern const AP_HAL::HAL& hal;
 
 static int libc_vsnprintf(char* str, size_t size, const char *format, va_list ap) 
 {
@@ -42,4 +50,43 @@ int PX4Util::vsnprintf_P(char* str, size_t size, const prog_char_t *format,
     return libc_vsnprintf(str, size, format, ap);
 }
 
+extern bool _px4_thread_should_exit;
+
+/*
+  start an instance of nsh
+ */
+bool PX4Util::run_debug_shell(AP_HAL::BetterStream *stream)
+{
+	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;
+
+	// 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, false);
+
+	// this shouldn't happen
+	hal.console->printf("shell exited\n");
+	return true;
+}
+
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_HAL_PX4/Util.h b/libraries/AP_HAL_PX4/Util.h
index a9ba932425..27365a674c 100644
--- a/libraries/AP_HAL_PX4/Util.h
+++ b/libraries/AP_HAL_PX4/Util.h
@@ -12,6 +12,7 @@ public:
     int vsnprintf(char* str, size_t size, const char *format, va_list ap);
     int vsnprintf_P(char* str, size_t size, const prog_char_t *format,
             va_list ap);
+    bool run_debug_shell(AP_HAL::BetterStream *stream);
 };
 
 #endif // __AP_HAL_PX4_UTIL_H__