vl53lxx driver: add -b, -a and -R flags to choose i2c busses and sensor rotation and remove nuttx stuff such as exit functions

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -56,14 +56,12 @@
 #include <string.h>
 #include <fcntl.h>
 #include <poll.h>
-#include <errno.h>
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
 #include <vector>
 
 #include <perf/perf_counter.h>
-#include <systemlib/err.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -673,6 +671,11 @@ int 	info();
 int
 start(uint8_t rotation)
 {
+	if (g_dev != nullptr) {
+		PX4_ERR("already started");
+		return PX4_ERROR;
+	}
+
 	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if (start_bus(rotation, bus_options[i]) == PX4_OK) {
 			return PX4_OK;
@@ -693,11 +696,6 @@ start_bus(uint8_t rotation, int i2c_bus)
 {
 	int fd = -1;
 
-	if (g_dev != nullptr) {
-		PX4_ERR("already started");
-		return PX4_ERROR;
-	}
-
 	/* create the driver */
 	g_dev = new SF1XX(rotation, i2c_bus);
 

src/drivers/distance_sensor/teraranger/teraranger.cpp

@@ -53,13 +53,11 @@
 #include <string.h>
 #include <fcntl.h>
 #include <poll.h>
-#include <errno.h>
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
 
 #include <perf/perf_counter.h>
-#include <systemlib/err.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -784,6 +782,11 @@ int 	info();
 int
 start(uint8_t rotation)
 {
+	if (g_dev != nullptr) {
+		PX4_ERR("already started");
+		return PX4_ERROR;
+	}
+
 	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
 		if (start_bus(rotation, bus_options[i]) == PX4_OK) {
 			return PX4_OK;
@@ -802,12 +805,7 @@ start(uint8_t rotation)
 int
 start_bus(uint8_t rotation, int i2c_bus)
 {
-	int fd;
-
-	if (g_dev != nullptr) {
-		PX4_ERR("already started");
-		return PX4_ERROR;
-	}
+	int fd = -1;
 
 	/* create the driver */
 	g_dev = new TERARANGER(rotation, i2c_bus);
@@ -832,10 +830,15 @@ start_bus(uint8_t rotation, int i2c_bus)
 		goto fail;
 	}
 
+	close(fd);
 	return PX4_OK;
 
 fail:
 
+	if (fd >= 0) {
+		close(fd);
+	}
+
 	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
@@ -929,6 +932,7 @@ test()
 		return PX4_ERROR;
 	}
 
+	close(fd);
 	PX4_INFO("PASS");
 	return PX4_OK;
 
@@ -957,6 +961,7 @@ reset()
 		return PX4_ERROR;
 	}
 
+	close(fd);
 	return PX4_OK;
 }
 

src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp

@@ -53,13 +53,11 @@
 #include <string.h>
 #include <fcntl.h>
 #include <poll.h>
-#include <errno.h>
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
 
 #include <perf/perf_counter.h>
-#include <systemlib/err.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -67,16 +65,11 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/obstacle_distance.h>
 
 #include <board_config.h>
 
 /* Configuration Constants */
-#ifdef PX4_I2C_BUS_EXPANSION
-#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION
-#else
-#define VL53LXX_BUS 0
-#endif
+#define VL53LXX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
 
 #define VL53LXX_BASEADDR 0b0101001 // 7-bit address
 #define VL53LXX_DEVICE_PATH "/dev/vl53lxx"
@@ -116,7 +109,7 @@ class VL53LXX : public device::I2C
 {
 public:
 	VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
-		int bus = VL53LXX_BUS, int address = VL53LXX_BASEADDR);
+		int bus = VL53LXX_BUS_DEFAULT, int address = VL53LXX_BASEADDR);
 
 	virtual ~VL53LXX();
 
@@ -477,7 +470,6 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
 	ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0);
 
 	if (OK != ret) {
-		DEVICE_LOG("i2c::transfer returned %d", ret);
 		perf_count(_comms_errors);
 		return ret;
 	}
