distance_sensor: changed all distance sensors to orb_advertise_multi

src/drivers/ll40ls/LidarLiteI2C.cpp

@@ -61,6 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
 	_sensor_ok(false),
 	_collect_phase(false),
 	_class_instance(-1),
+	_orb_class_instance(-1),
 	_distance_sensor_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")),
@@ -128,7 +129,8 @@ int LidarLiteI2C::init()
 		struct distance_sensor_s ds_report;
 		measure();
 		_reports->get(&ds_report);
-		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+							     &_orb_class_instance, ORB_PRIO_LOW);
 
 		if (_distance_sensor_topic < 0) {
 			debug("failed to create distance_sensor object. Did you start uOrb?");

src/drivers/ll40ls/LidarLiteI2C.h

@@ -103,6 +103,7 @@ private:
 	bool                _sensor_ok;
 	bool                _collect_phase;
 	int                 _class_instance;
+	int		    _orb_class_instance;
 
 	orb_advert_t        _distance_sensor_topic;
 

src/drivers/ll40ls/LidarLitePWM.cpp

@@ -58,6 +58,7 @@ LidarLitePWM::LidarLitePWM(const char *path) :
 	_work{},
 	_reports(nullptr),
 	_class_instance(-1),
+	_orb_class_instance(-1),
 	_pwmSub(-1),
 	_pwm{},
 	_distance_sensor_topic(-1),
@@ -112,7 +113,8 @@ int LidarLitePWM::init()
 		struct distance_sensor_s ds_report;
 		measure();
 		_reports->get(&ds_report);
-		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+							     &_orb_class_instance, ORB_PRIO_LOW);
 
 		if (_distance_sensor_topic < 0) {
 			debug("failed to create distance_sensor object. Did you start uOrb?");

src/drivers/ll40ls/LidarLitePWM.h

@@ -108,6 +108,7 @@ private:
 	work_s			_work;
 	ringbuffer::RingBuffer	*_reports;
 	int			_class_instance;
+	int			_orb_class_instance;
 	int			_pwmSub;
 	struct pwm_input_s	_pwm;
 	orb_advert_t	        _distance_sensor_topic;

src/drivers/mb12xx/mb12xx.cpp

@@ -130,7 +130,8 @@ private:
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
-	int					_class_instance;
+	int				_class_instance;
+	int				_orb_class_instance;
 
 	orb_advert_t		_distance_sensor_topic;
 
@@ -209,6 +210,7 @@ MB12XX::MB12XX(int bus, int address) :
 	_measure_ticks(0),
 	_collect_phase(false),
 	_class_instance(-1),
+	_orb_class_instance(-1),
 	_distance_sensor_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
@@ -269,9 +271,10 @@ MB12XX::init()
 
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
 		/* get a publish handle on the range finder topic */
-		struct distance_sensor_s rf_report = {};
+		struct distance_sensor_s ds_report = {};
 
-		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+							     &_orb_class_instance, ORB_PRIO_LOW);
 
 		if (_distance_sensor_topic < 0) {
 			log("failed to create sensor_range_finder object. Did you start uOrb?");

src/drivers/px4flow/px4flow.cpp

@@ -67,6 +67,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_px4flow.h>
+#include <drivers/drv_range_finder.h>
 #include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
@@ -125,10 +126,12 @@ protected:
 private:
 
 	work_s				_work;
-	ringbuffer::RingBuffer			*_reports;
+	ringbuffer::RingBuffer		*_reports;
 	bool				_sensor_ok;
-	int					_measure_ticks;
+	int				_measure_ticks;
 	bool				_collect_phase;
+	int			_class_instance;
+	int			_orb_class_instance;
 	orb_advert_t		_px4flow_topic;
 	orb_advert_t		_distance_sensor_topic;
 
@@ -188,6 +191,8 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
 	_sensor_ok(false),
 	_measure_ticks(0),
 	_collect_phase(false),
+	_class_instance(-1),
+	_orb_class_instance(-1),
 	_px4flow_topic(-1),
 	_distance_sensor_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
@@ -195,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
 	_sensor_rotation(rotation)
 {
-	// enable debug() calls
+	// disable debug() calls
 	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
@@ -230,6 +235,20 @@ PX4FLOW::init()
 		goto out;
 	}
 
+	_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 distance_sensor_s ds_report = {};
+
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+							     &_orb_class_instance, ORB_PRIO_HIGH);
+
+		if (_distance_sensor_topic < 0) {
+			log("failed to create sensor_range_finder object. Did you start uOrb?");
+		}
+	}
+
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
@@ -518,12 +537,7 @@ PX4FLOW::collect()
 	distance_report.id = 0;
 	distance_report.orientation = 8;
 
