From 1ee3484827a05188fd415f308adcf7abd121b8f5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 4 Jul 2021 14:15:11 -0400
Subject: [PATCH] drivers/gps: close port if configure fails

 - open if necessary at beginning of each iteration
---
 src/drivers/gps/gps.cpp | 69 +++++++++++++++++++++--------------------
 1 file changed, 35 insertions(+), 34 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 3b86b6dc23..7b09218046 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -666,35 +666,6 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
 void
 GPS::run()
 {
-	/* open the serial port */
-	_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
-
-	if (_serial_fd < 0) {
-		PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
-		return;
-	}
-
-#ifdef __PX4_LINUX
-
-	if (_interface == GPSHelper::Interface::SPI) {
-		int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
-		int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
-
-		if (status_value < 0) {
-			PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-			return;
-		}
-
-		status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
-
-		if (status_value < 0) {
-			PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
-			return;
-		}
-	}
-
-#endif /* __PX4_LINUX */
-
 	param_t handle = param_find("GPS_YAW_OFFSET");
 	float heading_offset = 0.f;
 
@@ -751,6 +722,36 @@ GPS::run()
 			_helper = nullptr;
 		}
 
+		if (_serial_fd < 0) {
+			/* open the serial port */
+			_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+			if (_serial_fd < 0) {
+				PX4_ERR("failed to open %s err: %d", _port, errno);
+				px4_sleep(1);
+				continue;
+			}
+
+#ifdef __PX4_LINUX
+
+			if (_interface == GPSHelper::Interface::SPI) {
+				int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
+				int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}
+
+				status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
+
+				if (status_value < 0) {
+					PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
+				}
+			}
+
+#endif /* __PX4_LINUX */
+		}
+
 		switch (_mode) {
 		case GPS_DRIVER_MODE_NONE:
 			_mode = GPS_DRIVER_MODE_UBX;
@@ -911,6 +912,11 @@ GPS::run()
 			}
 		}
 
+		if (_serial_fd >= 0) {
+			::close(_serial_fd);
+			_serial_fd = -1;
+		}
+
 		if (_mode_auto) {
 			switch (_mode) {
 			case GPS_DRIVER_MODE_UBX:
@@ -946,11 +952,6 @@ GPS::run()
 	}
 
 	PX4_INFO("exiting");
-
-	if (_serial_fd >= 0) {
-		::close(_serial_fd);
-		_serial_fd = -1;
-	}
 }
 
 int