From 6bb0046407917326ab067a33af42fdd816079510 Mon Sep 17 00:00:00 2001
From: Gabriel Moreno <gabrielm@cs.cmu.edu>
Date: Thu, 19 Oct 2017 11:06:14 -0400
Subject: [PATCH] urtps: fix UART speed setting to work in Linux

The previous code used the numeric baudrate as the speed argument
for cfsetispeed() and cfsetospeed(). This works in OSX and NuttX,
but is not portable.

This fix introduces a new method to convert baudrate to speed using
constants such as B115200.

Signed-off-by: Gabriel Moreno <gabrielm@cs.cmu.edu>
---
 msg/templates/urtps/microRTPS_transport.cpp | 93 ++++++++++++++++++++-
 msg/templates/urtps/microRTPS_transport.h   |  2 +
 2 files changed, 93 insertions(+), 2 deletions(-)

diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp
index 0b89cbba5e..997a6ae2dc 100644
--- a/msg/templates/urtps/microRTPS_transport.cpp
+++ b/msg/templates/urtps/microRTPS_transport.cpp
@@ -31,7 +31,6 @@
  ****************************************************************************/
 #include <unistd.h>
 #include <fcntl.h>
-#include <termios.h>
 #include <stdio.h>
 #include <errno.h>
 #include <sys/socket.h>
@@ -307,7 +306,16 @@ int UART_node::init()
 	// USB serial is indicated by /dev/ttyACM0
 	if (strcmp(uart_name, "/dev/ttyACM0") != 0 && strcmp(uart_name, "/dev/ttyACM1") != 0) {
 		// Set baud rate
-		if (cfsetispeed(&uart_config, baudrate) < 0 || cfsetospeed(&uart_config, baudrate) < 0) {
+		speed_t speed;
+
+		if (!baudrate_to_speed(baudrate, &speed)) {
+			printf("ERR SET BAUD %s: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n",
+			       uart_name, baudrate);
+			close();
+			return -EINVAL;
+		}
+
+		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
 			int errno_bkp = errno;
 			PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno);
 			close();
@@ -386,6 +394,87 @@ ssize_t UART_node::node_write(void *buffer, size_t len)
 	return ::write(uart_fd, buffer, len);
 }
 
+bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed)
+{
+#ifndef B460800
+#define B460800 460800
+#endif
+
+#ifndef B500000
+#define B500000 500000
+#endif
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
+#ifndef B1000000
+#define B1000000 1000000
+#endif
+
+	switch (bauds) {
+	case 0:      *speed = B0;      break;
+
+	case 50:     *speed = B50;     break;
+
+	case 75:     *speed = B75;     break;
+
+	case 110:    *speed = B110;    break;
+
+	case 134:    *speed = B134;    break;
+
+	case 150:    *speed = B150;    break;
+
+	case 200:    *speed = B200;    break;
+
+	case 300:    *speed = B300;    break;
+
+	case 600:    *speed = B600;    break;
+
+	case 1200:   *speed = B1200;   break;
+
+	case 1800:   *speed = B1800;   break;
+
+	case 2400:   *speed = B2400;   break;
+
+	case 4800:   *speed = B4800;   break;
+
+	case 9600:   *speed = B9600;   break;
+
+	case 19200:  *speed = B19200;  break;
+
+	case 38400:  *speed = B38400;  break;
+
+	case 57600:  *speed = B57600;  break;
+
+	case 115200: *speed = B115200; break;
+
+	case 230400: *speed = B230400; break;
+
+	case 460800: *speed = B460800; break;
+
+	case 500000: *speed = B500000; break;
+
+	case 921600: *speed = B921600; break;
+
+	case 1000000: *speed = B1000000; break;
+
+#ifdef B1500000
+
+	case 1500000: *speed = B1500000; break;
+#endif
+
+#ifdef B3000000
+
+	case 3000000: *speed = B3000000; break;
+#endif
+
+	default:
+		return false;
+	}
+
+	return true;
+}
 
 UDP_node::UDP_node(uint16_t _udp_port_recv, uint16_t _udp_port_send):
 	sender_fd(-1),
diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h
index 03d47670f9..121736f79e 100644
--- a/msg/templates/urtps/microRTPS_transport.h
+++ b/msg/templates/urtps/microRTPS_transport.h
@@ -35,6 +35,7 @@
 #include <cstring>
 #include <arpa/inet.h>
 #include <poll.h>
+#include <termios.h>
 
 class Transport_node
 {
@@ -99,6 +100,7 @@ protected:
 	ssize_t node_read(void *buffer, size_t len);
 	ssize_t node_write(void *buffer, size_t len);
 	bool fds_OK();
+	bool baudrate_to_speed(uint32_t bauds, speed_t *speed);
 
 	int uart_fd;
 	char uart_name[64] = {};