Add 'forced on' flow control mode to mavlink UART (#8776)

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,38 +808,33 @@ 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;
 	}
 
 	struct termios uart_config;
 
 	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 @@ Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz:
 	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);
 

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 @@ class Mavlink
 		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 @@ class Mavlink
 
 	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 @@ class Mavlink
 	 *
 	 * @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 @@ class Mavlink
 	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 @@ class Mavlink
 	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);