Merge branch 'master' of github.com:PX4/Firmware

1 files changed, 138 insertions, 57 deletions

diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp
index a69e6ee55..8b156f7ba 100644
--- a/src/drivers/ll40ls/ll40ls.cpp
+++ b/src/drivers/ll40ls/ll40ls.cpp
@@ -74,7 +74,8 @@
 /* Configuration Constants */
 #define LL40LS_BUS 			PX4_I2C_BUS_EXPANSION
 #define LL40LS_BASEADDR 	0x42 /* 7-bit address */
-#define LL40LS_DEVICE_PATH 	"/dev/ll40ls"
+#define LL40LS_DEVICE_PATH_INT 	"/dev/ll40ls_int"
+#define LL40LS_DEVICE_PATH_EXT 	"/dev/ll40ls_ext"
 
 /* LL40LS Registers addresses */
 
@@ -101,7 +102,7 @@ static const int ERROR = -1;
 class LL40LS : public device::I2C
 {
 public:
-	LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
+	LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR);
 	virtual ~LL40LS();
 
 	virtual int 		init();
@@ -132,6 +133,10 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
+	uint16_t		_last_distance;
+
+	/**< the bus the device is connected to */
+	int			_bus;
 
 	/**
 	* Test whether the device supported by the driver is present at a
@@ -188,8 +193,8 @@ private:
  */
 extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
 
-LL40LS::LL40LS(int bus, int address) :
-	I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
+LL40LS::LL40LS(int bus, const char *path, int address) :
+	I2C("LL40LS", path, bus, address, 100000),
 	_min_distance(LL40LS_MIN_DISTANCE),
 	_max_distance(LL40LS_MAX_DISTANCE),
 	_reports(nullptr),
@@ -200,7 +205,9 @@ LL40LS::LL40LS(int bus, int address) :
 	_range_finder_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")),
+	_last_distance(0),
+	_bus(bus)
 {
 	// up the retries since the device misses the first measure attempts
 	I2C::_retries = 3;
@@ -521,6 +528,8 @@ LL40LS::collect()
 	float si_units = distance * 0.01f; /* cm to m */
 	struct range_finder_report report;
 
+	_last_distance = distance;
+
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
@@ -648,6 +657,8 @@ LL40LS::print_info()
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
 	_reports->print_info("report queue");
+	printf("distance: %ucm (0x%04x)\n", 
+	       (unsigned)_last_distance, (unsigned)_last_distance);
 }
 
 /**
@@ -662,55 +673,89 @@ namespace ll40ls
 #endif
 const int ERROR = -1;
 
-LL40LS	*g_dev;
+LL40LS	*g_dev_int;
+LL40LS	*g_dev_ext;
 
-void	start();
-void	stop();
-void	test();
-void	reset();
-void	info();
+void	start(int bus);
+void	stop(int bus);
+void	test(int bus);
+void	reset(int bus);
+void	info(int bus);
+void	usage();
 
 /**
  * Start the driver.
  */
 void
-start()
+start(int bus)
 {
-	int fd;
-
-	if (g_dev != nullptr) {
-		errx(1, "already started");
+	/* create the driver, attempt expansion bus first */
+	if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
+		if (g_dev_ext != nullptr)
+			errx(0, "already started external");
+		g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
+		if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
+			delete g_dev_ext;
+			g_dev_ext = nullptr;
+		}
 	}
 
-	/* create the driver */
-	g_dev = new LL40LS(LL40LS_BUS);
+#ifdef PX4_I2C_BUS_ONBOARD
+	/* if this failed, attempt onboard sensor */
+	if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
+		if (g_dev_int != nullptr)
+			errx(0, "already started internal");
+		g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
+		if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+			/* tear down the failing onboard instance */
+			delete g_dev_int;
+			g_dev_int = nullptr;
 
-	if (g_dev == nullptr) {
-		goto fail;
-	}
-
-	if (OK != g_dev->init()) {
-		goto fail;
+			if (bus == PX4_I2C_BUS_ONBOARD) {
+				goto fail;
+			}
+		}
+		if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
+			goto fail;
+		}
 	}
+#endif
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		goto fail;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		goto fail;
-	}
+	if (g_dev_int != nullptr) {
+		int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
+                if (fd == -1) {
+                    goto fail;
+                }
+		int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+		close(fd);
+		if (ret < 0) {
+			goto fail;
+		}
+        }
+
+	if (g_dev_ext != nullptr) {
+		int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
+                if (fd == -1) {
+                    goto fail;
+                }
+		int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+		close(fd);
+		if (ret < 0) {
+			goto fail;
+		}
+        }
 
 	exit(0);
 
 fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
+		delete g_dev_int;
+		g_dev_int = nullptr;
+	}
+	if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
+		delete g_dev_ext;
+		g_dev_ext = nullptr;
 	}
 
 	errx(1, "driver start failed");
@@ -719,11 +764,12 @@ fail:
 /**
  * Stop the driver
  */
-void stop()
+void stop(int bus)
 {
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
+	LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext);
+	if (*g_dev != nullptr) {
+		delete *g_dev;
+		*g_dev = nullptr;
 
 	} else {
 		errx(1, "driver not running");
@@ -738,16 +784,17 @@ void stop()
  * and automatic modes.
  */
 void
-test()
+test(int bus)
 {
 	struct range_finder_report report;
 	ssize_t sz;
 	int ret;
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
 
-	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
+		err(1, "%s open failed (try 'll40ls start' if the driver is not running", path);
 	}
 
 	/* do a simple demand read */
@@ -803,9 +850,10 @@ test()
  * Reset the driver.
  */
 void
-reset()
+reset(int bus)
 {
-	int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
+	const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
+	int fd = open(path, O_RDONLY);
 
 	if (fd < 0) {
 		err(1, "failed ");
@@ -826,8 +874,9 @@ reset()
  * Print a little info about the driver.
  */
 void
-info()
+info(int bus)
 {
+	LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
 	if (g_dev == nullptr) {
 		errx(1, "driver not running");
 	}
@@ -838,44 +887,76 @@ info()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'");
+	warnx("options:");
+	warnx("    -X only external bus");
+#ifdef PX4_I2C_BUS_ONBOARD
+	warnx("    -I only internal bus");
+#endif
+}
+
 } // namespace
 
 int
 ll40ls_main(int argc, char *argv[])
 {
+	int ch;
+	int bus = -1;
+
+	while ((ch = getopt(argc, argv, "XI")) != EOF) {
+		switch (ch) {
+#ifdef PX4_I2C_BUS_ONBOARD
+		case 'I':
+			bus = PX4_I2C_BUS_ONBOARD;
+			break;
+#endif
+		case 'X':
+			bus = PX4_I2C_BUS_EXPANSION;
+			break;
+		default:
+			ll40ls::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];	
+	
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start")) {
-		ll40ls::start();
+	if (!strcmp(verb, "start")) {
+		ll40ls::start(bus);
 	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
-		ll40ls::stop();
+	if (!strcmp(verb, "stop")) {
+		ll40ls::stop(bus);
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test")) {
-		ll40ls::test();
+	if (!strcmp(verb, "test")) {
+		ll40ls::test(bus);
 	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
-		ll40ls::reset();
+	if (!strcmp(verb, "reset")) {
+		ll40ls::reset(bus);
 	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
-		ll40ls::info();
+	if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
+		ll40ls::info(bus);
 	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");