mavlink: add a shell using SERIAL_CONTROL MAVLink message

src/modules/mavlink/CMakeLists.txt

@@ -49,6 +49,7 @@ px4_add_module(
 		mavlink_receiver.cpp
 		mavlink_ftp.cpp
 		mavlink_log_handler.cpp
+		mavlink_shell.cpp
 	DEPENDS
 		platforms__common
 	)

src/modules/mavlink/mavlink_main.cpp

@@ -185,6 +185,7 @@ Mavlink::Mavlink() :
 	_parameters_manager(nullptr),
 	_mavlink_ftp(nullptr),
 	_mavlink_log_handler(nullptr),
+	_mavlink_shell(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_radio_id(0),
@@ -1517,6 +1518,35 @@ Mavlink::get_rate_mult()
 	return _rate_mult;
 }
 
+MavlinkShell*
+Mavlink::get_shell()
+{
+	if (!_mavlink_shell) {
+		_mavlink_shell = new MavlinkShell();
+		if (!_mavlink_shell) {
+			PX4_ERR("Failed to allocate a shell");
+		} else {
+			int ret = _mavlink_shell->start();
+			if (ret != 0) {
+				PX4_ERR("Failed to start shell (%i)", ret);
+				delete _mavlink_shell;
+				_mavlink_shell = nullptr;
+			}
+		}
+	}
+
+	return _mavlink_shell;
+}
+
+void
+Mavlink::close_shell()
+{
+	if (_mavlink_shell) {
+		delete _mavlink_shell;
+		_mavlink_shell = nullptr;
+	}
+}
+
 void
 Mavlink::update_rate_mult()
 {
@@ -2078,6 +2108,17 @@ Mavlink::task_main(int argc, char *argv[])
 			_logbuffer.put(&mavlink_log);
 		}
 
