Merge remote-tracking branch 'px4/pr/2200' into lidarlite_merge

src/drivers/drv_range_finder.h

@@ -46,16 +46,12 @@
 
 #define RANGE_FINDER_BASE_DEVICE_PATH	"/dev/range_finder"
 #define RANGE_FINDER0_DEVICE_PATH	"/dev/range_finder0"
-#define MB12XX_MAX_RANGEFINDERS	12	//Maximum number of RangeFinders that can be connected
+#define MB12XX_MAX_RANGEFINDERS	12	// Maximum number of Maxbotix sensors on bus
 
-#define range_finder_report range_finder_s
-#define __orb_sensor_range_finder __orb_range_finder
-
-#include <uORB/topics/range_finder.h>
-
-#ifndef RANGE_FINDER_TYPE_LASER
-#define RANGE_FINDER_TYPE_LASER 0
-#endif
+/*
+ * ObjDev tag for px4flow data.
+ */
+ORB_DECLARE(distance_sensor);
 
 /*
  * ioctl() definitions

src/drivers/mb12xx/mb12xx.cpp

@@ -70,6 +70,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>
 
@@ -131,7 +132,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;
@@ -208,7 +209,7 @@ MB12XX::MB12XX(int bus, int address) :
 	_measure_ticks(0),
 	_collect_phase(false),
 	_class_instance(-1),
-	_range_finder_topic(-1),
+	_distance_sensor_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
@@ -255,7 +256,7 @@ MB12XX::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
 
 	_index_counter = MB12XX_BASEADDR;	/* set temp sonar i2c address to base adress */
 	set_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
@@ -268,11 +269,11 @@ MB12XX::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 rf_report = {};
 
-		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
 
-		if (_range_finder_topic < 0) {
+		if (_distance_sensor_topic < 0) {
 			log("failed to create sensor_range_finder object. Did you start uOrb?");
 		}
 	}
@@ -469,8 +470,8 @@ ssize_t
 MB12XX::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 */
@@ -571,51 +572,20 @@ MB12XX::collect()
 
 	uint16_t distance = val[0] << 8 | val[1];
 	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
-	struct range_finder_report report;
+	struct distance_sensor_s report;
 
-	/* 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);
-
-	/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
-	if (addr_ind.size() == 1) {
-		report.distance = si_units;
-
-		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
-			report.distance_vector[i] = 0;
-		}
-
-		report.just_updated = 0;
-
-	} else {
-		/* for multiple sonars connected */
-
-		/* don't use the orginial single sonar variable */
-		report.distance = 0;
-
-		/* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values 			of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest 			value for each connected sonar */
-		_latest_sonar_measurements[_cycle_counter] = si_units;
-
-		for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
-			report.distance_vector[i] = _latest_sonar_measurements[i];
-		}
-
-		/* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been 		collected as this could be of use for Kalman filters */
-		report.just_updated = _index_counter;
-
-		/* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
-		for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
-			report.distance_vector[addr_ind.size() + i] = 0;
-		}
-	}
-
-	report.minimum_distance = get_minimum_distance();
-	report.maximum_distance = get_maximum_distance();
-	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	report.time_boot_ms = hrt_absolute_time();
+	report.id = 1;
+	report.type = 1;
+	report.orientation = 8;
+	report.current_distance = si_units;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.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)) {
@@ -840,7 +810,7 @@ void stop()
 void
 test()
 {
-	struct range_finder_report report;
+	struct distance_sensor_s report;
 	ssize_t sz;
 	int ret;
 
@@ -858,8 +828,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated);
-	warnx("time:        %lld", report.timestamp);
+	warnx("time:        %d", report.time_boot_ms);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -887,13 +856,7 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-
-		/* Print the sonar rangefinder report sonar distance vector */
-		for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) {
-			warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1);
-		}
-
-		warnx("time:        %lld", report.timestamp);
+		warnx("time:        %d", report.time_boot_ms);
 	}
 
 	/* reset the sensor polling to default rate */

src/drivers/sf0x/sf0x.cpp

@@ -69,6 +69,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>
 
@@ -128,7 +129,7 @@ private:
 	enum SF0X_PARSE_STATE		_parse_state;
 	hrt_abstime			_last_read;
 
