sf1xx driver: add -b and -a flags to choose i2c busses and remove nuttx stuff such as exit functions

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -75,7 +75,7 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define SF1XX_BUS 		PX4_I2C_BUS_EXPANSION
+#define SF1XX_BUS_DEFAULT 		PX4_I2C_BUS_EXPANSION
 #define SF1XX_BASEADDR 	0x66
 #define SF1XX_DEVICE_PATH	"/dev/sf1xx"
 
@@ -87,7 +87,7 @@
 class SF1XX : public device::I2C
 {
 public:
-	SF1XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SF1XX_BUS,
+	SF1XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SF1XX_BUS_DEFAULT,
 	      int address = SF1XX_BASEADDR);
 	virtual ~SF1XX();
 
@@ -637,26 +637,69 @@ namespace sf1xx
 
 SF1XX	*g_dev;
 
-void	start(uint8_t rotation);
-void	stop();
-void	test();
-void	reset();
-void	info();
+int bus_options[] = {
+#ifdef PX4_I2C_BUS_EXPANSION
+	PX4_I2C_BUS_EXPANSION,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	PX4_I2C_BUS_EXPANSION1,
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION2
+	PX4_I2C_BUS_EXPANSION2,
+#endif
+#ifdef PX4_I2C_BUS_ONBOARD
+	PX4_I2C_BUS_ONBOARD,
+#endif
+};
+
+#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
+
+int 	start(uint8_t rotation);
+int 	start_bus(uint8_t rotation, int i2c_bus);
+int 	stop();
+int 	test();
+int 	reset();
+int 	info();
 
 /**
- * Start the driver.
+ *
+ * Attempt to start driver on all available I2C busses.
+ *
+ * This function will return as soon as the first sensor
+ * is detected on one of the available busses or if no
+ * sensors are detected.
+ *
  */
-void
+int
 start(uint8_t rotation)
+{
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (start_bus(rotation, bus_options[i]) == PX4_OK) {
+			return PX4_OK;
+		}
+	}
+
+	return PX4_ERROR;
+}
+
+/**
+ * Start the driver on a specific bus.
+ *
+ * This function only returns if the sensor is up and running
+ * or could not be detected successfully.
+ */
+int
+start_bus(uint8_t rotation, int i2c_bus)
 {
 	int fd = -1;
 
 	if (g_dev != nullptr) {
-		errx(1, "already started");
+		PX4_ERR("already started");
+		return PX4_ERROR;
 	}
 
 	/* create the driver */
-	g_dev = new SF1XX(rotation, SF1XX_BUS);
+	g_dev = new SF1XX(rotation, i2c_bus);
 
 	if (g_dev == nullptr) {
 		goto fail;
@@ -679,7 +722,7 @@ start(uint8_t rotation)
 	}
 
 	::close(fd);
-	exit(0);
+	return PX4_OK;
 
 fail:
 
@@ -688,23 +731,26 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	PX4_ERR("not started on bus %d", i2c_bus);
+	return PX4_ERROR;
 }
 
 /**
  * Stop the driver
  */
-void stop()
+int
+stop()
 {
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 
 	} else {
-		errx(1, "driver not running");
+		PX4_ERR("driver not running");
+		return PX4_ERROR;
 	}
 
-	exit(0);
+	return PX4_OK;
 }
 
 /**
@@ -712,7 +758,7 @@ void stop()
  * make sure we can collect data from the sensor in polled
  * and automatic modes.
  */
-void
+int
 test()
 {
 	struct distance_sensor_s report;
@@ -722,21 +768,24 @@ test()
 	int fd = open(SF1XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'sf1xx start' if the driver is not running", SF1XX_DEVICE_PATH);
+		PX4_ERR("%s open failed (try 'sf1xx start' if the driver is not running)", SF1XX_DEVICE_PATH);
+		return PX4_ERROR;
 	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
 	if (sz != sizeof(report)) {
-		err(1, "immediate read failed");
+		PX4_ERR("immediate read failed");
+		return PX4_ERROR;
 	}
 
 	print_message(report);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		errx(1, "failed to set 2Hz poll rate");
+		PX4_ERR("failed to set 2Hz poll rate");
+		return PX4_ERROR;
 	}
 
 	/* read the sensor 5x and report each value */
@@ -749,14 +798,16 @@ test()
 		ret = poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			errx(1, "timed out waiting for sensor data");
+			PX4_ERR("timed out waiting for sensor data");
+			return PX4_ERROR;
 		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			err(1, "periodic read failed");
+			PX4_ERR("periodic read failed");
+			return PX4_ERROR;
 		}
 
 		print_message(report);
