diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index bc573e500e..023fddf476 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/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 ) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed0713421b..3818ddf818 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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)) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b2f616a98a..a4c695cfa3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 81f123ef03..d0a97ab1bd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8d3b3d0396..b2521a6813 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/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 * @author Anton Babushkin @@ -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); diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp new file mode 100644 index 0000000000..ea509151ef --- /dev/null +++ b/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 + */ + +#include "mavlink_shell.h" +#include + +#include +#include +#include + + +#ifdef __PX4_NUTTX +#include +#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; +} diff --git a/src/modules/mavlink/mavlink_shell.h b/src/modules/mavlink/mavlink_shell.h new file mode 100644 index 0000000000..f4a788a023 --- /dev/null +++ b/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 + */ + + +#include +#include +#include + +#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; +};