-	orb_advert_t			_range_finder_topic;
+	orb_advert_t			_distance_sensor_topic;
 
 	unsigned			_consecutive_fail_count;
 
@@ -194,7 +195,7 @@ SF0X::SF0X(const char *port) :
 	_linebuf_index(0),
 	_parse_state(SF0X_PARSE_STATE0_UNSYNC),
 	_last_read(0),
-	_range_finder_topic(-1),
+	_distance_sensor_topic(-1),
 	_consecutive_fail_count(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
@@ -270,7 +271,7 @@ SF0X::init()
 		if (ret != OK) break;
 
 		/* allocate basic report buffers */
-		_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
+		_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
 		if (_reports == nullptr) {
 			warnx("mem err");
 			ret = -1;
@@ -278,11 +279,11 @@ SF0X::init()
 		}
 
 		/* get a publish handle on the range finder topic */
-		struct range_finder_report zero_report;
+		struct distance_sensor_s zero_report;
 		memset(&zero_report, 0, sizeof(zero_report));
-		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
+		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report);
 
-		if (_range_finder_topic < 0) {
+		if (_distance_sensor_topic < 0) {
 			warnx("advert err");
 		}
 	} while(0);
@@ -442,8 +443,8 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 SF0X::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 */
@@ -571,18 +572,19 @@ SF0X::collect()
 
 	debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
 
-	struct range_finder_report report;
+	struct distance_sensor_s report;
 
-	/* 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();
-	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
+	report.time_boot_ms = hrt_absolute_time();
+	report.id = 2;
+	report.type = 0;
+	report.orientation = 8;
+	report.current_distance = si_units;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0;
 
 	/* publish it */
-	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
 
 	if (_reports->force(&report)) {
 		perf_count(_buffer_overflows);
@@ -815,7 +817,7 @@ void stop()
 void
 test()
 {
-	struct range_finder_report report;
+	struct distance_sensor_s report;
 	ssize_t sz;
 
 	int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
@@ -832,8 +834,8 @@ test()
 	}
 
 	warnx("single read");
-	warnx("val:  %0.2f m", (double)report.distance);
-	warnx("time: %lld", report.timestamp);
+	warnx("val:  %0.2f m", (double)report.current_distance);
+	warnx("time: %d", report.time_boot_ms);
 
 	/* start the sensor polling at 2 Hz rate */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -863,8 +865,8 @@ test()
 		}
 
 		warnx("read #%u", i);
-		warnx("val:  %0.3f m", (double)report.distance);
-		warnx("time: %lld", report.timestamp);
+		warnx("val:  %0.3f m", (double)report.current_distance);
+		warnx("time: %d", report.time_boot_ms);
 	}
 
 	/* reset the sensor polling to the default rate */

src/drivers/trone/trone.cpp

@@ -68,6 +68,7 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>
 
@@ -128,7 +129,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;
@@ -209,7 +210,7 @@ static const uint8_t crc_table[] = {
 	0xfa, 0xfd, 0xf4, 0xf3
 };
 