-	if (_distance_sensor_topic < 0) {
+	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
-		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &distance_report);
-	} else {
-		/* publish it */
-		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
-	}
 
 	/* post a report to the ring */
 	if (_reports->force(&report)) {

src/drivers/sf0x/sf0x.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -119,7 +119,7 @@ private:
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
-	ringbuffer::RingBuffer			*_reports;
+	ringbuffer::RingBuffer		*_reports;
 	bool				_sensor_ok;
 	int				_measure_ticks;
 	bool				_collect_phase;
@@ -129,6 +129,9 @@ private:
 	enum SF0X_PARSE_STATE		_parse_state;
 	hrt_abstime			_last_read;
 
+	int				_class_instance;
+	int				_orb_class_instance;
+
 	orb_advert_t			_distance_sensor_topic;
 
 	unsigned			_consecutive_fail_count;
@@ -195,6 +198,8 @@ SF0X::SF0X(const char *port) :
 	_linebuf_index(0),
 	_parse_state(SF0X_PARSE_STATE0_UNSYNC),
 	_last_read(0),
+	_class_instance(-1),
+	_orb_class_instance(-1),
 	_distance_sensor_topic(-1),
 	_consecutive_fail_count(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
@@ -256,6 +261,14 @@ SF0X::~SF0X()
 	if (_reports != nullptr) {
 		delete _reports;
 	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_buffer_overflows);
 }
 
 int
@@ -278,14 +291,20 @@ SF0X::init()
 			break;
 		}
 
-		/* get a publish handle on the range finder topic */
+		_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
-		struct distance_sensor_s zero_report;
-		memset(&zero_report, 0, sizeof(zero_report));
-		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report);
 
-		if (_distance_sensor_topic < 0) {
+		if (_class_instance == CLASS_DEVICE_PRIMARY) {
-			warnx("advert err");
+			/* get a publish handle on the range finder topic */
+			struct distance_sensor_s ds_report = {};
+
+			_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+								     &_orb_class_instance, ORB_PRIO_HIGH);
+
+			if (_distance_sensor_topic < 0) {
+				log("failed to create sensor_range_finder object. Did you start uOrb?");
+			}
 		}
+
 	} while(0);
 
 	/* close the fd */

src/drivers/trone/trone.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -127,7 +127,8 @@ private:
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
-	int					_class_instance;
+	int				_class_instance;
+	int				_orb_class_instance;
 
 	orb_advert_t		_distance_sensor_topic;
 
@@ -236,6 +237,7 @@ TRONE::TRONE(int bus, int address) :
 	_measure_ticks(0),
 	_collect_phase(false),
 	_class_instance(-1),
+	_orb_class_instance(-1),
 	_distance_sensor_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
@@ -292,13 +294,15 @@ TRONE::init()
 
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
 		/* get a publish handle on the range finder topic */
-		struct distance_sensor_s rf_report;
+		struct distance_sensor_s ds_report;
 		measure();
-		_reports->get(&rf_report);
+		_reports->get(&ds_report);
-		_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report);
+
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+							     &_orb_class_instance, ORB_PRIO_LOW);
 
 		if (_distance_sensor_topic < 0) {
-			debug("failed to create sensor_range_finder object. Did you start uOrb?");
+			log("failed to create sensor_range_finder object. Did you start uOrb?");
 		}
 	}