ll40ls: first stab at adapting ll40ls to the new distance_sensor msg

src/drivers/ll40ls/LidarLiteI2C.cpp

@@ -61,7 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
 	_sensor_ok(false),
 	_collect_phase(false),
 	_class_instance(-1),
-	_range_finder_topic(-1),
+	_distance_sensor_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")),
@@ -115,7 +115,7 @@ int LidarLiteI2C::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
 
 	if (_reports == nullptr) {
 		goto out;
@@ -125,13 +125,13 @@ int LidarLiteI2C::init()
 
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
 		/* get a publish handle on the range finder topic */
-		struct range_finder_report rf_report;
+		struct distance_sensor_s ds_report;
 		measure();
-		_reports->get(&rf_report);
-		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+		_reports->get(&ds_report);
+		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
 
-		if (_range_finder_topic < 0) {
-			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		if (_distance_sensor_topic < 0) {
+			debug("failed to create distance_sensor object. Did you start uOrb?");
 		}
 	}
 
@@ -229,8 +229,8 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(struct range_finder_report);
-	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -400,11 +400,11 @@ int LidarLiteI2C::collect()
 		return ret;
 	}
 
-	uint16_t distance = (val[0] << 8) | val[1];
-	float si_units = distance * 0.01f; /* cm to m */
-	struct range_finder_report report;
+	uint16_t distance_cm = (val[0] << 8) | val[1];
+	float distance_m = float(distance_cm) * 1e-2f;
+	struct distance_sensor_s report;
 
