Merge pull request #727 from PX4/beta_mavlink2_flow_control

UART flow control

src/modules/mavlink/mavlink_main.cpp

@@ -104,55 +104,90 @@ static struct file_operations fops;
  */
 extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
+static uint64_t last_write_times[6] = {0};
+
 /*
  * Internal function to send the bytes through the right serial port
  */
 void
 mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
 {
-	int uart = -1;
+	Mavlink *instance;
 
 	switch (channel) {
 	case MAVLINK_COMM_0:
-		uart = Mavlink::get_uart_fd(0);
+		instance = Mavlink::get_instance(0);
 		break;
 
 	case MAVLINK_COMM_1:
-		uart = Mavlink::get_uart_fd(1);
+		instance = Mavlink::get_instance(1);
 		break;
 
 	case MAVLINK_COMM_2:
-		uart = Mavlink::get_uart_fd(2);
+		instance = Mavlink::get_instance(2);
 		break;
 
 	case MAVLINK_COMM_3:
-		uart = Mavlink::get_uart_fd(3);
+		instance = Mavlink::get_instance(3);
 		break;
 #ifdef MAVLINK_COMM_4
 
 	case MAVLINK_COMM_4:
-		uart = Mavlink::get_uart_fd(4);
+		instance = Mavlink::get_instance(4);
 		break;
 #endif
 #ifdef MAVLINK_COMM_5
 
 	case MAVLINK_COMM_5:
-		uart = Mavlink::get_uart_fd(5);
+		instance = Mavlink::get_instance(5);
 		break;
 #endif
 #ifdef MAVLINK_COMM_6
 
 	case MAVLINK_COMM_6:
-		uart = Mavlink::get_uart_fd(6);
+		instance = Mavlink::get_instance(6);
 		break;
 #endif
 	}
 
+	/* no valid instance, bail */
+	if (!instance)
+		return;
+
+	int uart = instance->get_uart_fd();
+
 	ssize_t desired = (sizeof(uint8_t) * length);
+
+	/*
+	 * Check if the OS buffer is full and disable HW
+	 * flow control if it continues to be full
+	 */
+	int buf_free = 0;
+
+	if (instance->get_flow_control_enabled()
+		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+
+		if (buf_free == 0) {
+
+			if (last_write_times[(unsigned)channel] != 0 &&
+			hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) {
+
+				warnx("DISABLING HARDWARE FLOW CONTROL");
+				instance->enable_flow_control(false);
+			}
+
+		} else {
+
+			/* apparently there is space left, although we might be
+			 * partially overflooding the buffer already */
+			last_write_times[(unsigned)channel] = hrt_absolute_time();
+		}
+	}
+
 	ssize_t ret = write(uart, ch, desired);
 
 	if (ret != desired) {
-		warn("write err");
+		// XXX do something here, but change to using FIONWRITE and OS buf size for detection
 	}
 
 }
@@ -173,6 +208,7 @@ Mavlink::Mavlink() :
 	_mavlink_param_queue_index(0),
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
+	_flow_control_enabled(true),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
@@ -512,7 +548,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
+		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
 		close(_uart_fd);
 		return -1;
 	}
@@ -528,7 +564,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 		/* Set baud rate */
 		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
 			close(_uart_fd);
 			return -1;
 		}
@@ -536,14 +572,46 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	}
 
 	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR SET CONF %s\n", uart_name);
 		close(_uart_fd);
 		return -1;
 	}
 
+	/*
+	 * 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.
+	 */
+	(void)tcgetattr(_uart_fd, &uart_config);
+	uart_config.c_cflag |= CRTS_IFLOW;
+	(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+	/* setup output flow control */
+	if (enable_flow_control(true)) {
+		warnx("ERR FLOW CTRL EN");
+	}
+
 	return _uart_fd;
 }
 
+int
+Mavlink::enable_flow_control(bool enabled)
+{
+	struct termios uart_config;
+	int ret = tcgetattr(_uart_fd, &uart_config);
+	if (enabled) {
+		uart_config.c_cflag |= CRTSCTS;
+	} else {
+		uart_config.c_cflag &= ~CRTSCTS;
+	}
+	ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
+
+	if (!ret) {
+		_flow_control_enabled = enabled;
+	}
+
+	return ret;
+}
+
 int
 Mavlink::set_hil_enabled(bool hil_enabled)
 {
@@ -1554,10 +1622,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 int
 Mavlink::task_main(int argc, char *argv[])
 {
-	/* inform about start */
-	warnx("start");
-	fflush(stdout);
-
 	int ch;
 	_baudrate = 57600;
 	_datarate = 0;
@@ -1686,8 +1750,7 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	warnx("data rate: %d bytes/s", _datarate);
-	warnx("port: %s, baudrate: %d", _device_name, _baudrate);
+	warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1732,9 +1795,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
 
-	warnx("started");
-	mavlink_log_info(_mavlink_fd, "[mavlink] started");
-
 	/* add default streams depending on mode and intervals depending on datarate */
 	float rate_mult = _datarate / 1000.0f;
 

src/modules/mavlink/mavlink_main.h

@@ -156,6 +156,8 @@ public:
 
 	bool		get_hil_enabled() { return _mavlink_hil_enabled; };
 
+	bool		get_flow_control_enabled() { return _flow_control_enabled; }
+
 	/**
 	 * Handle waypoint related messages.
 	 */
@@ -184,6 +186,13 @@ public:
 
 	int get_instance_id();
 
+	/**
+	 * Enable / disable hardware flow control.
+	 *
+	 * @param enabled	True if hardware flow control should be enabled
+	 */
+	int enable_flow_control(bool enabled);
+
 	mavlink_channel_t get_channel();
 
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
@@ -241,6 +250,8 @@ private:
 	char 	*_subscribe_to_stream;
 	float	_subscribe_to_stream_rate;
 
+	bool		_flow_control_enabled;
+
 	/**
 	 * Send one parameter.
 	 *