@@ -764,55 +815,76 @@ test()
 
 	/* reset the sensor polling to default rate */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		errx(1, "failed to set default poll rate");
+		PX4_ERR("failed to set default poll rate");
+		return PX4_ERROR;
 	}
 
 	::close(fd);
-	errx(0, "PASS");
+
+	PX4_INFO("PASS");
+	return PX4_OK;
 }
 
 /**
  * Reset the driver.
  */
-void
+int
 reset()
 {
 	int fd = open(SF1XX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "failed ");
+		PX4_ERR("failed");
+		return PX4_ERROR;
 	}
 
 	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		err(1, "driver reset failed");
+		PX4_ERR("driver reset failed");
+		return PX4_ERROR;
 	}
 
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		err(1, "driver poll restart failed");
+		PX4_ERR("driver poll restart failed");
+		return PX4_ERROR;
 	}
 
 	::close(fd);
-	exit(0);
+
+	return PX4_OK;
 }
 
 /**
  * Print a little info about the driver.
  */
-void
+int
 info()
 {
 	if (g_dev == nullptr) {
-		errx(1, "driver not running");
+		PX4_ERR("driver not running");
+		return PX4_ERROR;
 	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
 
-	exit(0);
+	return PX4_OK;
 }
 
 } /* namespace */
 
+
+static void
+sf1xx_usage()
+{
+	PX4_INFO("usage: sf1xx command [options]");
+	PX4_INFO("options:");
+	PX4_INFO("\t-b --bus i2cbus (%d)", SF1XX_BUS_DEFAULT);
+	PX4_INFO("\t-a --all");
+	PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	PX4_INFO("command:");
+	PX4_INFO("\tstart|stop|test|reset|info");
+}
+
 int
 sf1xx_main(int argc, char *argv[])
 {
@@ -820,16 +892,27 @@ sf1xx_main(int argc, char *argv[])
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	bool start_all = false;
 
-	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+	int i2c_bus = SF1XX_BUS_DEFAULT;
+
+	while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
 			rotation = (uint8_t)atoi(myoptarg);
 			break;
 
+		case 'b':
+			i2c_bus = atoi(myoptarg);
+			break;
+
+		case 'a':
+			start_all = true;
+			break;
+
 		default:
 			PX4_WARN("Unknown option!");
-			return -1;
+			goto out_error;
 		}
 	}
 
@@ -841,38 +924,43 @@ sf1xx_main(int argc, char *argv[])
 	 * Start/load the driver.
 	 */
 	if (!strcmp(argv[myoptind], "start")) {
-		sf1xx::start(rotation);
+		if (start_all) {
+			return sf1xx::start(rotation);
+
+		} else {
+			return sf1xx::start_bus(rotation, i2c_bus);
+		}
 	}
 
 	/*
 	 * Stop the driver
 	 */
 	if (!strcmp(argv[myoptind], "stop")) {
-		sf1xx::stop();
+		return sf1xx::stop();
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
 	if (!strcmp(argv[myoptind], "test")) {
-		sf1xx::test();
+		return sf1xx::test();
 	}
 
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(argv[myoptind], "reset")) {
-		sf1xx::reset();
+		return sf1xx::reset();
 	}
 
 	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
-		sf1xx::info();
+		return sf1xx::info();
 	}
 
 out_error:
-	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
-	return -1;
+	sf1xx_usage();
+	return PX4_ERROR;
 }

src/drivers/distance_sensor/teraranger/teraranger.cpp

@@ -773,7 +773,7 @@ int 	reset();
 int 	info();
 
 /**
- * Start the driver.
+ *
  * Attempt to start driver on all available I2C busses.
  *
  * This function will return as soon as the first sensor
@@ -841,7 +841,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	PX4_WARN("not started on bus %d", i2c_bus);
+	PX4_ERR("not started on bus %d", i2c_bus);
 	return PX4_ERROR;
 }