-	if (distance == 0) {
+	if (distance_cm == 0) {
 		_zero_counter++;
 
 		if (_zero_counter == 20) {
@@ -425,25 +425,23 @@ int LidarLiteI2C::collect()
 		_zero_counter = 0;
 	}
 
-	_last_distance = distance;
+	_last_distance = distance_m;
 
 	/* 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);
-	report.distance = si_units;
-	report.minimum_distance = get_minimum_distance();
-	report.maximum_distance = get_maximum_distance();
-
-	if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
-		report.valid = 1;
-
-	} else {
-		report.valid = 0;
-	}
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	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;
+	/* TODO: set proper ID */
+	report.id = 0;
 
 	/* publish it, if we are the primary */
-	if (_range_finder_topic >= 0) {
-		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	if (_distance_sensor_topic >= 0) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
 	}
 
 	if (_reports->force(&report)) {

src/drivers/ll40ls/LidarLiteI2C.h

@@ -50,6 +50,7 @@
 #include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/subsystem_info.h>
 
 /* Configuration Constants */
@@ -103,7 +104,7 @@ private:
 	bool                _collect_phase;
 	int                 _class_instance;
 
-	orb_advert_t        _range_finder_topic;
+	orb_advert_t        _distance_sensor_topic;
 
 	perf_counter_t      _sample_perf;
 	perf_counter_t      _comms_errors;

src/drivers/ll40ls/LidarLitePWM.cpp

@@ -60,7 +60,7 @@ LidarLitePWM::LidarLitePWM(const char *path) :
 	_class_instance(-1),
 	_pwmSub(-1),
 	_pwm{},
-	_range_finder_topic(-1),
+	_distance_sensor_topic(-1),
 	_range{},
 	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")),
 	_read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")),
@@ -82,7 +82,7 @@ LidarLitePWM::~LidarLitePWM()
 		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
 	}
 
-	// free perf counters
+	/* free perf counters */
 	perf_free(_sample_perf);
 	perf_free(_buffer_overflows);
 	perf_free(_sensor_zero_resets);
@@ -91,7 +91,6 @@ LidarLitePWM::~LidarLitePWM()
 
 int LidarLitePWM::init()
 {
-
 	/* do regular cdev init */
 	int ret = CDev::init();
 
@@ -100,7 +99,7 @@ int LidarLitePWM::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
 
 	if (_reports == nullptr) {
 		return ERROR;
@@ -109,14 +108,14 @@ int LidarLitePWM::init()
 	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
 
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* get a publish handle on the range finder topic */
-		struct range_finder_report rf_report;
+		/* get a publish handle on the distance_sensor topic */
+		struct distance_sensor_s ds_report;
 		measure();
-		_reports->get(&rf_report);
-		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+		_reports->get(&ds_report);
+		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
 
-		if (_range_finder_topic < 0) {
-			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		if (_distance_sensor_topic < 0) {
+			debug("failed to create distance_sensor object. Did you start uOrb?");
 		}
 	}
 
@@ -130,7 +129,7 @@ void LidarLitePWM::print_info()
 	perf_print_counter(_buffer_overflows);
 	perf_print_counter(_sensor_zero_resets);
 	warnx("poll interval:  %u ticks", getMeasureTicks());
-	warnx("distance: %.3fm", (double)_range.distance);
+	warnx("distance: %.3fm", (double)_range.current_distance);
 }
 
 void LidarLitePWM::print_registers()
@@ -180,26 +179,25 @@ int LidarLitePWM::measure()
 		return ERROR;
 	}
 
-	_range.type = RANGE_FINDER_TYPE_LASER;
-	_range.error_count = _pwm.error_count;
-	_range.maximum_distance = get_maximum_distance();
-	_range.minimum_distance = get_minimum_distance();
-	_range.distance = _pwm.pulse_width / 1000.0f;   //10 usec = 1 cm distance for LIDAR-Lite
-	_range.distance_vector[0] = _range.distance;
-	_range.just_updated = 0;
-	_range.valid = true;
+	_range.timestamp = hrt_absolute_time();
+	_range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	_range.max_distance = get_maximum_distance();
+	_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;
+	/* TODO: set proper ID */
+	_range.id = 0;
 
-	// Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0)
-	if (_range.distance <= 0.0f) {
-		_range.valid = false;
-		_range.error_count++;
+	/* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
+	if (_range.current_distance <= 0.0f) {
 		perf_count(_sensor_zero_resets);
 		perf_end(_sample_perf);
 		return reset_sensor();
 	}
 
-	if (_range_finder_topic >= 0) {
-		orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_range);
+	if (_distance_sensor_topic >= 0) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range);
 	}
 
 	if (_reports->force(&_range)) {
@@ -213,8 +211,8 @@ int LidarLitePWM::measure()
 
 ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(struct range_finder_report);
-	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */

src/drivers/ll40ls/LidarLitePWM.h

@@ -53,6 +53,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/pwm_input.h>
+#include <uORB/topics/distance_sensor.h>
 
 
 
@@ -109,8 +110,8 @@ private:
 	int			_class_instance;
 	int			_pwmSub;
 	struct pwm_input_s	_pwm;
-	orb_advert_t	        _range_finder_topic;
-	range_finder_report	_range;
+	orb_advert_t	        _distance_sensor_topic;
+	struct distance_sensor_s _range;
 
 	perf_counter_t	        _sample_perf;
 	perf_counter_t	        _read_errors;

src/drivers/ll40ls/ll40ls.cpp

@@ -239,7 +239,7 @@ void stop(const bool use_i2c, const int bus)
 void
 test(const bool use_i2c, const int bus)
 {
-	struct range_finder_report report;
+	struct distance_sensor_s report;
 	ssize_t sz;
 	int ret;
 
@@ -266,7 +266,7 @@ test(const bool use_i2c, const int bus)
 	}
 
 	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("measurement: %0.2f m", (double)report.current_distance);
 	warnx("time:        %lld", report.timestamp);
 
 	/* start the sensor polling at 2Hz */
@@ -295,7 +295,7 @@ test(const bool use_i2c, const int bus)
 		}
 
 		warnx("periodic read %u", i);
-		warnx("measurement: %0.3f m", (double)report.distance);
+		warnx("measurement: %0.3f m", (double)report.current_distance);
 		warnx("time:        %lld", report.timestamp);
 	}