From 83133b1bcae2eae59383dad7d1b6070657e9d279 Mon Sep 17 00:00:00 2001
From: David Riseborough <drisebor@hotmail.com>
Date: Fri, 2 Feb 2018 19:59:03 +1100
Subject: [PATCH] This commit fixes the way baud rate is generated from the
 program argument in the FTPS client and agent.

A table has been added to the FTPS client and agent code that correlates
the baud rate value with the encoding.

A function has been added to the FTPS client and agent to take the program
argument for baud rate and use it to look up the table and return the entry
containing both the value and the encoding.

The value is displayed for the user and the encoding is sent to the uart
node constructor.

Signed-off-by: David Riseborough <drisebor@hotmail.com>
---
 .../urtps/microRTPS_agent.cpp.template        | 37 ++++++++++++++++---
 .../micrortps_client/microRTPS_client.h       | 10 ++++-
 .../microRTPS_client_main.cpp                 | 30 +++++++++++++--
 3 files changed, 67 insertions(+), 10 deletions(-)

diff --git a/msg/templates/urtps/microRTPS_agent.cpp.template b/msg/templates/urtps/microRTPS_agent.cpp.template
index af3fd609f3..9c6e5162e9 100644
--- a/msg/templates/urtps/microRTPS_agent.cpp.template
+++ b/msg/templates/urtps/microRTPS_agent.cpp.template
@@ -58,6 +58,7 @@ recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgSc
 #include <chrono>
 #include <ctime>
 #include <csignal>
+#include <termios.h>
 
 #include <fastcdr/Cdr.h>
 #include <fastcdr/FastCdr.h>
@@ -73,7 +74,8 @@ recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgSc
 // Default values
 #define DEVICE "/dev/ttyACM0"
 #define SLEEP_US 1
-#define BAUDRATE 460800
+#define BAUDRATE B460800
+#define BAUDRATE_VAL 460800
 #define POLL_MS 0
 #define WAIT_CNST 2
 #define DEFAULT_RECV_PORT 2020
@@ -87,6 +89,22 @@ Transport_node *transport_node = nullptr;
 RtpsTopics topics;
 uint32_t total_sent = 0, sent = 0;
 