-static uint8_t crc8(uint8_t *p, uint8_t len) {
+/* static uint8_t crc8(uint8_t *p, uint8_t len) {
 	uint16_t i;
 	uint16_t crc = 0x0;
 
@@ -219,7 +220,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) {
 	}
 
 	return crc & 0xFF;
-}
+}*/
 
 /*
  * Driver 'main' command.
@@ -235,7 +236,7 @@ TRONE::TRONE(int bus, int address) :
 	_measure_ticks(0),
 	_collect_phase(false),
 	_class_instance(-1),
-	_range_finder_topic(-1),
+	_distance_sensor_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
@@ -281,7 +282,7 @@ TRONE::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
 
 	if (_reports == nullptr) {
 		goto out;
@@ -291,12 +292,12 @@ TRONE::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 rf_report;
 		measure();
 		_reports->get(&rf_report);
-		_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
+		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
 
-		if (_range_finder_topic < 0) {
+		if (_distance_sensor_topic < 0) {
 			debug("failed to create sensor_range_finder object. Did you start uOrb?");
 		}
 	}
@@ -468,8 +469,8 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 TRONE::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 */
@@ -568,20 +569,20 @@ TRONE::collect()
 
 	uint16_t distance = (val[0] << 8) | val[1];
 	float si_units = distance *  0.001f; /* mm to m */
-	struct range_finder_report report;
-
-	/* 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();
-	report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	struct distance_sensor_s report;
 
+	report.time_boot_ms = hrt_absolute_time();
+	report.id = 3;
+	report.type = 0;
+	report.orientation = 8;
+	report.current_distance = si_units;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.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)) {
@@ -787,7 +788,7 @@ void stop()
 void
 test()
 {
-	struct range_finder_report report;
+	struct distance_sensor_s report;
 	ssize_t sz;
 	int ret;
 
@@ -805,8 +806,8 @@ test()
 	}
 
 	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.distance);
-	warnx("time:        %lld", report.timestamp);
+	warnx("measurement: %0.2f m", (double)report.current_distance);
+	warnx("time:        %d", report.time_boot_ms);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -834,9 +835,8 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("valid %u", report.valid);
-		warnx("measurement: %0.3f", (double)report.distance);
-		warnx("time:        %lld", report.timestamp);
+		warnx("measurement: %0.3f", (double)report.current_distance);
+		warnx("time:        %d", report.time_boot_ms);
 	}
 
 	/* reset the sensor polling to default rate */

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -58,13 +58,13 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/distance_sensor.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
-#include <drivers/drv_range_finder.h>
 
 #include <geo/geo.h>
 #include <systemlib/perf_counter.h>
@@ -162,7 +162,7 @@ private:
     struct vehicle_local_position_s     _local_pos;     /**< local vehicle position */
     struct vehicle_gps_position_s       _gps;           /**< GPS position */
     struct wind_estimate_s              _wind;          /**< wind estimate */
-    struct range_finder_report          _distance;      /**< distance estimate */
+    struct distance_sensor_s            _distance;      /**< distance estimate */
     struct vehicle_land_detected_s      _landDetector;
     struct actuator_armed_s             _armed;
 

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	/*
 	 * do subscriptions
 	 */
-	_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
+	_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
 	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -1471,11 +1471,11 @@ void AttitudePositionEstimatorEKF::pollData()
 	orb_check(_distance_sub, &_newRangeData);
 
 	if (_newRangeData) {
-		orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
-
-		if (_distance.valid) {
-			_ekf->rngMea = _distance.distance;
-			_distance_last_valid = _distance.timestamp;
+		orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
+		if ((_distance.current_distance > _distance.min_distance)
+				&& (_distance.current_distance < _distance.max_distance)) {
+			_ekf->rngMea = _distance.current_distance;
+			_distance_last_valid = _distance.time_boot_ms;
 
 		} else {
 			_newRangeData = false;

src/modules/mavlink/mavlink_messages.cpp

@@ -68,9 +68,9 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/distance_sensor.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
-#include <drivers/drv_range_finder.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
 
@@ -2178,7 +2178,6 @@ protected:
 	}
 };
 
-
 class MavlinkStreamDistanceSensor : public MavlinkStream
 {
 public:
@@ -2204,12 +2203,12 @@ public:
 
 	unsigned get_size()
 	{
-		return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	MavlinkOrbSubscription *_range_sub;
-	uint64_t _range_time;
+	MavlinkOrbSubscription *_distance_sensor_sub;
+	uint64_t _dist_sensor_time;
 
 	/* do not allow top copying this class */
 	MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
@@ -2217,36 +2216,44 @@ private:
 
 protected:
 	explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))),
-		_range_time(0)
+		_distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))),
+		_dist_sensor_time(0)
 	{}
 
 	void send(const hrt_abstime t)
 	{
-		struct range_finder_report range;
+		struct distance_sensor_s dist_sensor;
 
-		if (_range_sub->update(&_range_time, &range)) {
+		if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) {
 
 			mavlink_distance_sensor_t msg;
 
-			msg.time_boot_ms = range.timestamp / 1000;
+			msg.time_boot_ms = dist_sensor.time_boot_ms;
 
-			switch (range.type) {
-			case RANGE_FINDER_TYPE_LASER:
+			switch (dist_sensor.type) {
+			case MAV_DISTANCE_SENSOR_ULTRASOUND:
+				msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND;
+				break;
+
+			case MAV_DISTANCE_SENSOR_LASER:
 				msg.type = MAV_DISTANCE_SENSOR_LASER;
 				break;
 
+			case MAV_DISTANCE_SENSOR_INFRARED:
+				msg.type = MAV_DISTANCE_SENSOR_INFRARED;
+				break;
+
 			default:
 				msg.type = MAV_DISTANCE_SENSOR_LASER;
 				break;
 			}
 
-			msg.id = 0;
-			msg.orientation = 0;
-			msg.min_distance = range.minimum_distance * 100;
-			msg.max_distance = range.maximum_distance * 100;
-			msg.current_distance = range.distance * 100;
-			msg.covariance = 20;
+			msg.id = dist_sensor.id;
+			msg.orientation = dist_sensor.orientation;
+			msg.min_distance = dist_sensor.min_distance;
+			msg.max_distance = dist_sensor.max_distance;
+			msg.current_distance = dist_sensor.current_distance;
+			msg.covariance = dist_sensor.covariance;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
 		}

src/modules/mavlink/mavlink_receiver.cpp

@@ -56,7 +56,6 @@
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
-#include <drivers/drv_range_finder.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_battery_pub(-1),
 	_cmd_pub(-1),
 	_flow_pub(-1),
-	_range_pub(-1),
+	_distance_sensor_pub(-1),
 	_offboard_control_mode_pub(-1),
 	_actuator_controls_pub(-1),
 	_global_vel_sp_pub(-1),
@@ -211,6 +210,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_timesync(msg);
 		break;
 
+	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
+		handle_message_distance_sensor(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -412,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
 	}
+
+	/* Use distance value for distance sensor topic */
+	struct distance_sensor_s d;
+	memset(&d, 0, sizeof(d));
+
+	d.time_boot_ms = hrt_absolute_time();
+	d.min_distance = 0.3f;
+	d.max_distance = 5.0f;
+	d.current_distance = flow.distance;
+	d.type = 1;
+	d.id = 0;
+	d.orientation = 8;
+	d.covariance = 0.0;
+
+	if (_distance_sensor_pub < 0) {
+		_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
+	}
 }
 
 void
@@ -444,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 		orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
 	}
 
