uORB::Publication improvements and cleanup (#14784)

- create common uORB::PublicationBase
 - uORB::PublicationQueued types are now type aliases
 - ORB_PRIO use enum type everywhere to avoid accidental misuse
 - PX4Accelerometer/PX4Gyroscope/etc driver libs explicitly advertise on construction, unadvertise on destruction. This is a workaround for any potential issues that might appear when accel/gyro cdev and uORB indexing doesn't align.

src/drivers/camera_capture/camera_capture.hpp

@@ -50,7 +50,6 @@
 #include <px4_platform_common/workqueue.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/vehicle_command.h>

src/drivers/camera_trigger/camera_trigger.cpp

@@ -56,7 +56,7 @@
 #include <parameters/param.h>
 #include <systemlib/mavlink_log.h>
 
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/camera_capture.h>

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -117,7 +117,7 @@ private:
 SF1XX::SF1XX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
 	I2C(DRV_DIST_DEVTYPE_SF1XX, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(DRV_DIST_DEVTYPE_SF1XX, rotation)
+	_px4_rangefinder(DRV_DIST_DEVTYPE_SF1XX, ORB_PRIO_DEFAULT, rotation)
 {
 }
 

src/drivers/gps/gps.cpp

@@ -53,7 +53,7 @@
 #include <px4_platform_common/cli.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/gps_dump.h>

src/drivers/imu/bmi055/BMI055_accel.cpp

@@ -47,7 +47,7 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER
 BMI055_accel::BMI055_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device,
 			   enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) :
 	BMI055(DRV_ACC_DEVTYPE_BMI055, "bmi055_accel", path_accel, bus_option, bus, device, spi_mode, bus_frequency, rotation),
-	_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
+	_px4_accel(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi055_accel_bad_registers")),

src/drivers/imu/bmi055/BMI055_gyro.cpp

@@ -52,7 +52,7 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]
 BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_gyro, uint32_t device,
 			 enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) :
 	BMI055(DRV_GYR_DEVTYPE_BMI055, "bmi055_gyro", path_gyro, bus_option, bus, device, spi_mode, bus_frequency, rotation),
-	_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
+	_px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers"))

src/drivers/imu/bmi088/BMI088_accel.cpp

@@ -56,7 +56,7 @@ BMI088_accel::BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path
 			   enum Rotation rotation,
 			   int bus_frequency, spi_mode_e spi_mode) :
 	BMI088("bmi088_accel", path_accel, bus_option, bus, DRV_ACC_DEVTYPE_BMI088, device, spi_mode, bus_frequency, rotation),
-	_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
+	_px4_accel(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")),

src/drivers/imu/bmi088/BMI088_gyro.cpp

@@ -60,7 +60,7 @@ BMI088_gyro::BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g
 			 enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) :
 	BMI088("bmi088_gyro", path_gyro, bus_option, bus, DRV_GYR_DEVTYPE_BMI088, device, spi_mode, bus_frequency,
 	       rotation),
-	_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
+	_px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
 	_bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers"))

src/drivers/px4io/px4io.cpp

@@ -80,7 +80,7 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>

src/drivers/safety_button/SafetyButton.hpp

@@ -43,7 +43,6 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>

src/drivers/telemetry/iridiumsbd/IridiumSBD.h

@@ -40,7 +40,6 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/subsystem_info.h>
 

src/drivers/uavcan/uavcan_servers.hpp

@@ -38,7 +38,6 @@
 #include <drivers/device/Device.hpp>
 #include <lib/perf/perf_counter.h>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/uavcan_parameter_value.h>
 #include <uORB/topics/vehicle_command_ack.h>
 

src/lib/avoidance/ObstacleAvoidance.hpp

@@ -47,7 +47,6 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/vehicle_command.h>

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -50,7 +50,6 @@
 #include <px4_platform_common/module_params.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/collision_constraints.h>
 #include <uORB/topics/distance_sensor.h>

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -63,7 +63,7 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit
 	return clip_count;
 }
 
-PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
+PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
 	CDev(nullptr),
 	_sensor_pub{ORB_ID(sensor_accel), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
@@ -72,7 +72,11 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
 	_device_id{device_id},
 	_rotation{rotation}
 {
+	// register class and advertise immediately to keep instance numbering in sync
 	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
+	_sensor_pub.advertise();
+	_sensor_integrated_pub.advertise();
+	_sensor_status_pub.advertise();
 }
 
 PX4Accelerometer::~PX4Accelerometer()
@@ -80,6 +84,11 @@ PX4Accelerometer::~PX4Accelerometer()
 	if (_class_device_instance != -1) {
 		unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _class_device_instance);
 	}
+
+	_sensor_pub.unadvertise();
+	_sensor_fifo_pub.unadvertise();
+	_sensor_integrated_pub.unadvertise();
+	_sensor_status_pub.unadvertise();
 }
 
 int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -40,7 +40,7 @@
 #include <lib/drivers/device/integrator.h>
 #include <lib/ecl/geo/geo.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/PublicationQueuedMulti.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_accel_fifo.h>
 #include <uORB/topics/sensor_accel_integrated.h>
@@ -49,7 +49,7 @@
 class PX4Accelerometer : public cdev::CDev
 {
 public:
-	PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
+	PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Accelerometer() override;
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;

src/lib/drivers/barometer/PX4Barometer.cpp

@@ -36,11 +36,12 @@
 
 #include <lib/drivers/device/Device.hpp>
 
-PX4Barometer::PX4Barometer(uint32_t device_id, uint8_t priority) :
+PX4Barometer::PX4Barometer(uint32_t device_id, ORB_PRIO priority) :
 	CDev(nullptr),
 	_sensor_baro_pub{ORB_ID(sensor_baro), priority}
 {
 	_class_device_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
+	_sensor_baro_pub.advertise();
 
 	_sensor_baro_pub.get().device_id = device_id;
 }
@@ -50,6 +51,8 @@ PX4Barometer::~PX4Barometer()
 	if (_class_device_instance != -1) {
 		unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_device_instance);
 	}
+
+	_sensor_baro_pub.unadvertise();
 }
 
 void

src/lib/drivers/barometer/PX4Barometer.hpp

@@ -45,7 +45,7 @@ class PX4Barometer : public cdev::CDev
 {
 
 public:
-	PX4Barometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT);
+	PX4Barometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT);
 	~PX4Barometer() override;
 
 	const sensor_baro_s &get() { return _sensor_baro_pub.get(); }

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -63,7 +63,7 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit
 	return clip_count;
 }
 
-PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
+PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
 	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_gyro), priority},
@@ -73,7 +73,11 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	_device_id{device_id},
 	_rotation{rotation}
 {
+	// register class and advertise immediately to keep instance numbering in sync
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
+	_sensor_pub.advertise();
+	_sensor_integrated_pub.advertise();
+	_sensor_status_pub.advertise();
 
 	updateParams();
 }
@@ -83,6 +87,11 @@ PX4Gyroscope::~PX4Gyroscope()
 	if (_class_device_instance != -1) {
 		unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_device_instance);
 	}
+
+	_sensor_pub.unadvertise();
+	_sensor_fifo_pub.unadvertise();
+	_sensor_integrated_pub.unadvertise();
+	_sensor_status_pub.unadvertise();
 }
 
 int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -40,7 +40,7 @@
 #include <lib/drivers/device/integrator.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/PublicationQueuedMulti.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_gyro_integrated.h>
@@ -49,7 +49,7 @@
 class PX4Gyroscope : public cdev::CDev, public ModuleParams
 {
 public:
-	PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
+	PX4Gyroscope(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Gyroscope() override;
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;

src/lib/drivers/magnetometer/PX4Magnetometer.cpp

@@ -36,13 +36,15 @@
 
 #include <lib/drivers/device/Device.hpp>
 
-PX4Magnetometer::PX4Magnetometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
+PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
 	CDev(nullptr),
 	_sensor_mag_pub{ORB_ID(sensor_mag), priority},
 	_rotation{rotation}
 {
 	_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
 
+	_sensor_mag_pub.advertise();
+
 	_sensor_mag_pub.get().device_id = device_id;
 	_sensor_mag_pub.get().scaling = 1.0f;
 	_sensor_mag_pub.get().temperature = NAN;
@@ -53,6 +55,8 @@ PX4Magnetometer::~PX4Magnetometer()
 	if (_class_device_instance != -1) {
 		unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance);
 	}
+
+	_sensor_mag_pub.unadvertise();
 }
 
 int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)

