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

src/drivers/distance_sensor/teraranger/teraranger.cpp

@@ -71,7 +71,7 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define TERARANGER_BUS           PX4_I2C_BUS_EXPANSION
+#define TERARANGER_BUS_DEFAULT           PX4_I2C_BUS_EXPANSION
 #define TRONE_BASEADDR      0x30 /* 7-bit address */
 #define TREVO_BASEADDR      0x31 /* 7-bit address */
 #define TERARANGER_DEVICE_PATH   	"/dev/teraranger"
@@ -101,7 +101,7 @@ class TERARANGER : public device::I2C
 {
 public:
 	TERARANGER(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
-		   int bus = TERARANGER_BUS, int address = TRONE_BASEADDR);
+		   int bus = TERARANGER_BUS_DEFAULT, int address = TRONE_BASEADDR);
 	virtual ~TERARANGER();
 
 	virtual int 		init();
@@ -748,26 +748,69 @@ namespace teraranger
 
 TERARANGER	*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;
 
 	if (g_dev != nullptr) {
-		errx(1, "already started");
+		PX4_ERR("already started");
+		return PX4_ERROR;
 	}
 
 	/* create the driver */
-	g_dev = new TERARANGER(rotation, TERARANGER_BUS);
+	g_dev = new TERARANGER(rotation, i2c_bus);
 
 
 	if (g_dev == nullptr) {
@@ -789,7 +832,7 @@ start(uint8_t rotation)
 		goto fail;
 	}
 
-	exit(0);
+	return PX4_OK;
 
 fail:
 
@@ -798,23 +841,26 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	PX4_WARN("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;
 }
 
 /**
@@ -822,7 +868,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;
@@ -832,21 +878,24 @@ test()
 	int fd = open(TERARANGER_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'teraranger start' if the driver is not running", TERARANGER_DEVICE_PATH);
+		PX4_ERR("%s open failed (try 'teraranger start' if the driver is not running)", TERARANGER_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 50x and report each value */
@@ -859,14 +908,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);
@@ -874,53 +925,73 @@ 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;
 	}
 
-	errx(0, "PASS");
+	PX4_INFO("PASS");
+	return PX4_OK;
+
 }
 
 /**
  * Reset the driver.
  */
-void
+int
 reset()
 {
 	int fd = open(TERARANGER_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;
 	}
 
-	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
+teraranger_usage()
+{
+	PX4_INFO("usage: teraranger command [options]");
+	PX4_INFO("options:");
+	PX4_INFO("\t-b --bus i2cbus (%d)", TERARANGER_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
 teraranger_main(int argc, char *argv[])
 {
@@ -928,16 +999,27 @@ teraranger_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;
+
+	int i2c_bus = TERARANGER_BUS_DEFAULT;
 
-	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+	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;
 		}
 	}
 
@@ -949,38 +1031,44 @@ teraranger_main(int argc, char *argv[])
 	 * Start/load the driver.
 	 */
 	if (!strcmp(argv[myoptind], "start")) {
-		teraranger::start(rotation);
+
+		if (start_all) {
+			return teraranger::start(rotation);
+
+		} else {
+			return teraranger::start_bus(rotation, i2c_bus);
+		}
 	}
 
 	/*
 	 * Stop the driver
 	 */
 	if (!strcmp(argv[myoptind], "stop")) {
-		teraranger::stop();
+		return teraranger::stop();
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
 	if (!strcmp(argv[myoptind], "test")) {
-		teraranger::test();
+		return teraranger::test();
 	}
 
 	/*
 	 * Reset the driver.
 	 */
 	if (!strcmp(argv[myoptind], "reset")) {
-		teraranger::reset();
+		return teraranger::reset();
 	}
 
 	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
-		teraranger::info();
+		return teraranger::info();
 	}
 
 out_error:
-	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
-	return -1;
+	teraranger_usage();
+	return PX4_ERROR;
 }