+struct baudtype {
+    speed_t code;
+    uint32_t val;
+};
+
+const baudtype baudlist[] = {
+    [0] = {.code = B0, .val = 0},
+    [1] = {.code = B9600, .val = 9600},
+    [2] = {.code = B19200, .val = 19200},
+    [3] = {.code = B38400, .val = 38400},
+    [4] = {.code = B57600, .val = 57600},
+    [5] = {.code = B115200, .val = 115200},
+    [6] = {.code = B230400, .val = 230400},
+    [7] = {.code = B460800, .val = 460800},
+};
+
 struct options {
     enum class eTransports
     {
@@ -96,7 +114,7 @@ struct options {
     eTransports transport = options::eTransports::UART;
     char device[64] = DEVICE;
     int sleep_us = SLEEP_US;
-    uint32_t baudrate = BAUDRATE;
+    baudtype baudrate = {.code=BAUDRATE,.val=BAUDRATE_VAL};
     int poll_ms = POLL_MS;
     uint16_t recv_port = DEFAULT_RECV_PORT;
     uint16_t send_port = DEFAULT_SEND_PORT;
@@ -115,6 +133,15 @@ static void usage(const char *name)
              name);
 }
 
+baudtype getbaudrate(char *valstr)
+{
+    uint32_t baudval = strtoul(valstr, nullptr, 10);
+    for (unsigned int i=1; i<sizeof(baudlist)/sizeof(baudtype); i++) {
+        if (baudlist[i].val==baudval) return baudlist[i];
+    }
+    return baudlist[0];
+}
+
 static int parse_options(int argc, char **argv)
 {
     int ch;
@@ -128,7 +155,7 @@ static int parse_options(int argc, char **argv)
                                                 :options::eTransports::UART;  break;
             case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break;
             case 'w': _options.sleep_us       = strtol(optarg, nullptr, 10);  break;
-            case 'b': _options.baudrate       = strtoul(optarg, nullptr, 10); break;
+            case 'b': _options.baudrate       = getbaudrate(optarg);  break;
             case 'p': _options.poll_ms        = strtol(optarg, nullptr, 10);  break;
             case 'r': _options.recv_port      = strtoul(optarg, nullptr, 10); break;
             case 's': _options.send_port      = strtoul(optarg, nullptr, 10); break;
@@ -199,9 +226,9 @@ int main(int argc, char** argv)
     {
         case options::eTransports::UART:
         {
-            transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
+            transport_node = new UART_node(_options.device, _options.baudrate.code, _options.poll_ms);
             printf("\nUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n\n",
-                   _options.device, _options.baudrate, _options.sleep_us, _options.poll_ms);
+                   _options.device, _options.baudrate.val, _options.sleep_us, _options.poll_ms);
         }
         break;
         case options::eTransports::UDP:
diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
index 1485975db6..221398e787 100644
--- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
+++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
@@ -52,7 +52,8 @@
 #define UPDATE_TIME_MS 0
 #define LOOPS -1
 #define SLEEP_MS 1
-#define BAUDRATE 460800
+#define BAUDRATE B460800
+#define BAUDRATE_VAL 460800
 #define DEVICE "/dev/ttyACM0"
 #define POLL_MS 1
 #define DEFAULT_RECV_PORT 2019
@@ -61,6 +62,11 @@
 void *send(void *data);
 void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop);
 
+struct baudtype {
+	speed_t code;
+	uint32_t val;
+};
+
 struct options {
 	enum class eTransports {
 		UART,
@@ -71,7 +77,7 @@ struct options {
 	int update_time_ms = UPDATE_TIME_MS;
 	int loops = LOOPS;
 	int sleep_ms = SLEEP_MS;
-	uint32_t baudrate = BAUDRATE;
+	struct baudtype baudrate = {.code = BAUDRATE, .val = BAUDRATE_VAL};
 	int poll_ms = POLL_MS;
 	uint16_t recv_port = DEFAULT_RECV_PORT;
 	uint16_t send_port = DEFAULT_SEND_PORT;
diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
index f760576abd..086a9f008a 100644
--- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
+++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
@@ -53,6 +53,19 @@ bool _should_exit_task = false;
 Transport_node *transport_node = nullptr;
 struct options _options;
 
+const baudtype baudlist[] = {
+	[0] = {.code = B0, .val = 0},
+	[1] = {.code = B9600, .val = 9600},
+	[2] = {.code = B19200, .val = 19200},
+	[3] = {.code = B38400, .val = 38400},
+	[4] = {.code = B57600, .val = 57600},
+	[5] = {.code = B115200, .val = 115200},
+	[6] = {.code = B230400, .val = 230400},
+	[7] = {.code = B460800, .val = 460800}
+};
+
+baudtype getbaudrate(const char *valstr);
+
 static void usage(const char *name)
 {
 	PRINT_MODULE_USAGE_NAME("micrortps_client", "communication");
@@ -74,6 +87,17 @@ static void usage(const char *name)
 	PRINT_MODULE_USAGE_COMMAND("status");
 }
 
+baudtype getbaudrate(const char *valstr)
+{
+	uint32_t baudval = strtoul(valstr, nullptr, 10);
+
+	for (unsigned int i = 1; i < sizeof(baudlist) / sizeof(baudtype); i++) {
+		if (baudlist[i].val == baudval) { return baudlist[i]; }
+	}
+
+	return baudlist[0];
+}
+
 static int parse_options(int argc, char *argv[])
 {
 	int ch;
@@ -94,7 +118,7 @@ static int parse_options(int argc, char *argv[])
 
 		case 'w': _options.sleep_ms       = strtol(myoptarg, nullptr, 10);    break;
 
-		case 'b': _options.baudrate       = strtoul(myoptarg, nullptr, 10);     break;
+		case 'b': _options.baudrate       = getbaudrate(myoptarg); break;
 
 		case 'p': _options.poll_ms        = strtol(myoptarg, nullptr, 10);      break;
 
@@ -131,9 +155,9 @@ static int micrortps_start(int argc, char *argv[])
 
 	switch (_options.transport) {
 	case options::eTransports::UART: {
-			transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
+			transport_node = new UART_node(_options.device, _options.baudrate.code, _options.poll_ms);
 			printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n",
-			       _options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms);
+			       _options.device, _options.baudrate.val, _options.sleep_ms, _options.poll_ms);
 		}
 		break;