@@ -486,7 +478,6 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
 	ret = transfer(nullptr, 0, &value, 1);
 
 	if (OK != ret) {
-		DEVICE_LOG("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		return ret;
 	}
@@ -505,7 +496,7 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length)
 	ret = transfer(&reg_address, 1, nullptr, 0);
 
 	if (OK != ret) {
-		DEVICE_LOG("i2c::transfer returned %d", ret);
+		perf_count(_comms_errors);
 		return ret;
 	}
 
@@ -513,7 +504,6 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length)
 	ret = transfer(nullptr, 0, &value[0], length);
 
 	if (OK != ret) {
-		DEVICE_LOG("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		return ret;
 	}
@@ -536,7 +526,6 @@ VL53LXX::writeRegister(uint8_t reg_address, uint8_t value)
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		DEVICE_LOG("i2c::transfer returned %d", ret);
 		return ret;
 	}
 
@@ -566,7 +555,6 @@ VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value,
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		DEVICE_LOG("i2c::transfer returned %d", ret);
 		return ret;
 	}
 
@@ -979,26 +967,68 @@ namespace vl53lxx
 
 VL53LXX	*g_dev;
 
-void	start(uint8_t rotation);
-void	stop();
-void	test();
-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 	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)
 {
-	int fd = -1;
-
 	if (g_dev != nullptr) {
-		errx(1, "already started");
+		PX4_ERR("already started");
+		return PX4_ERROR;
 	}
 
-	/* create the driver */
-	g_dev = new VL53LXX(rotation, VL53LXX_BUS);
+	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;
+
+	/* create the driver */
+	g_dev = new VL53LXX(rotation, i2c_bus);
 
 	if (g_dev == nullptr) {
 		goto fail;
@@ -1019,7 +1049,8 @@ start(uint8_t rotation)
 		goto fail;
 	}
 
-	exit(0);
+	close(fd);
+	return PX4_OK;
 
 fail:
 
@@ -1032,23 +1063,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;
 }
 
 /**
@@ -1056,7 +1090,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;
@@ -1065,84 +1099,126 @@ test()
 	int fd = open(VL53LXX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'vl53lxx start' if the driver is not running)", VL53LXX_DEVICE_PATH);
+		PX4_ERR("%s open failed (try 'vl53lxx start' if the driver is not running)", VL53LXX_DEVICE_PATH);
+		return PX4_ERROR;
 	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
 	if (sz != sizeof(report)) {
-		PX4_ERR("ret: %d, expected: %d", sz, sizeof(report));
-		err(1, "immediate acc read failed");
+		PX4_ERR("immediate read failed");
+		return PX4_ERROR;
 	}
 
 	print_message(report);
 
 	close(fd);
 
-	errx(0, "PASS");
+	PX4_INFO("PASS");
+	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 vl53lxx
 
 
+static void
+vl53lxx_usage()
+{
+	PX4_INFO("usage: vl53lxx command [options]");
+	PX4_INFO("options:");
+	PX4_INFO("\t-b --bus i2cbus (%d)", VL53LXX_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|info");
+}
+
+
 int
 vl53lxx_main(int argc, char *argv[])
 {
+	int ch;
 	int myoptind = 1;
+	const char *myoptarg = nullptr;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	bool start_all = false;
 
-	if (argc < 2) {
-		goto out;
+	int i2c_bus = VL53LXX_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!");
+			goto out_error;
+		}
 	}
 
 	/*
 	 * Start/load the driver.
 	 */
 	if (!strcmp(argv[myoptind], "start")) {
-		vl53lxx::start(rotation);
+		if (start_all) {
+			return vl53lxx::start(rotation);
+
+		} else {
+			return vl53lxx::start_bus(rotation, i2c_bus);
+		}
 	}
 
 	/*
 	 * Stop the driver
 	 */
 	if (!strcmp(argv[myoptind], "stop")) {
-		vl53lxx::stop();
+		return vl53lxx::stop();
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
 	if (!strcmp(argv[myoptind], "test")) {
-		vl53lxx::test();
+		return vl53lxx::test();
 	}
 
 	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
-		vl53lxx::info();
+		return vl53lxx::info();
 	}
 
-out:
+out_error:
 
-	PX4_ERR("unrecognized command, try 'start', 'test', or 'info'");
+	vl53lxx_usage();
 	return PX4_ERROR;
 }