diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 87276e968d..18c5646ec0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2018 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 @@ -228,7 +228,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _udp_initialised(false), - _flow_control_enabled(false), + _flow_control_mode(Mavlink::FLOW_CONTROL_OFF), _last_write_success_time(0), _last_write_try_time(0), _mavlink_start_time(0), @@ -634,7 +634,7 @@ int Mavlink::get_component_id() return mavlink_system.compid; } -int Mavlink::mavlink_open_uart(int baud, const char *uart_name) +int Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_control) { #ifndef B460800 #define B460800 460800 @@ -808,30 +808,25 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name) return -1; } - if (!_is_usb_uart) { - /* - * Setup hardware flow control. If the port has no RTS pin this call will fail, - * which is not an issue, but requires a separate call so we can fail silently. - */ - - /* setup output flow control */ - if (enable_flow_control(true)) { - PX4_WARN("hardware flow control not supported"); - } + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ - } else { - _flow_control_enabled = false; + /* setup output flow control */ + if (enable_flow_control(force_flow_control ? FLOW_CONTROL_ON : FLOW_CONTROL_AUTO)) { + PX4_WARN("hardware flow control not supported"); } return _uart_fd; } int -Mavlink::enable_flow_control(bool enabled) +Mavlink::enable_flow_control(enum FLOW_CONTROL_MODE mode) { // We can't do this on USB - skip if (_is_usb_uart) { - _flow_control_enabled = false; + _flow_control_mode = FLOW_CONTROL_OFF; return OK; } @@ -839,7 +834,7 @@ Mavlink::enable_flow_control(bool enabled) int ret = tcgetattr(_uart_fd, &uart_config); - if (enabled) { + if (mode) { uart_config.c_cflag |= CRTSCTS; } else { @@ -850,7 +845,7 @@ Mavlink::enable_flow_control(bool enabled) ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); if (!ret) { - _flow_control_enabled = enabled; + _flow_control_mode = mode; } return ret; @@ -901,15 +896,15 @@ Mavlink::get_free_tx_buf() (void) ioctl(_uart_fd, FIONSPACE, (unsigned long)&buf_free); #endif - if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { - /* Disable hardware flow control: + if (_flow_control_mode == FLOW_CONTROL_AUTO && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { + /* Disable hardware flow control in FLOW_CONTROL_AUTO mode: * if no successful write since a defined time * and if the last try was not the last successful write */ if (_last_write_try_time != 0 && hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && _last_write_success_time != _last_write_try_time) { - enable_flow_control(false); + enable_flow_control(FLOW_CONTROL_OFF); } } } @@ -1737,6 +1732,7 @@ Mavlink::task_main(int argc, char *argv[]) _baudrate = 57600; _datarate = 0; _mode = MAVLINK_MODE_NORMAL; + bool _force_flow_control = false; #ifdef __PX4_NUTTX /* the NuttX optarg handler does not @@ -1759,7 +1755,7 @@ Mavlink::task_main(int argc, char *argv[]) int temp_int_arg; #endif - while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:t:fwx", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:t:fwxz", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(myoptarg, nullptr, 10); @@ -1884,6 +1880,10 @@ Mavlink::task_main(int argc, char *argv[]) _ftp_on = true; break; + case 'z': + _force_flow_control = true; + break; + default: err_flag = true; break; @@ -1917,7 +1917,7 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* default values for arguments */ - _uart_fd = mavlink_open_uart(_baudrate, _device_name); + _uart_fd = mavlink_open_uart(_baudrate, _device_name, _force_flow_control); if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { PX4_ERR("could not open %s", _device_name); @@ -2582,7 +2582,7 @@ Mavlink::display_status() printf("\tno telem status.\n"); } - printf("\tflow control:\t%s\n", (_flow_control_enabled) ? "ON" : "OFF"); + printf("\tflow control:\t%s\n", (_flow_control_mode) ? "ON" : "OFF"); printf("\trates:\n"); printf("\ttx: %.3f kB/s\n", (double)_rate_tx); printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); @@ -2625,7 +2625,6 @@ Mavlink::stream_command(int argc, char *argv[]) int temp_int_arg; bool provided_device = false; bool provided_network_port = false; - /* * Called via main with original argv * mavlink start @@ -2788,6 +2787,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true); PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true); PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true); + PRINT_MODULE_USAGE_PARAM_FLAG('z', "Force flow control always on", true); PRINT_MODULE_USAGE_ARG("on|off", "Enable/disable", true); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 873438644e..6e1ac495c6 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2018 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 @@ -178,6 +178,12 @@ public: BROADCAST_MODE_ON }; + enum FLOW_CONTROL_MODE { + FLOW_CONTROL_OFF = 0, + FLOW_CONTROL_AUTO, + FLOW_CONTROL_ON + }; + static const char *mavlink_mode_str(enum MAVLINK_MODE mode) { switch (mode) { @@ -218,7 +224,7 @@ public: bool get_forward_externalsp() { return _forward_externalsp; } - bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_flow_control_enabled() { return _flow_control_mode; } bool get_forwarding_on() { return _forwarding_on; } @@ -316,7 +322,7 @@ public: * * @param enabled True if hardware flow control should be enabled */ - int enable_flow_control(bool enabled); + int enable_flow_control(enum FLOW_CONTROL_MODE enabled); #endif mavlink_channel_t get_channel(); @@ -542,7 +548,7 @@ private: float _subscribe_to_stream_rate; bool _udp_initialised; - bool _flow_control_enabled; + enum FLOW_CONTROL_MODE _flow_control_mode; uint64_t _last_write_success_time; uint64_t _last_write_try_time; uint64_t _mavlink_start_time; @@ -608,7 +614,7 @@ private: void mavlink_update_system(); #ifndef __PX4_QURT - int mavlink_open_uart(int baudrate, const char *uart_name); + int mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control); #endif static int interval_from_rate(float rate);