From 7a933483405962291eb7706c7de169ce50223955 Mon Sep 17 00:00:00 2001
From: Simon Laube <simon@leitwert.ch>
Date: Tue, 30 Jun 2015 18:28:19 +0200
Subject: [PATCH] implemented retrying the connection to the px4flow sensor
 before giving up.

---
 src/drivers/px4flow/px4flow.cpp | 133 ++++++++++++++++++--------------
 1 file changed, 76 insertions(+), 57 deletions(-)

diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index ba438e276e..8d0af42524 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -636,7 +636,11 @@ namespace px4flow
 #endif
 const int ERROR = -1;
 
-PX4FLOW	*g_dev;
+PX4FLOW	*g_dev = nullptr;
+bool start_in_progress = false;
+
+const int START_RETRY_COUNT = 5;
+const int START_RETRY_TIMEOUT = 1000;
 
 void	start();
 void	stop();
@@ -651,78 +655,93 @@ void
 start()
 {
 	int fd;
+	
+	/* entry check: */
+	if (start_in_progress) {
+		errx(1, "start in progress");
+	}
+	start_in_progress = true;
 
 	if (g_dev != nullptr) {
+		start_in_progress = false;
 		errx(1, "already started");
 	}
 
-	const int busses_to_try[] = {
-		PX4_I2C_BUS_EXPANSION,
-		#ifdef PX4_I2C_BUS_ESC
-		PX4_I2C_BUS_ESC,
-		#endif
-		PX4_I2C_BUS_ONBOARD,
-		-1
-	};
+	int retry_nr = 0;
+	while (1) {
+		const int busses_to_try[] = {
+			PX4_I2C_BUS_EXPANSION,
+			#ifdef PX4_I2C_BUS_ESC
+			PX4_I2C_BUS_ESC,
+			#endif
+			PX4_I2C_BUS_ONBOARD,
+			-1
+		};
 
-	const int *cur_bus = busses_to_try;
-	while(*cur_bus != -1) {
-		/* create the driver */
-		//warnx("trying bus %d", *cur_bus);
-		g_dev = new PX4FLOW(*cur_bus);
-		if (g_dev == nullptr) {
-			/* this is a fatal error */
-			break;
+		const int *cur_bus = busses_to_try;
+
+		while(*cur_bus != -1) {
+			/* create the driver */
+			/* warnx("trying bus %d", *cur_bus); */
+			g_dev = new PX4FLOW(*cur_bus);
+			if (g_dev == nullptr) {
+				/* this is a fatal error */
+				break;
+			}
+
+			/* init the driver: */
+			if (OK == g_dev->init()) {
+				/* success! */
+				break;
+			}
+
+			/* destroy it again because it failed. */
+			delete g_dev;
+			g_dev = nullptr;
+
+			/* try next! */
+			cur_bus++;
 		}
-		
-		/* init the driver: */
-		if (OK == g_dev->init()) {
+
+		/* check whether we found it: */
+		if (*cur_bus != -1) {
+			
+			/* check for failure: */
+			if (g_dev == nullptr) {
+				break;
+			}
+
+			/* set the poll rate to default, starts automatic data collection */
+			fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
+
+			if (fd < 0) {
+				break;
+			}
+
+			if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
+				break;
+			}
+
 			/* success! */
+			start_in_progress = false;
+			exit(0);
+		}
+
+		if (retry_nr < START_RETRY_COUNT) {
+			warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
+			usleep(START_RETRY_TIMEOUT * 1000);
+			retry_nr++;
+		} else {
 			break;
 		}
-		
-		/* destroy it again because it failed. */
-		delete g_dev;
-		g_dev = nullptr;
-		
-		/* try next! */
-		cur_bus++;
 	}
 	
-	/* check whether we found it: */
-	if (*cur_bus == -1) {
-		goto not_found;
-	}
-	
-	/* check for failure: */
-	if (g_dev == nullptr) {
-		goto fatal_fail;
-	}
-	
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		goto fatal_fail;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
-		goto fatal_fail;
-	}
-
-	exit(0);
-
-not_found:
-	/* for now we do the same as if there was a fatal failure. */
-	warnx("PX4FLOW not found on I2C busses");
-	
-fatal_fail:
-
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
-
+	
+	start_in_progress = false;
 	errx(1, "PX4FLOW could not be started over I2C");
 }