-	/* Use distance value for range finder report */
-	struct range_finder_report r;
-	memset(&r, 0, sizeof(r));
+	/* Use distance value for distance sensor topic */
+	struct distance_sensor_s d;
+	memset(&d, 0, sizeof(d));
 
-	r.timestamp = hrt_absolute_time();
-	r.error_count = 0;
-	r.type = RANGE_FINDER_TYPE_LASER;
-	r.distance = flow.distance;
-	r.minimum_distance = 0.0f;
-	r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
-	r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
+	d.time_boot_ms = hrt_absolute_time();
+	d.min_distance = 0.3f;
+	d.max_distance = 5.0f;
+	d.current_distance = flow.distance;
+	d.type = 1;
+	d.id = 0;
+	d.orientation = 8;
+	d.covariance = 0.0;
 
-	if (_range_pub < 0) {
-		_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
+	if (_distance_sensor_pub < 0) {
+		_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
 	} else {
-		orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
 	}
 }
 
@@ -497,6 +520,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
+{
+	/* distance sensor */
+	mavlink_distance_sensor_t dist_sensor;
+	mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
+
+	struct distance_sensor_s d;
+	memset(&d, 0, sizeof(d));
+
+	d.time_boot_ms = dist_sensor.time_boot_ms;
+	d.min_distance = dist_sensor.min_distance;
+	d.max_distance = dist_sensor.max_distance;
+	d.current_distance = dist_sensor.current_distance;
+	d.type = dist_sensor.type;
+	d.id = dist_sensor.id;
+	d.orientation = dist_sensor.orientation;
+	d.covariance = dist_sensor.covariance;
+
+	/// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum
+
+	if (_distance_sensor_pub < 0) {
+		_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
+
+	} else {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
+	}
+}
+
 void
 MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -73,6 +73,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_force_setpoint.h>
 #include <uORB/topics/time_offset.h>
