From f66b0ad8ac926a79608cf872c8e49ea7ace92288 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:04:30 +0100 Subject: Added hardware flow control to mavlink app. Auto-disables if nothing can be written to the port --- src/modules/mavlink/mavlink_main.cpp | 46 ++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 667580452..ed3b265f9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -104,6 +104,8 @@ 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 */ @@ -149,10 +151,28 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } 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 (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 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"); + } + ssize_t ret = write(uart, ch, desired); if (ret != desired) { warn("write err"); + } else { + last_write_times[(unsigned)channel] = hrt_absolute_time(); } } @@ -541,9 +561,35 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * 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 */ + (void)enable_flow_control(true); + 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); + + return ret; +} + int Mavlink::set_hil_enabled(bool hil_enabled) { -- cgit v1.2.3