From 9accf81299c0c2fbde51aa086a59602c02cb2b15 Mon Sep 17 00:00:00 2001
From: Eric Katzfey <eric.katzfey@modalai.com>
Date: Wed, 19 Jul 2023 17:21:59 -0700
Subject: [PATCH] Changed the default baudrate handling around a little to make
 it cleaner

---
 platforms/common/Serial.cpp           | 12 ++++------
 platforms/qurt/src/px4/SerialImpl.cpp |  4 ----
 src/drivers/gps/gps.cpp               | 33 ++++++++++++++++++++-------
 3 files changed, 29 insertions(+), 20 deletions(-)

diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp
index d565e31177..bee87dbf51 100644
--- a/platforms/common/Serial.cpp
+++ b/platforms/common/Serial.cpp
@@ -7,14 +7,10 @@ Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity pa
 	       FlowControl flowcontrol) :
 	_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
 {
-
-
-	// TODO: Device
-
-	// set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
-
-	// char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
-	// set_device_bus(c - 48); // sub 48 to convert char to integer
+	// If no baudrate was specified then set it to a reasonable default value
+	if (baudrate == 0) {
+		(void) _impl.setBaudrate(9600);
+	}
 }
 
 Serial::~Serial()
diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp
index a231d241ac..24eb5f4e8b 100644
--- a/platforms/qurt/src/px4/SerialImpl.cpp
+++ b/platforms/qurt/src/px4/SerialImpl.cpp
@@ -23,10 +23,6 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P
 	} else {
 		_port[0] = 0;
 	}
-
-	// Start off with a valid bitrate to make sure open can succeed
-	if (_baudrate == 0) { _baudrate = 9600; }
-
 }
 
 SerialImpl::~SerialImpl()
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 19ab352ca2..645b1d7737 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -472,12 +472,12 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 	int ret = 0;
 	const unsigned character_count = 32; // minimum bytes that we want to read
 	const int max_timeout = 50;
-	int _timeout = math::min(max_timeout, timeout);
+	int timeout_adjusted = math::min(max_timeout, timeout);
 
 	handleInjectDataTopic();
 
 	if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-		ret = _uart->readAtLeast(buf, buf_length, character_count, _timeout);
+		ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted);
 
 // SPI is only supported on LInux
 #if defined(__PX4_LINUX)
@@ -495,7 +495,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 		fds[0].fd = _spi_fd;
 		fds[0].events = POLLIN;
 
-		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), _timeout);
+		ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted);
 
 		if (ret > 0) {
 			/* if we have new data from GPS, go handle it */
@@ -614,8 +614,8 @@ bool GPS::injectData(uint8_t *data, size_t len)
 int GPS::setBaudrate(unsigned baud)
 {
 	if (_interface == GPSHelper::Interface::UART) {
-		if (_uart) {
-			return _uart->setBaudrate(baud);
+		if ((_uart) && (_uart->setBaudrate(baud))) {
+			return 0;
 		}
 
 #ifdef __PX4_LINUX
@@ -786,7 +786,9 @@ GPS::run()
 		}
 
 		if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) {
-			_uart = new Serial(_port, _configured_baudrate);
+
+			// Create the UART port instance
+			_uart = new Serial(_port);
 
 			if (_uart == nullptr) {
 				PX4_ERR("Error creating serial device %s", _port);
@@ -794,7 +796,22 @@ GPS::run()
 				continue;
 			}
 
-			_uart->open();
+			// Configure the desired baudrate if one was specified by the user.
+			// Otherwise the default baudrate will be used.
+			if (_configured_baudrate) {
+				if (! _uart->setBaudrate(_configured_baudrate)) {
+					PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port);
+					px4_sleep(1);
+					continue;
+				}
+			}
+
+			// Open the UART. If this is successful then the UART is ready to use.
+			if (! _uart->open()) {
+				PX4_ERR("Error opening serial device  %s", _port);
+				px4_sleep(1);
+				continue;
+			}
 
 #ifdef __PX4_LINUX
 
@@ -1010,7 +1027,7 @@ GPS::run()
 		}
 
 		if ((_interface == GPSHelper::Interface::UART) && (_uart)) {
-			_uart->close();
+			(void) _uart->close();
 			delete _uart;
 
 #ifdef __PX4_LINUX