+#include <uORB/topics/distance_sensor.h>
 
 #include "mavlink_ftp.h"
 
@@ -134,6 +135,7 @@ private:
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
+	void handle_message_distance_sensor(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
@@ -164,7 +166,7 @@ private:
 	orb_advert_t _battery_pub;
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
-	orb_advert_t _range_pub;
+	orb_advert_t _distance_sensor_pub;
 	orb_advert_t _offboard_control_mode_pub;
 	orb_advert_t _actuator_controls_pub;
 	orb_advert_t _global_vel_sp_pub;

src/modules/sdlog2/sdlog2.c

@@ -61,8 +61,6 @@
 #include <math.h>
 #include <time.h>
 
-#include <drivers/drv_range_finder.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
@@ -92,6 +90,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/system_power.h>
@@ -1084,7 +1083,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 		struct battery_status_s battery;
 		struct telemetry_status_s telemetry;
-		struct range_finder_report range_finder;
+		struct distance_sensor_s distance_sensor;
 		struct estimator_status_report estimator_status;
 		struct tecs_status_s tecs_status;
 		struct system_power_s system_power;
@@ -1174,7 +1173,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int global_vel_sp_sub;
 		int battery_sub;
 		int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
-		int range_finder_sub;
+		int distance_sensor_sub;
 		int estimator_status_sub;
 		int tecs_status_sub;
 		int system_power_sub;
@@ -1208,7 +1207,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.esc_sub = -1;
 	subs.global_vel_sp_sub = -1;
 	subs.battery_sub = -1;
-	subs.range_finder_sub = -1;
+	subs.distance_sensor_sub = -1;
 	subs.estimator_status_sub = -1;
 	subs.tecs_status_sub = -1;
 	subs.system_power_sub = -1;
@@ -1820,12 +1819,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* --- BOTTOM DISTANCE --- */
-		if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) {
+		/* --- DISTANCE SENSOR --- */
+		if (copy_if_updated(ORB_ID(distance_sensor), &subs.distance_sensor_sub, &buf.distance_sensor)) {
 			log_msg.msg_type = LOG_DIST_MSG;
-			log_msg.body.log_DIST.bottom = buf.range_finder.distance;
-			log_msg.body.log_DIST.bottom_rate = 0.0f;
-			log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
+			log_msg.body.log_DIST.id = buf.distance_sensor.id;
+			log_msg.body.log_DIST.type = buf.distance_sensor.type;
+			log_msg.body.log_DIST.orientation = buf.distance_sensor.orientation;
+			log_msg.body.log_DIST.current_distance = buf.distance_sensor.current_distance;
+			log_msg.body.log_DIST.covariance = buf.distance_sensor.covariance;
 			LOGBUFFER_WRITE_AND_COUNT(DIST);
 		}
 

src/modules/sdlog2/sdlog2_messages.h

@@ -295,12 +295,14 @@ struct log_BATT_s {
 	float discharged;
 };
 
-/* --- DIST - DISTANCE TO SURFACE --- */
+/* --- DIST - RANGE SENSOR DISTANCE --- */
 #define LOG_DIST_MSG 21
 struct log_DIST_s {
-	float bottom;
-	float bottom_rate;
-	uint8_t flags;
+	uint8_t id;
+	uint8_t type;
+	uint8_t orientation;
+	uint16_t current_distance;
+	uint8_t covariance;
 };
 
 /* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */

src/modules/uORB/objects_common.cpp

@@ -57,9 +57,6 @@ ORB_DEFINE(sensor_gyro, struct gyro_report);
 #include <drivers/drv_baro.h>
 ORB_DEFINE(sensor_baro, struct baro_report);
 
-#include <drivers/drv_range_finder.h>
-ORB_DEFINE(sensor_range_finder, struct range_finder_report);
-
 #include <drivers/drv_pwm_output.h>
 ORB_DEFINE(output_pwm, struct pwm_output_values);
 
@@ -259,3 +256,6 @@ ORB_DEFINE(time_offset, struct time_offset_s);
 
 #include "topics/mc_att_ctrl_status.h"
 ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s);
+
+#include "topics/distance_sensor.h"
+ORB_DEFINE(distance_sensor, struct distance_sensor_s);