src/lib/drivers/magnetometer/PX4Magnetometer.hpp

@@ -45,7 +45,7 @@ class PX4Magnetometer : public cdev::CDev
 {
 
 public:
-	PX4Magnetometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
+	PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Magnetometer() override;
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -35,13 +35,20 @@
 
 #include <lib/drivers/device/Device.hpp>
 
-PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t priority, const uint8_t device_orientation) :
+PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const ORB_PRIO priority, const uint8_t device_orientation) :
 	_distance_sensor_pub{ORB_ID(distance_sensor), priority}
 {
+	_distance_sensor_pub.advertise();
+
 	set_device_id(device_id);
 	set_orientation(device_orientation);
 }
 
+PX4Rangefinder::~PX4Rangefinder()
+{
+	_distance_sensor_pub.unadvertise();
+}
+
 void PX4Rangefinder::set_device_type(uint8_t device_type)
 {
 	// TODO: range finders should have device ids

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -44,9 +44,9 @@ class PX4Rangefinder
 
 public:
 	PX4Rangefinder(const uint32_t device_id,
-		       const uint8_t priority = ORB_PRIO_DEFAULT,
+		       const ORB_PRIO priority = ORB_PRIO_DEFAULT,
 		       const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
-	~PX4Rangefinder() = default;
+	~PX4Rangefinder();
 
 	void print_status();
 

src/lib/flight_tasks/FlightTasks.hpp

@@ -44,7 +44,7 @@
 #include "FlightTask.hpp"
 #include "FlightTasks_generated.hpp"
 #include <lib/weather_vane/WeatherVane.hpp>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_command.h>

src/lib/mixer_module/mixer_module.cpp

@@ -35,7 +35,7 @@
 
 #include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
 
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <px4_platform_common/log.h>
 
 using namespace time_literals;

src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp

@@ -39,7 +39,7 @@
 #include <px4_platform_common/px4_config.h>
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 

src/modules/commander/Commander.hpp

@@ -46,7 +46,6 @@
 
 // publications
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/test_motor.h>

src/modules/commander/accelerometer_calibration.cpp

@@ -556,7 +556,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 
 		if (device_id[cur_accel] != 0) {
 			// Get priority
-			int32_t prio;
+			ORB_PRIO prio = ORB_PRIO_UNINITIALIZED;
 			orb_priority(worker_data.subs[cur_accel], &prio);
 
 			if (prio > device_prio_max) {

src/modules/commander/gyro_calibration.cpp

@@ -332,7 +332,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 		if (worker_data.device_id[cur_gyro] != 0) {
 			// Get priority
-			int32_t prio;
+			ORB_PRIO prio = ORB_PRIO_UNINITIALIZED;
 			orb_priority(worker_data.gyro_sensor_sub[cur_gyro], &prio);
 
 			if (prio > device_prio_max) {

src/modules/commander/mag_calibration.cpp

@@ -639,7 +639,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 			if (device_ids[cur_mag] != 0) {
 				// Get priority
-				int32_t prio;
+				ORB_PRIO prio = ORB_PRIO_UNINITIALIZED;
 				orb_priority(worker_data.sub_mag[cur_mag], &prio);
 
 				if (prio > device_prio_max) {

src/modules/events/send_event.h

@@ -39,7 +39,7 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 

src/modules/events/status_display.h

@@ -42,7 +42,7 @@
 
 #include <drivers/drv_hrt.h>
 
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>

src/modules/load_mon/load_mon.cpp

@@ -48,7 +48,6 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <systemlib/cpuload.h>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/task_stack_info.h>
 

src/modules/logger/log_writer_mavlink.h

@@ -34,7 +34,7 @@
 #pragma once
 
 #include <stdint.h>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/ulog_stream.h>
 #include <uORB/topics/ulog_stream_ack.h>
 

src/modules/logger/logger.cpp

@@ -45,7 +45,7 @@
 #include <stdlib.h>
 #include <time.h>
 
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/uORBTopics.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_command_ack.h>

src/modules/mavlink/mavlink_main.cpp

@@ -51,7 +51,7 @@
 #include <lib/ecl/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/version/version.h>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 
 #include "mavlink_receiver.h"
 #include "mavlink_main.h"

src/modules/mavlink/mavlink_main.h

@@ -71,7 +71,7 @@
 #include <px4_platform_common/posix.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/uthash/utlist.h>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/radio_status.h>

src/modules/mavlink/mavlink_parameters.h

@@ -46,7 +46,6 @@
 
 #include "mavlink_bridge_header.h"
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rc_parameter_map.h>
 #include <uORB/topics/uavcan_parameter_request.h>

src/modules/mavlink/mavlink_receiver.h

@@ -54,7 +54,6 @@
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>

src/modules/mavlink/tune_publisher.h

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/tunes/tunes.h>
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -54,7 +54,6 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/home_position.h>

src/modules/muorb/test/muorb_test_example.cpp

@@ -44,7 +44,6 @@
 #include <stdio.h>
 #include <px4_platform_common/defines.h>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 
 px4::AppState MuorbTestExample::appState;

src/modules/navigator/navigator.h

@@ -58,7 +58,7 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/home_position.h>

src/modules/sensors/voted_sensors_update.cpp

@@ -154,7 +154,7 @@ void VotedSensorsUpdate::parametersUpdate()
 				_gyro.enabled[driver_index] = (device_enabled == 1);
 
 				if (!_gyro.enabled[driver_index]) {
-					_gyro.priority[driver_index] = 0;
+					_gyro.priority[driver_index] = ORB_PRIO_UNINITIALIZED;
 				}
 
 				gyro_calibration_s gscale{};
@@ -250,7 +250,7 @@ void VotedSensorsUpdate::parametersUpdate()
 				_accel.enabled[driver_index] = (device_enabled == 1);
 
 				if (!_accel.enabled[driver_index]) {
-					_accel.priority[driver_index] = 0;
+					_accel.priority[driver_index] = ORB_PRIO_UNINITIALIZED;
 				}
 
 				accel_calibration_s ascale{};
@@ -387,7 +387,7 @@ void VotedSensorsUpdate::parametersUpdate()
 				// the mags that were published after the initial parameterUpdate
 				// would be given the priority even if disabled. Reset it to 0 in this case
 				if (!_mag.enabled[topic_instance]) {
-					_mag.priority[topic_instance] = 0;
+					_mag.priority[topic_instance] = ORB_PRIO_UNINITIALIZED;
 				}
 
 				mag_calibration_s mscale{};
@@ -702,14 +702,16 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
 						      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
 
 				// reduce priority of failed sensor to the minimum
-				sensor.priority[failover_index] = 1;
+				sensor.priority[failover_index] = ORB_PRIO_MIN;
 
 				PX4_ERR("Sensor %s #%i failed. Reconfiguring sensor priorities.", sensor_name, failover_index);
 
 				int ctr_valid = 0;
 
 				for (uint8_t i = 0; i < sensor.subscription_count; i++) {
-					if (sensor.priority[i] > 1) { ctr_valid++; }
+					if (sensor.priority[i] > ORB_PRIO_MIN) {
+						ctr_valid++;
+					}
 
 					PX4_WARN("Remaining sensors after failover event %u: %s #%u priority: %u", failover_index, sensor_name, i,
 						 sensor.priority[i]);

src/modules/sensors/voted_sensors_update.h

@@ -54,7 +54,6 @@
 #include <lib/mag_compensation/MagCompensation.hpp>
 
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_accel_integrated.h>
 #include <uORB/topics/sensor_combined.h>
@@ -156,7 +155,7 @@ private:
 		uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
 		DataValidatorGroup voter{1};
 		unsigned int last_failover_count{0};
-		uint8_t priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
+		ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
 		uint8_t last_best_vote{0}; /**< index of the latest best vote */
 		uint8_t subscription_count{0};
 		bool enabled[SENSOR_COUNT_MAX] {true, true, true, true};

src/modules/temperature_compensation/TemperatureCompensationModule.cpp

@@ -35,7 +35,7 @@
 
 #include "temperature_calibration/temperature_calibration.h"
 
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_command.h>
 
 #include <systemlib/mavlink_log.h>

src/modules/temperature_compensation/temperature_calibration/task.cpp

@@ -39,7 +39,7 @@
  * @author Beat Küng <beat-kueng@gmx.net>
  */
 
-#include <uORB/PublicationQueued.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <mathlib/mathlib.h>
 #include <px4_platform_common/atomic.h>

src/modules/uORB/CMakeLists.txt

@@ -43,8 +43,6 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
 			ORBSet.hpp
 			Publication.hpp
 			PublicationMulti.hpp
-			PublicationQueued.hpp
-			PublicationQueuedMulti.hpp
 			Subscription.cpp
 			Subscription.hpp
 			SubscriptionCallback.hpp

src/modules/uORB/Publication.hpp

@@ -40,16 +40,47 @@
 
 #include <px4_platform_common/defines.h>
 #include <systemlib/err.h>
+
 #include <uORB/uORB.h>
+#include "uORBDeviceNode.hpp"
+#include <uORB/topics/uORBTopics.hpp>
 
 namespace uORB
 {
 
+class PublicationBase
+{
+public:
+
+	bool advertised() const { return _handle != nullptr; }
+
+	bool unadvertise() { return (DeviceNode::unadvertise(_handle) == PX4_OK); }
+
+	orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
+
+protected:
+
+	PublicationBase(ORB_ID id) : _orb_id(id) {}
+
+	~PublicationBase()
+	{
+		if (_handle != nullptr) {
+			// don't automatically unadvertise queued publications (eg vehicle_command)
+			if (static_cast<DeviceNode *>(_handle)->get_queue_size() == 1) {
+				unadvertise();
+			}
+		}
+	}
+
+	orb_advert_t _handle{nullptr};
+	const ORB_ID _orb_id;
+};
+
 /**
- * Base publication wrapper class
+ * uORB publication wrapper class
  */
-template<typename T>
+template<typename T, uint8_t ORB_QSIZE = 1>
-class Publication
+class Publication : public PublicationBase
 {
 public:
 
@@ -58,8 +89,17 @@ public:
 	 *
 	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
 	 */
-	Publication(const orb_metadata *meta) : _meta(meta) {}
+	Publication(ORB_ID id) : PublicationBase(id) {}
-	~Publication() { orb_unadvertise(_handle); }
+	Publication(const orb_metadata *meta) : PublicationBase(static_cast<ORB_ID>(meta->o_id)) {}
+
+	bool advertise()
+	{
+		if (!advertised()) {
+			_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
+		}
+
+		return advertised();
+	}
 
 	/**
 	 * Publish the struct
@@ -67,25 +107,12 @@ public:
 	 */
 	bool publish(const T &data)
 	{
-		if (_handle != nullptr) {
+		if (!advertised()) {
-			return (orb_publish(_meta, _handle, &data) == PX4_OK);
+			advertise();
-
-		} else {
-			orb_advert_t handle = orb_advertise(_meta, &data);
-
-			if (handle != nullptr) {
-				_handle = handle;
-				return true;
-			}
 		}
 
-		return false;
+		return (DeviceNode::publish(get_topic(), _handle, &data) == PX4_OK);
 	}
-
-protected:
-	const orb_metadata *_meta;
-
-	orb_advert_t _handle{nullptr};
 };
 
 /**
@@ -100,8 +127,8 @@ public:
 	 *
 	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
 	 */
+	PublicationData(ORB_ID id) : Publication<T>(id) {}
 	PublicationData(const orb_metadata *meta) : Publication<T>(meta) {}
-	~PublicationData() = default;
 
 	T	&get() { return _data; }
 	void	set(const T &data) { _data = data; }
@@ -118,4 +145,10 @@ private:
 	T _data{};
 };
 
+
+template<class T>
+using PublicationQueued = Publication<T, T::ORB_QUEUE_LENGTH>;
+
+
+
 } // namespace uORB