Fixed HW flow control enable / disable scheme, console output cleanup on start

1 files changed, 23 insertions, 22 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 7e1af8824..4420e04fd 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -160,23 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 	if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
 
-		if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 &&
+		if (buf_free == 0) {
+
+			if (last_write_times[(unsigned)channel] != 0 &&
 			hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) {
 
-			struct termios uart_config;
-			(void)tcgetattr(uart, &uart_config);
-			uart_config.c_cflag &= ~CRTSCTS;
-			(void)tcsetattr(uart, TCSANOW, &uart_config);
-			warnx("DISABLING HARDWARE FLOW CONTROL");
+				struct termios uart_config;
+				(void)tcgetattr(uart, &uart_config);
+				uart_config.c_cflag &= ~CRTSCTS;
+				(void)tcsetattr(uart, TCSANOW, &uart_config);
+				warnx("DISABLING HARDWARE FLOW CONTROL");
+			}
+
+		} 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");
-	} else {
-		last_write_times[(unsigned)channel] = hrt_absolute_time();
+		// XXX do something here, but change to using FIONWRITE and OS buf size for detection
 	}
 
 }
@@ -536,7 +543,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;
 	}
@@ -552,7 +559,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;
 		}
@@ -560,7 +567,7 @@ 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;
 	}
@@ -574,7 +581,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	(void)tcsetattr(_uart_fd, TCSANOW, &uart_config);
 
 	/* setup output flow control */
-	(void)enable_flow_control(true);
+	if (enable_flow_control(true)) {
+		warnx("ERR FLOW CTRL EN");
+	}
 
 	return _uart_fd;
 }
@@ -1604,10 +1613,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;
@@ -1736,8 +1741,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);
@@ -1782,9 +1786,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;