+		/* check for shell output */
+		if (_mavlink_shell && _mavlink_shell->available() > 0) {
+			mavlink_serial_control_t msg;
+			msg.baudrate = 0;
+			msg.flags = SERIAL_CONTROL_FLAG_REPLY;
+			msg.timeout = 0;
+			msg.device = SERIAL_CONTROL_DEV_SHELL;
+			msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
+			mavlink_msg_serial_control_send_struct(get_channel(), &msg);
+		}
+
 		/* check for requested subscriptions */
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {

src/modules/mavlink/mavlink_main.h

@@ -69,6 +69,7 @@
 #include "mavlink_parameters.h"
 #include "mavlink_ftp.h"
 #include "mavlink_log_handler.h"
+#include "mavlink_shell.h"
 
 enum Protocol {
 	SERIAL = 0,
@@ -411,7 +412,7 @@ public:
 	bool			accepting_commands() { return true; /* non-trivial side effects ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG));*/ }
 
 	/**
-	 * Wether or not the system should be logging
+	 * Whether or not the system should be logging
 	 */
 	bool			get_logging_enabled() { return _logging_enabled; }
 
@@ -420,6 +421,12 @@ public:
 	int			get_data_rate() { return _datarate; }
 	void			set_data_rate(int rate) { if (rate > 0) _datarate = rate; }
 
+	/** get the Mavlink shell. Create a new one if there isn't one. It is *always* created via MavlinkReceiver thread.
+	 *  Returns nullptr if shell cannot be created */
+	MavlinkShell		*get_shell();
+	/** close the Mavlink shell if it is open */
+	void			close_shell();
+
 protected:
 	Mavlink			*next;
 
@@ -451,6 +458,7 @@ private:
 	MavlinkParametersManager	*_parameters_manager;
 	MavlinkFTP			*_mavlink_ftp;
 	MavlinkLogHandler		*_mavlink_log_handler;
+	MavlinkShell			*_mavlink_shell;
 
 	MAVLINK_MODE 		_mode;
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -263,6 +263,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_battery_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SERIAL_CONTROL:
+		handle_message_serial_control(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -1191,6 +1195,32 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
+{
+	mavlink_serial_control_t serial_control_mavlink;
+	mavlink_msg_serial_control_decode(msg, &serial_control_mavlink);
+
+	// we only support shell commands
+	if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL
+			|| (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) {
+		return;
+	}
+
+	MavlinkShell* shell = _mavlink->get_shell();
+	if (shell) {
+		// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
+		if (serial_control_mavlink.count > 0) {
+			shell->write(serial_control_mavlink.data, serial_control_mavlink.count);
+		}
+
+		// if no response requested, assume the shell is no longer used
+		if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
+			_mavlink->close_shell();
+		}
+	}
+}
+
 switch_pos_t
 MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -32,8 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_orb_listener.h
- * MAVLink 1.0 uORB listener definition
+ * @file mavlink_receiver.h
+ * MAVLink receiver thread
  *
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton@px4.io>
@@ -143,6 +143,7 @@ private:
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
 	void handle_message_battery_status(mavlink_message_t *msg);
+	void handle_message_serial_control(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 

src/modules/mavlink/mavlink_shell.cpp

@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_shell.cpp
+ * A shell to be used via MAVLink
+ *
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include "mavlink_shell.h"
+#include <px4_defines.h>
+
+#include <unistd.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+
+
+#ifdef __PX4_NUTTX
+#include <apps/nsh.h>
+#endif /* __PX4_NUTTX */
+
+
+MavlinkShell::MavlinkShell()
+{
+}
+
+MavlinkShell::~MavlinkShell()
+{
+	//closing the pipes will stop the thread as well
+	if (_to_shell_fd >= 0) {
+		PX4_INFO("Stopping mavlink shell");
+		close(_to_shell_fd);
+	}
+	if (_from_shell_fd >= 0) {
+		close(_from_shell_fd);
+	}
+}
+
+int MavlinkShell::start()
+{
+	//this currently only works for NuttX
+#ifndef __PX4_NUTTX
+	return -1;
+#endif /* __PX4_NUTTX */
+
+
+	PX4_INFO("Starting mavlink shell");
+
+	int p1[2], p2[2];
+
+	/* Create the shell task and redirect its stdin & stdout. If we used pthread, we would redirect
+	 * stdin/out of the calling process as well, so we need px4_task_spawn_cmd. However NuttX only
+	 * keeps (duplicates) the first 3 fd's when creating a new task, all others are not inherited.
+	 * This means we need to temporarily change the first 3 fd's of the current task (or at least
+	 * the first 2 if stdout=stderr).
+	 * And we hope :-) that during the temporary phase, no other thread from the same task writes to
+	 * stdout (as it would end up in the pipe).
+	 */
+
+	if (pipe(p1) != 0) {
+		return -errno;
+	}
+	if (pipe(p2) != 0) {
+		close(p1[0]);
+		close(p1[1]);
+		return -errno;
+	}
+
+	int ret = 0;
+
+	_from_shell_fd  = p1[0];
+	_to_shell_fd = p2[1];
+	_shell_fds[0]  = p2[0];
+	_shell_fds[1] = p1[1];
+
+	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) {
+		fd_backups[i] = dup(i);
+		if (fd_backups[i] == -1) {
+			ret = -errno;
+		}
+	}
+	dup2(_shell_fds[0], 0);
+	dup2(_shell_fds[1], 1);
+
+	if (ret == 0) {
+		_task = px4_task_spawn_cmd("mavlink_shell",
+				SCHED_DEFAULT,
+				SCHED_PRIORITY_DEFAULT,
+				2048,
+				&MavlinkShell::shell_start_thread,
+				nullptr);
+		if (_task < 0) {
+			ret = -1;
+		}
+	}
+
+	//restore fd's
+	for (int i = 0; i < 2; ++i) {
+		if (dup2(fd_backups[i], i) == -1) {
+			ret = -errno;
+		}
+		close(fd_backups[i]);
+	}
+
+	//close unused pipe fd's
+	close(_shell_fds[0]);
+	close(_shell_fds[1]);
+
+	return ret;
+}
+
+int MavlinkShell::shell_start_thread(int argc, char *argv[])
+{
+	dup2(1, 2); //redirect stderror to stdout
+
+#ifdef __PX4_NUTTX
+	nsh_consolemain(0, NULL);
+#endif /* __PX4_NUTTX */
+
+	return 0;
+}
+
+size_t MavlinkShell::write(uint8_t* buffer, size_t len)
+{
+	return ::write(_to_shell_fd, buffer, len);
+}
+
+size_t MavlinkShell::read(uint8_t* buffer, size_t len)
+{
+	return ::read(_from_shell_fd, buffer, len);
+}
+
+size_t MavlinkShell::available()
+{
+	int ret = 0;
+	if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) {
+		return ret;
+	}
+	return 0;
+}

src/modules/mavlink/mavlink_shell.h

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_shell.h
+ * A shell to be used via MAVLink
+ *
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+
+#include <stddef.h>
+#include <stdint.h>
+#include <px4_tasks.h>
+
+#pragma once
+
+class MavlinkShell
+{
+public:
+	MavlinkShell();
+
+	~MavlinkShell();
+
+	/**
+	 * Start the mavlink shell.
+	 *
+	 * @return		0 on success.
+	 */
+	int		start();
+
+
+	/**
+	 * Write to the shell
+	 * @return number of written bytes
+	 */
+	size_t write(uint8_t* buffer, size_t len);
+
+	/**
+	 * Read from the shell. This is blocking, if 0 bytes are available, this will block.
+	 * @param len buffer length
+	 * @return number of bytes read.
+	 */
+	size_t read(uint8_t* buffer, size_t len);
+
+	/**
+	 * Get the number of bytes that can be read.
+	 */
+	size_t available();
+
+private:
+
+	int _to_shell_fd = -1; /** fd to write to the shell */
+	int _from_shell_fd = -1; /** fd to read from the shell */
+	int _shell_fds[2] = {-1, -1}; /** stdin & out used by the shell */
+	px4_task_t _task;
+
+	static int shell_start_thread(int argc, char *argv[]);
+
+	/* do not allow copying this class */
+	MavlinkShell(const MavlinkShell &) = delete;
+	MavlinkShell operator=(const MavlinkShell &) = delete;
+};