From ae123d9e20f2038710cc514fdcab68f187a8dd99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 Apr 2016 09:15:06 +0200 Subject: [PATCH] MAVLink app: Do not accept config commands on wireless links when USB is connected --- src/modules/mavlink/mavlink_main.cpp | 29 ++++++++++++++++-------- src/modules/mavlink/mavlink_main.h | 7 ++++++ src/modules/mavlink/mavlink_receiver.cpp | 22 ++++++++++++++---- 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 166249ce12..6653a88660 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -121,6 +121,7 @@ extern mavlink_system_t mavlink_system; static void usage(void); bool Mavlink::_boot_complete = false; +bool Mavlink::_config_link_on = false; Mavlink::Mavlink() : _device_name("/dev/ttyS1"), @@ -654,7 +655,6 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return _uart_fd; } - /* Try to set baud rate */ struct termios uart_config; int termios_state; @@ -1137,6 +1137,10 @@ Mavlink::init_udp() void Mavlink::handle_message(const mavlink_message_t *msg) { + if (!accepting_commands()) { + return; + } + /* handle packet with mission manager */ _mission_manager->handle_message(msg); @@ -1931,6 +1935,11 @@ Mavlink::task_main(int argc, char *argv[]) _main_loop_delay = 2000; } + /* hard limit to 100 Hz at least */ + if (_main_loop_delay > 10000) { + _main_loop_delay = 10000; + } + /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -2274,6 +2283,13 @@ Mavlink::display_status() switch (_rstatus.type) { case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); break; case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB: @@ -2285,24 +2301,17 @@ Mavlink::display_status() break; } - printf("\trssi:\t\t%d\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); - printf("\tnoise:\t\t%d\n", _rstatus.noise); - printf("\tremote noise:\t%u\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\n", _rstatus.rxerrors); - printf("\tfixed:\t\t%u\n", _rstatus.fixed); - printf("\tflow control:\t%s\n", (_flow_control_enabled) ? "ON" : "OFF"); - } else { printf("\tno telem status.\n"); } + printf("\tflow control:\t%s\n", (_flow_control_enabled) ? "ON" : "OFF"); printf("\trates:\n"); printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); printf("\trx: %.3f kB/s\n", (double)_rate_rx); printf("\trate mult: %.3f\n", (double)_rate_mult); + printf("\taccepting commands: %s\n", (accepting_commands()) ? "YES" : "NO"); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6d621f4d88..4be6000c75 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -164,6 +164,10 @@ public: bool get_forwarding_on() { return _forwarding_on; } + bool get_config_link_on() { return _config_link_on; } + + void set_config_link_on(bool on) { _config_link_on = on; } + /** * Set the boot complete flag on all instances * @@ -353,6 +357,8 @@ public: bool is_usb_uart() { return _is_usb_uart; } + bool accepting_commands() { return ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG)); } + /** * Wether or not the system should be logging */ @@ -475,6 +481,7 @@ private: param_t _param_forward_externalsp; unsigned _system_type; + static bool _config_link_on; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9928b73af6..48a08c312f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -155,13 +155,23 @@ MavlinkReceiver::~MavlinkReceiver() void MavlinkReceiver::handle_message(mavlink_message_t *msg) { + if (!_mavlink->get_config_link_on()) { + if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_CONFIG) { + _mavlink->set_config_link_on(true); + } + } + switch (msg->msgid) { case MAVLINK_MSG_ID_COMMAND_LONG: - handle_message_command_long(msg); + if (_mavlink->accepting_commands()) { + handle_message_command_long(msg); + } break; case MAVLINK_MSG_ID_COMMAND_INT: - handle_message_command_int(msg); + if (_mavlink->accepting_commands()) { + handle_message_command_int(msg); + } break; case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: @@ -173,7 +183,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_SET_MODE: - handle_message_set_mode(msg); + if (_mavlink->accepting_commands()) { + handle_message_set_mode(msg); + } break; case MAVLINK_MSG_ID_ATT_POS_MOCAP: @@ -213,7 +225,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - handle_message_request_data_stream(msg); + if (_mavlink->accepting_commands()) { + handle_message_request_data_stream(msg); + } break; case MAVLINK_MSG_ID_SYSTEM_TIME: