distance_sensor: adopt message in both range drivers and on ekf_att_pos_estimator

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

@@ -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>
 

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),
@@ -134,8 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_att_sp{},
 	_rates_sp{},
 	_time_offset_avg_alpha(0.6),
-	_time_offset(0),
-	_distance_sensor_pub(-1)
+	_time_offset(0)
 {
 
 }
@@ -417,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 = 3.0f;
+	d.max_distance = 50.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
@@ -449,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 = 3.0f;
+	d.max_distance = 50.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);
 	}
 }
 

src/modules/mavlink/mavlink_receiver.h

@@ -166,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;
@@ -192,7 +192,6 @@ private:
 	struct vehicle_rates_setpoint_s _rates_sp;
 	double _time_offset_avg_alpha;
 	uint64_t _time_offset;
-	orb_advert_t _distance_sensor_pub;
 
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver &);

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);