distance_sensor: adopt message in both range drivers and on ekf_att_p…

…os_estimator

src/drivers/drv_range_finder.h

@@ -48,14 +48,10 @@
 #define RANGE_FINDER0_DEVICE_PATH	"/dev/range_finder0"
 #define MB12XX_MAX_RANGEFINDERS	12	//Maximum number of RangeFinders that can be connected
 
-#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/ll40ls/ll40ls.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>
 
@@ -144,7 +145,7 @@ class LL40LS : public device::I2C
 	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;
@@ -224,7 +225,7 @@ LL40LS::LL40LS(int bus, const char *path, 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, "ll40ls_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")),
@@ -278,7 +279,7 @@ LL40LS::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer(2, sizeof(range_finder_report));
+	_reports = new RingBuffer(2, sizeof(distance_sensor_s));
 
 	if (_reports == nullptr) {
 		goto out;
@@ -288,12 +289,12 @@ LL40LS::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?");
 		}
 	}
@@ -497,8 +498,8 @@ LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg)
 ssize_t
 LL40LS::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 */
@@ -664,8 +665,8 @@ LL40LS::collect()
 	}
 
 	uint16_t distance = (val[0] << 8) | val[1];
-	float si_units = distance * 0.01f; /* cm to m */
-	struct range_finder_report report;
+	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
+	struct distance_sensor_s report;
 
 	if (distance == 0) {
 		_zero_counter++;
@@ -688,22 +689,18 @@ LL40LS::collect()
 
 	_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);
-	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.time_boot_ms = hrt_absolute_time();
+	report.id = 0;
+	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)) {
@@ -958,7 +955,7 @@ void stop(int bus)
 void
 test(int bus)
 {
-	struct range_finder_report report;
+	struct distance_sensor_s report;
 	ssize_t sz;
 	int ret;
 	const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
@@ -977,8 +974,8 @@ test(int bus)
 	}
 
 	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)) {
@@ -1006,8 +1003,8 @@ test(int bus)
 		}
 
 		warnx("periodic read %u", i);
-		warnx("measurement: %0.3f", (double)report.distance);
-		warnx("time:        %lld", report.timestamp);
+		warnx("measurement: %0.3f m", (double)report.current_distance);
+		warnx("time:        %d", report.time_boot_ms);
 	}
 
 	/* reset the sensor polling to default rate */

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 @@ class MB12XX : public device::I2C
 	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(2, sizeof(range_finder_report));
+	_reports = new 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 = 0;
+	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/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 @@ class AttitudePositionEstimatorEKF
     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

@@ -71,7 +71,6 @@
 #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>