LidarLite: update for new orientation convention

add possibility to specify orientation and adapt to new defaults and use px4_getopt

src/drivers/ll40ls/LidarLiteI2C.cpp

@@ -49,8 +49,9 @@
 #include <stdio.h>
 #include <drivers/drv_hrt.h>
 
-LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
+LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address, uint8_t rotation) :
 	I2C("LL40LS", path, bus, address, 100000),
+	_rotation(rotation),
 	_work{},
 	_reports(nullptr),
 	_sensor_ok(false),
@@ -454,7 +455,7 @@ int LidarLiteI2C::collect()
 	report.covariance = 0.0f;
 	/* the sensor is in fact a laser + sonar but there is no enum for this */
 	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
-	report.orientation = 8;
+	report.orientation = _rotation;
 	/* TODO: set proper ID */
 	report.id = 0;
 

src/drivers/ll40ls/LidarLiteI2C.h

@@ -71,7 +71,8 @@
 class LidarLiteI2C : public LidarLite, public device::I2C
 {
 public:
-	LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR);
+	LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR,
+		     uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	virtual ~LidarLiteI2C();
 
 	int         init() override;
@@ -100,6 +101,7 @@ protected:
 	int                 reset_sensor();
 
 private:
+	uint8_t _rotation;
 	work_s              _work;
 	ringbuffer::RingBuffer          *_reports;
 	bool                _sensor_ok;

src/drivers/ll40ls/LidarLitePWM.cpp

@@ -49,8 +49,9 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_input.h>
 
-LidarLitePWM::LidarLitePWM(const char *path) :
+LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) :
 	CDev("LidarLitePWM", path),
+	_rotation(rotation),
 	_work{},
 	_reports(nullptr),
 	_class_instance(-1),
@@ -178,7 +179,7 @@ int LidarLitePWM::measure()
 	_range.min_distance = get_minimum_distance();
 	_range.current_distance = float(_pwm.pulse_width) * 1e-3f;   /* 10 usec = 1 cm distance for LIDAR-Lite */
 	_range.covariance = 0.0f;
-	_range.orientation = 8;
+	_range.orientation = _rotation;
 	/* TODO: set proper ID */
 	_range.id = 0;
 

src/drivers/ll40ls/LidarLitePWM.h

@@ -60,7 +60,7 @@
 class LidarLitePWM : public LidarLite, public device::CDev
 {
 public:
-	LidarLitePWM(const char *path);
+	LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	virtual ~LidarLitePWM();
 
 	int init() override;
@@ -107,6 +107,7 @@ protected:
 	void task_main_trampoline(int argc, char *argv[]);
 
 private:
+	uint8_t _rotation;
 	work_s			_work;
 	ringbuffer::RingBuffer	*_reports;
 	int			_class_instance;

src/drivers/ll40ls/ll40ls.cpp

@@ -49,6 +49,7 @@
 #include <cstdlib>
 #include <string.h>
 #include <stdio.h>
+#include <platforms/px4_getopt.h>
 
 #ifndef CONFIG_SCHED_WORKQUEUE
 # error This requires CONFIG_SCHED_WORKQUEUE.
@@ -93,7 +94,7 @@ namespace ll40ls
 
 LidarLite *instance = nullptr;
 
-void	start(enum LL40LS_BUS busid);
+void	start(enum LL40LS_BUS busid, uint8_t rotation);
 void	stop();
 void	test();
 void	reset();
@@ -104,7 +105,7 @@ void	usage();
 /**
  * Start the driver.
  */
-void start(enum LL40LS_BUS busid)
+void start(enum LL40LS_BUS busid, uint8_t rotation)
 {
 	int fd, ret;
 
@@ -113,7 +114,7 @@ void start(enum LL40LS_BUS busid)
 	}
 
 	if (busid == LL40LS_BUS_PWM) {
-		instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM);
+		instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation);
 
 		if (!instance) {
 			warnx("Failed to instantiate LidarLitePWM");
@@ -131,7 +132,7 @@ void start(enum LL40LS_BUS busid)
 				continue;
 			}
 
-			instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname);
+			instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation);
 
 			if (!instance) {
 				warnx("Failed to instantiate LidarLiteI2C");
@@ -340,6 +341,7 @@ usage()
 #ifdef PX4_I2C_BUS_ONBOARD
 	warnx("    -I only internal bus");
 #endif
+	warnx("E.g. ll40ls start i2c -R 0");
 }
 
 } // namespace
@@ -348,9 +350,12 @@ int
 ll40ls_main(int argc, char *argv[])
 {
 	int ch;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
 	enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 
-	while ((ch = getopt(argc, argv, "XI")) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "IXR:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 #ifdef PX4_I2C_BUS_ONBOARD
 
@@ -363,6 +368,11 @@ ll40ls_main(int argc, char *argv[])
 			busid = LL40LS_BUS_I2C_EXTERNAL;
 			break;
 
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
+			break;
+
 		default:
 			ll40ls::usage();
 			return 0;
@@ -370,8 +380,8 @@ ll40ls_main(int argc, char *argv[])
 	}
 
 	/* determine protocol first because it's needed next */
-	if (argc > optind + 1) {
-		const char *protocol = argv[optind + 1];
+	if (argc > myoptind + 1) {
+		const char *protocol = argv[myoptind + 1];
 
 		if (!strcmp(protocol, "pwm")) {
 			busid = LL40LS_BUS_PWM;;
@@ -387,11 +397,11 @@ ll40ls_main(int argc, char *argv[])
 	}
 
 	/* now determine action */
-	if (argc > optind) {
-		const char *verb = argv[optind];
+	if (argc > myoptind) {
+		const char *verb = argv[myoptind];
 
 		if (!strcmp(verb, "start")) {
-			ll40ls::start(busid);
+			ll40ls::start(busid, rotation);
 
 		} else if (!strcmp(verb, "stop")) {
 			ll40ls::stop();