diff --git a/msg/gps_dump.msg b/msg/gps_dump.msg
index 3718e457cb2c..26de92e833fa 100644
--- a/msg/gps_dump.msg
+++ b/msg/gps_dump.msg
@@ -6,3 +6,5 @@ uint64 timestamp		# time since system start (microseconds)
 uint8 len			# length of data, MSB bit set = message to the gps device,
 				# clear = message from the device
 uint8[79] data			# data to write to the log
+
+uint8 ORB_QUEUE_LENGTH = 8
diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg
index 00b1aa1b8abe..5daae0623ea2 100644
--- a/msg/gps_inject_data.msg
+++ b/msg/gps_inject_data.msg
@@ -2,3 +2,5 @@ uint64 timestamp		# time since system start (microseconds)
 uint8 len			# length of data
 uint8 flags         		# LSB: 1=fragmented
 uint8[182] data		# data to write to GPS device (RTCM message)
+
+uint8 ORB_QUEUE_LENGTH = 6
diff --git a/msg/led_control.msg b/msg/led_control.msg
index 582c9dfb733a..d602158e5d8d 100644
--- a/msg/led_control.msg
+++ b/msg/led_control.msg
@@ -33,3 +33,5 @@ uint8 mode # see MODE_*
 uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite
                  # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20
 uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY)
+
+uint8 ORB_QUEUE_LENGTH = 4      # needs to match BOARD_MAX_LEDS
diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg
index e7387b08ddb1..6a8ab72bc6d1 100644
--- a/msg/mavlink_log.msg
+++ b/msg/mavlink_log.msg
@@ -2,3 +2,5 @@ uint64 timestamp		# time since system start (microseconds)
 
 char[50] text
 uint8 severity # log level (same as in the linux kernel, starting with 0)
+
+uint8 ORB_QUEUE_LENGTH = 5
diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg
index 37eaca5e9485..389d5d61839c 100644
--- a/msg/subsystem_info.msg
+++ b/msg/subsystem_info.msg
@@ -34,4 +34,4 @@ bool enabled
 bool ok
 uint64 subsystem_type
 
-uint32 ORB_QUEUE_LENGTH = 5
+uint8 ORB_QUEUE_LENGTH = 5
diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg
index 9d897c644477..3f6f3248c84f 100644
--- a/msg/telemetry_status.msg
+++ b/msg/telemetry_status.msg
@@ -55,3 +55,5 @@ float32 rate_rx
 
 float32 rate_tx
 float32 rate_txerr
+
+uint8 ORB_QUEUE_LENGTH = 3
diff --git a/msg/test_motor.msg b/msg/test_motor.msg
index eaad0d74e702..55bfd3f8f29d 100644
--- a/msg/test_motor.msg
+++ b/msg/test_motor.msg
@@ -3,3 +3,5 @@ uint8 NUM_MOTOR_OUTPUTS = 8
 
 uint32 motor_number				# number of motor to spin
 float32 value					# output power, range [0..1]
+
+uint8 ORB_QUEUE_LENGTH = 4
diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg
index 603f034fa42a..99d2a332b341 100644
--- a/msg/transponder_report.msg
+++ b/msg/transponder_report.msg
@@ -22,4 +22,4 @@ uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16
 uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32
 uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256
 
-uint32 ORB_QUEUE_LENGTH = 10
+uint8 ORB_QUEUE_LENGTH = 10
diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg
index e6e480ebd407..cb188ce2974b 100644
--- a/msg/uavcan_parameter_request.msg
+++ b/msg/uavcan_parameter_request.msg
@@ -7,3 +7,5 @@ int16 param_index		# -1 if the param_id field should be used as identifier
 uint8 param_type		# MAVLink parameter type
 int64 int_value			# current value if param_type is int-like
 float32 real_value		# current value if param_type is float-like
+
+uint8 ORB_QUEUE_LENGTH = 3
diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg
index 0f7d29177497..4dedf1879d9a 100644
--- a/msg/vehicle_command_ack.msg
+++ b/msg/vehicle_command_ack.msg
@@ -13,7 +13,7 @@ uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3
 uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4
 uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5
 
-uint32 ORB_QUEUE_LENGTH = 3
+uint8 ORB_QUEUE_LENGTH = 3
 
 uint16 command
 uint8 result
diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp
index 074673cd6834..f731110bffa6 100644
--- a/src/drivers/camera_capture/camera_capture.cpp
+++ b/src/drivers/camera_capture/camera_capture.cpp
@@ -45,25 +45,12 @@
 
 namespace camera_capture
 {
-CameraCapture	*g_camera_capture;
+CameraCapture *g_camera_capture{nullptr};
 }
 
 CameraCapture::CameraCapture() :
-	_capture_enabled(false),
-	_gpio_capture(false),
-	_trigger_pub(nullptr),
-	_command_ack_pub(nullptr),
-	_command_sub(-1),
-	_trig_buffer(nullptr),
-	_camera_capture_mode(0),
-	_camera_capture_edge(0),
-	_capture_seq(0),
-	_last_trig_begin_time(0),
-	_last_exposure_time(0),
-	_capture_overflows(0)
+	ScheduledWorkItem(px4::wq_configurations::lp_default)
 {
-
-	memset(&_work, 0, sizeof(_work));
 	memset(&_work_publisher, 0, sizeof(_work_publisher));
 
 	// Capture Parameters
@@ -75,7 +62,6 @@ CameraCapture::CameraCapture() :
 
 	_p_camera_capture_edge = param_find("CAM_CAP_EDGE");
 	param_get(_p_camera_capture_edge, &_camera_capture_edge);
-
 }
 
 CameraCapture::~CameraCapture()
@@ -89,10 +75,8 @@ CameraCapture::~CameraCapture()
 }
 
 void
-CameraCapture::capture_callback(uint32_t chan_index,
-				hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
+CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
 {
-
 	_trigger.chan_index = chan_index;
 	_trigger.edge_time = edge_time;
 	_trigger.edge_state = edge_state;
@@ -114,7 +98,6 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg)
 	work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, dev, 0);
 
 	return PX4_OK;
-
 }
 
 void
@@ -130,7 +113,7 @@ CameraCapture::publish_trigger()
 {
 	bool publish = false;
 
-	struct camera_trigger_s	trigger {};
+	camera_trigger_s trigger{};
 
 	// MODES 1 and 2 are not fully tested
 	if (_camera_capture_mode == 0 || _gpio_capture) {
@@ -173,48 +156,23 @@ CameraCapture::publish_trigger()
 		return;
 	}
 
-	if (_trigger_pub == nullptr) {
-
-		_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
-
-	} else {
-
-		orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger);
-	}
-
+	_trigger_pub.publish(trigger);
 }
 
 void
-CameraCapture::capture_trampoline(void *context, uint32_t chan_index,
-				  hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
+CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
+				  uint32_t overflow)
 {
 	camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow);
 }
 
 void
-CameraCapture::cycle_trampoline(void *arg)
-{
-	CameraCapture *dev = reinterpret_cast<CameraCapture *>(arg);
-
-	dev->cycle();
-}
-
-void
-CameraCapture::cycle()
+CameraCapture::Run()
 {
-
-	if (_command_sub < 0) {
-		_command_sub = orb_subscribe(ORB_ID(vehicle_command));
-	}
-
-	bool updated = false;
-	orb_check(_command_sub, &updated);
-
 	// Command handling
-	if (updated) {
+	vehicle_command_s cmd{};
 
-		vehicle_command_s cmd;
-		orb_copy(ORB_ID(vehicle_command), _command_sub, &cmd);
+	if (_command_sub.update(&cmd)) {
 
 		// TODO : this should eventuallly be a capture control command
 		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
@@ -235,31 +193,17 @@ CameraCapture::cycle()
 			}
 
 			// Acknowledge the command
-			vehicle_command_ack_s command_ack = {
-				.timestamp = 0,
-				.result_param2 = 0,
-				.command = cmd.command,
-				.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED,
-				.from_external = false,
-				.result_param1 = 0,
-				.target_system = cmd.source_system,
-				.target_component = cmd.source_component
-			};
-
-			if (_command_ack_pub == nullptr) {
-				_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-								       vehicle_command_ack_s::ORB_QUEUE_LENGTH);
+			vehicle_command_ack_s command_ack{};
 
-			} else {
-				orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
+			command_ack.timestamp = hrt_absolute_time();
+			command_ack.command = cmd.command;
+			command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			command_ack.target_system = cmd.source_system;
+			command_ack.target_component = cmd.source_component;
 
-			}
+			_command_ack_pub.publish(command_ack);
 		}
-
 	}
-
-	work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture,
-		   USEC2TICK(100000)); // 100ms
 }
 
 void
@@ -267,9 +211,7 @@ CameraCapture::set_capture_control(bool enabled)
 {
 #if !defined CONFIG_ARCH_BOARD_AV_X_V1
 
-	int fd = -1;
-
-	fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
+	int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
 
 	if (fd < 0) {
 		PX4_ERR("open fail");
@@ -343,7 +285,9 @@ CameraCapture::set_capture_control(bool enabled)
 void
 CameraCapture::reset_statistics(bool reset_seq)
 {
-	if (reset_seq) { _capture_seq = 0; }
+	if (reset_seq) {
+		_capture_seq = 0;
+	}
 
 	_last_trig_begin_time = 0;
 	_last_exposure_time = 0;
@@ -361,9 +305,8 @@ CameraCapture::start()
 		return PX4_ERROR;
 	}
 
-	// start to monitor at low rates for capture control commands
-	work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this,
-		   USEC2TICK(1));
+	// run every 100 ms (10 Hz)
+	ScheduleOnInterval(100000, 10000);
 
 	return PX4_OK;
 }
@@ -371,8 +314,8 @@ CameraCapture::start()
 void
 CameraCapture::stop()
 {
+	ScheduleClear();
 
-	work_cancel(LPWORK, &_work);
 	work_cancel(HPWORK, &_work_publisher);
 
 	if (camera_capture::g_camera_capture != nullptr) {
diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp
index 95fc1a41c03a..c8654071169e 100644
--- a/src/drivers/camera_capture/camera_capture.hpp
+++ b/src/drivers/camera_capture/camera_capture.hpp
@@ -38,22 +38,23 @@
 
 #pragma once
 
-#include <fcntl.h>
-#include <string.h>
-
-#include <parameters/param.h>
-#include <px4_defines.h>
-#include <px4_workqueue.h>
-
 #include <drivers/device/ringbuffer.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_pwm_output.h>
 #include <drivers/drv_input_capture.h>
-
+#include <drivers/drv_pwm_output.h>
+#include <lib/parameters/param.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_module.h>
+#include <px4_tasks.h>
+#include <px4_workqueue.h>
+#include <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>
 #include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/uORB.h>
 
 #define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
 
@@ -61,7 +62,7 @@
 #define GPIO_TRIG_AVX /* PD14 */  (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14)
 
 
-class CameraCapture
+class CameraCapture : public px4::ScheduledWorkItem
 {
 public:
 	/**
@@ -86,10 +87,10 @@ class CameraCapture
 
 	void 			status();
 
-	void			cycle();
+	// Low-rate command handling loop
+	void			Run() override;
 
-	static void		capture_trampoline(void *context, uint32_t chan_index,
-			hrt_abstime edge_time, uint32_t edge_state,
+	static void		capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state,
 			uint32_t overflow);
 
 	void 			set_capture_control(bool enabled);
@@ -99,20 +100,16 @@ class CameraCapture
 	void			publish_trigger();
 
 
-	static struct work_s	_work;
 	static struct work_s	_work_publisher;
 
 private:
 
-	bool			_capture_enabled;
-	bool			_gpio_capture;
-
 	// Publishers
-	orb_advert_t	_trigger_pub;
-	orb_advert_t	_command_ack_pub;
+	uORB::PublicationQueued<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<camera_trigger_s>		_trigger_pub{ORB_ID(camera_trigger)};
 
 	// Subscribers
-	int				_command_sub;
+	uORB::Subscription				_command_sub{ORB_ID(vehicle_command)};
 
 	// Trigger Buffer
 	struct _trig_s {
@@ -120,30 +117,30 @@ class CameraCapture
 		hrt_abstime edge_time;
 		uint32_t edge_state;
 		uint32_t overflow;
-	};
+	} _trigger{};
 
-	struct _trig_s _trigger;
+	ringbuffer::RingBuffer	*_trig_buffer{nullptr};
 
-	ringbuffer::RingBuffer *_trig_buffer;
+	bool			_capture_enabled{false};
+	bool			_gpio_capture{false};
 
 	// Parameters
-	param_t 		_p_strobe_delay;
-	float			_strobe_delay;
-	param_t			_p_camera_capture_mode;
-	int32_t			_camera_capture_mode;
-	param_t			_p_camera_capture_edge;
-	int32_t			_camera_capture_edge;
+	param_t 		_p_strobe_delay{PARAM_INVALID};
+	float			_strobe_delay{0.0f};
+	param_t			_p_camera_capture_mode{PARAM_INVALID};
+	int32_t			_camera_capture_mode{0};
+	param_t			_p_camera_capture_edge{PARAM_INVALID};
+	int32_t			_camera_capture_edge{0};
 
 	// Signal capture statistics
-	uint32_t		_capture_seq;
-	hrt_abstime		_last_trig_begin_time;
-	hrt_abstime		_last_exposure_time;
-	hrt_abstime		_last_trig_time;
-	uint32_t 		_capture_overflows;
+	uint32_t		_capture_seq{0};
+	hrt_abstime		_last_trig_begin_time{0};
+	hrt_abstime		_last_exposure_time{0};
+	hrt_abstime		_last_trig_time{0};
+	uint32_t 		_capture_overflows{0};
 
 	// Signal capture callback
-	void			capture_callback(uint32_t chan_index,
-			hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
+	void			capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
 
 	// GPIO interrupt routine (for AV_X board)
 	static int		gpio_interrupt_routine(int irq, void *context, void *arg);
@@ -151,9 +148,6 @@ class CameraCapture
 	// Signal capture publish
 	static void		publish_trigger_trampoline(void *arg);
 
-	// Low-rate command handling loop
-	static void		cycle_trampoline(void *arg);
-
 };
-struct work_s CameraCapture::_work;
+
 struct work_s CameraCapture::_work_publisher;
diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index a5b118606921..44062d07f09b 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -55,6 +55,7 @@
 #include <parameters/param.h>
 #include <systemlib/mavlink_log.h>
 
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/camera_capture.h>
@@ -174,7 +175,8 @@ class CameraTrigger : public px4::ScheduledWorkItem
 	uORB::Subscription	_lpos_sub{ORB_ID(vehicle_local_position)};
 
 	orb_advert_t		_trigger_pub;
-	orb_advert_t		_cmd_ack_pub;
+
+	uORB::PublicationQueued<vehicle_command_ack_s>	_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	param_t			_p_mode;
 	param_t			_p_activation_time;
@@ -192,7 +194,7 @@ class CameraTrigger : public px4::ScheduledWorkItem
 	/**
 	 * Vehicle command handler
 	 */
-	void		Run();
+	void		Run() override;
 
 	/**
 	 * Fires trigger
@@ -250,7 +252,6 @@ CameraTrigger::CameraTrigger() :
 	_last_shoot_position(0.0f, 0.0f),
 	_valid_position(false),
 	_trigger_pub(nullptr),
-	_cmd_ack_pub(nullptr),
 	_trigger_mode(TRIGGER_MODE_NONE),
 	_cam_cap_fback(0),
 	_camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO),
@@ -477,12 +478,13 @@ CameraTrigger::stop()
 void
 CameraTrigger::test()
 {
-	vehicle_command_s vcmd = {};
+	vehicle_command_s vcmd{};
 	vcmd.timestamp = hrt_absolute_time();
 	vcmd.param5 = 1.0;
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
 
-	orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	vcmd_pub.publish(vcmd);
 }
 
 void
@@ -682,20 +684,15 @@ CameraTrigger::Run()
 
 	// Command ACK handling
 	if (updated && need_ack) {
-		vehicle_command_ack_s command_ack = {};
+		vehicle_command_ack_s command_ack{};
+
 		command_ack.timestamp = hrt_absolute_time();
 		command_ack.command = cmd.command;
 		command_ack.result = (uint8_t)cmd_result;
 		command_ack.target_system = cmd.source_system;
 		command_ack.target_component = cmd.source_component;
 
-		if (_cmd_ack_pub == nullptr) {
-			_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-							   vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_command_ack), _cmd_ack_pub, &command_ack);
-		}
+		_cmd_ack_pub.publish(command_ack);
 	}
 
 	ScheduleDelayed(poll_interval_usec);
diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h
index e02bb4fa2448..53dc01c2d26e 100644
--- a/src/drivers/drv_led.h
+++ b/src/drivers/drv_led.h
@@ -49,14 +49,12 @@
 #define BOARD_MAX_LEDS 4
 #endif
 
+static_assert(led_control_s::ORB_QUEUE_LENGTH >= BOARD_MAX_LEDS, "led_control_s::ORB_QUEUE_LENGTH too small");
+
 #if BOARD_MAX_LEDS > 8 // because led_mask is uint8_t
 #error "BOARD_MAX_LEDS too large. You need to change the led_mask type in the led_control uorb topic (and where it's used)"
 #endif
 
-
-// set the queue size to the number of LED's, so that each led can be controlled individually
-static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS;
-
 // Legacy paths - 2 are need to allow both pwm and i2c drviers to co-exist
 #define RGBLED0_DEVICE_PATH "/dev/rgbled0"         // Primary RGB LED on i2c
 #define RGBLED1_DEVICE_PATH "/dev/rgbled1"	   // Primary RGB LED(NCP5623C) on i2c
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 3979a267cebc..75dd3bad5018 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -52,6 +52,7 @@
 #include <px4_cli.h>
 #include <px4_getopt.h>
 #include <px4_module.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
@@ -172,9 +173,10 @@ class GPS : public ModuleBase<GPS>
 	const Instance 			_instance;
 
 	uORB::Subscription		_orb_inject_data_sub{ORB_ID(gps_inject_data)};
-	orb_advert_t			_dump_communication_pub{nullptr};		///< if non-null, dump communication
+	uORB::PublicationQueued<gps_dump_s>	_dump_communication_pub{ORB_ID(gps_dump)};
 	gps_dump_s			*_dump_to_device{nullptr};
 	gps_dump_s			*_dump_from_device{nullptr};
+	bool				_should_dump_communication{false};			///< if true, dump communication
 
 	static volatile bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1
 	/// and thus we wait until the first one publishes at least one message.
@@ -549,16 +551,16 @@ void GPS::initializeCommunicationDump()
 	memset(_dump_to_device, 0, sizeof(gps_dump_s));
 	memset(_dump_from_device, 0, sizeof(gps_dump_s));
 
-	int instance;
 	//make sure to use a large enough queue size, so that we don't lose messages. You may also want
 	//to increase the logger rate for that.
-	_dump_communication_pub = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance,
-				  ORB_PRIO_DEFAULT, 8);
+	_dump_communication_pub.publish(*_dump_from_device);
+
+	_should_dump_communication = true;
 }
 
 void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
 {
-	if (!_dump_communication_pub) {
+	if (!_should_dump_communication) {
 		return;
 	}
 
@@ -582,7 +584,7 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device)
 			}
 
 			dump_data->timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID(gps_dump), _dump_communication_pub, dump_data);
+			_dump_communication_pub.publish(*dump_data);
 			dump_data->len = 0;
 		}
 	}
@@ -820,10 +822,6 @@ GPS::run()
 
 	PX4_INFO("exiting");
 
-	if (_dump_communication_pub) {
-		orb_unadvertise(_dump_communication_pub);
-	}
-
 	if (_serial_fd >= 0) {
 		::close(_serial_fd);
 		_serial_fd = -1;
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 21c5abab43bd..045735d9001e 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -76,6 +76,7 @@
 #include <circuit_breaker/circuit_breaker.h>
 #include <systemlib/mavlink_log.h>
 
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -738,7 +739,8 @@ PX4IO::init()
 			vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
 
 			/* send command once */
-			orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+			uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+			vcmd_pub.publish(vcmd);
 
 			/* spin here until IO's state has propagated into the system */
 			do {
@@ -755,7 +757,7 @@ PX4IO::init()
 
 				/* re-send if necessary */
 				if (!actuator_armed.force_failsafe) {
-					orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
+					vcmd_pub.publish(vcmd);
 					PX4_WARN("re-sending flight termination cmd");
 				}
 
@@ -769,7 +771,8 @@ PX4IO::init()
 		vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
 
 		/* send command once */
-		orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+		uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+		vcmd_pub.publish(vcmd);
 
 		/* spin here until IO's state has propagated into the system */
 		do {
@@ -786,7 +789,7 @@ PX4IO::init()
 
 			/* re-send if necessary */
 			if (!actuator_armed.armed) {
-				orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
+				vcmd_pub.publish(vcmd);
 				PX4_WARN("re-sending arm cmd");
 			}
 
diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
index 9c118fc5c593..ade79e314ca4 100644
--- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
+++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp
@@ -1111,14 +1111,8 @@ void IridiumSBD::publish_iridium_status()
 
 	// publish the status if it changed
 	if (need_to_publish) {
-		if (_iridiumsbd_status_pub == nullptr) {
-			_iridiumsbd_status_pub = orb_advertise(ORB_ID(iridiumsbd_status), &_status);
-
-		} else {
-			orb_publish(ORB_ID(iridiumsbd_status), _iridiumsbd_status_pub, &_status);
-		}
+		_iridiumsbd_status_pub.publish(_status);
 	}
-
 }
 
 void IridiumSBD::publish_subsystem_status()
@@ -1134,12 +1128,7 @@ void IridiumSBD::publish_subsystem_status()
 		_info.enabled = enabled;
 		_info.ok = ok;
 
-		if (_subsystem_pub == nullptr) {
-			_subsystem_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH);
-
-		} else {
-			orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &_info);
-		}
+		_subsystem_pub.publish(_info);
 	}
 }
 
diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h
index 18085182a7c9..85fae0ab4201 100644
--- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h
+++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h
@@ -39,7 +39,8 @@
 #include <lib/cdev/CDev.hpp>
 #include <drivers/drv_hrt.h>
 
-#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/subsystem_info.h>
 
@@ -304,8 +305,8 @@ class IridiumSBD : public cdev::CDev
 	bool _writing_mavlink_packet = false;
 	uint16_t _packet_length = 0;
 
-	orb_advert_t _iridiumsbd_status_pub = nullptr;
-	orb_advert_t _subsystem_pub = nullptr;
+	uORB::Publication<iridiumsbd_status_s> _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)};
+	uORB::PublicationQueued<subsystem_info_s> _subsystem_pub{ORB_ID(subsystem_info)};
 
 	bool _test_pending = false;
 	char _test_command[32];
diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp
index ed96586bc5ab..c47c739fc19c 100644
--- a/src/drivers/uavcan/uavcan_servers.cpp
+++ b/src/drivers/uavcan/uavcan_servers.cpp
@@ -564,20 +564,14 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 			}
 
 			// Acknowledge the received command
-			vehicle_command_ack_s ack = {};
+			vehicle_command_ack_s ack{};
 			ack.timestamp = hrt_absolute_time();
 			ack.command = cmd.command;
 			ack.result = cmd_ack_result;
 			ack.target_system = cmd.source_system;
 			ack.target_component = cmd.source_component;
 
-			if (_command_ack_pub == nullptr) {
-				_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-			} else {
-				orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &ack);
-			}
-
+			_command_ack_pub.publish(ack);
 		}
 
 		// Shut down once armed
diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp
index ec01ea50102c..7465e546c35e 100644
--- a/src/drivers/uavcan/uavcan_servers.hpp
+++ b/src/drivers/uavcan/uavcan_servers.hpp
@@ -36,7 +36,9 @@
 #include <px4_config.h>
 #include "uavcan_driver.hpp"
 #include <drivers/device/device.h>
-#include <perf/perf_counter.h>
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/vehicle_command_ack.h>
 
 #include <uavcan/node/sub_node.hpp>
 #include <uavcan/protocol/node_status_monitor.hpp>
@@ -160,7 +162,7 @@ class __EXPORT UavcanServers
 
 	// uORB topic handle for MAVLink parameter responses
 	orb_advert_t _param_response_pub = nullptr;
-	orb_advert_t _command_ack_pub = nullptr;
+	uORB::PublicationQueued<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	typedef uavcan::MethodBinder<UavcanServers *,
 		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index cdc4aa16d2f7..8596b5745e99 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -137,22 +137,16 @@ void FlightTasks::reActivate()
 
 void FlightTasks::_updateCommand()
 {
-	// lazy subscription to command topic
-	if (_sub_vehicle_command < 0) {
-		_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
-	}
-
 	// check if there's any new command
-	bool updated = false;
-	orb_check(_sub_vehicle_command, &updated);
+	bool updated = _sub_vehicle_command.updated();
 
 	if (!updated) {
 		return;
 	}
 
 	// get command
-	struct vehicle_command_s command;
-	orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
+	vehicle_command_s command{};
+	_sub_vehicle_command.copy(&command);
 
 	// check what command it is
 	FlightTaskIndex desired_task = switchVehicleCommand(command.command);
@@ -183,19 +177,13 @@ void FlightTasks::_updateCommand()
 	}
 
 	// send back acknowledgment
-	vehicle_command_ack_s command_ack = {};
+	vehicle_command_ack_s command_ack{};
+	command_ack.timestamp = hrt_absolute_time();
 	command_ack.command = command.command;
 	command_ack.result = cmd_result;
 	command_ack.result_param1 = switch_result;
 	command_ack.target_system = command.source_system;
 	command_ack.target_component = command.source_component;
 
-	if (_pub_vehicle_command_ack == nullptr) {
-		_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-					   vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);
-
-	}
+	_pub_vehicle_command_ack.publish(command_ack);
 }
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index dc5020c2b2c7..cba7706d3ac7 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -45,6 +45,10 @@
 #include "SubscriptionArray.hpp"
 #include "FlightTasks_generated.hpp"
 #include <lib/WeatherVane/WeatherVane.hpp>
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_command.h>
 
 #include <new>
 
@@ -180,8 +184,10 @@ class FlightTasks
 	 */
 	void _updateCommand();
 	FlightTaskIndex switchVehicleCommand(const int command);
-	int _sub_vehicle_command = -1; /**< topic handle on which commands are received */
-	orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */
+
+	uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */
+
+	uORB::PublicationQueued<vehicle_command_ack_s>	_pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};
 
 	int _initTask(FlightTaskIndex task_index);
 };
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index 2fac77484c3a..fbdcb8f98231 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -63,19 +63,6 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
 
 }
 
-ObstacleAvoidance::~ObstacleAvoidance()
-{
-	//unadvertise publishers
-	if (_pub_traj_wp_avoidance_desired != nullptr) {
-		orb_unadvertise(_pub_traj_wp_avoidance_desired);
-	}
-
-	if (_pub_pos_control_status != nullptr) {
-		orb_unadvertise(_pub_pos_control_status);
-	}
-
-}
-
 bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
 {
 	if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
@@ -170,7 +157,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp,
 	vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
 	_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
 
-	_publishAvoidanceDesiredWaypoint();
+	_pub_traj_wp_avoidance_desired.publish(_desired_waypoint);
 
 	_desired_waypoint = empty_trajectory_waypoint;
 }
@@ -208,27 +195,7 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
 	// do not check for waypoints yaw acceptance in navigator
 	pos_control_status.yaw_acceptance = NAN;
 
-	if (_pub_pos_control_status == nullptr) {
-		_pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
-
-	} else {
-		orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
-
-	}
-
-}
-
-void
-ObstacleAvoidance::_publishAvoidanceDesiredWaypoint()
-{
-	// publish desired waypoint
-	if (_pub_traj_wp_avoidance_desired != nullptr) {
-		orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _pub_traj_wp_avoidance_desired, &_desired_waypoint);
-
-	} else {
-		_pub_traj_wp_avoidance_desired = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
-						 &_desired_waypoint);
-	}
+	_pub_pos_control_status.publish(pos_control_status);
 }
 
 void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
@@ -248,11 +215,5 @@ void ObstacleAvoidance::_publishVehicleCmdDoLoiter()
 	command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 
 	// publish the vehicle command
-	if (_pub_vehicle_command == nullptr) {
-		_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
-				       vehicle_command_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
-	}
+	_pub_vehicle_command.publish(command);
 }
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
index f873e1702b6e..f5e2803429c1 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
@@ -46,6 +46,8 @@
 #include <commander/px4_custom_mode.h>
 #include <drivers/drv_hrt.h>
 
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_status.h>
@@ -62,7 +64,7 @@ class ObstacleAvoidance : public ModuleParams
 {
 public:
 	ObstacleAvoidance(ModuleParams *parent);
-	~ObstacleAvoidance();
+	~ObstacleAvoidance() = default;
 
 	bool initializeSubscriptions(SubscriptionArray &subscription_array);
 
@@ -113,10 +115,11 @@ class ObstacleAvoidance : public ModuleParams
 		(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad    /**< Acceptance radius for multicopter altitude */
 	);
 
-	vehicle_trajectory_waypoint_s _desired_waypoint = {};  /**< desired vehicle trajectory waypoint to be sent to OA */
-	orb_advert_t _pub_traj_wp_avoidance_desired{nullptr}; /**< trajectory waypoint desired publication */
-	orb_advert_t _pub_pos_control_status{nullptr}; /**< position controller status publication */
-	orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */
+	vehicle_trajectory_waypoint_s _desired_waypoint{};  /**< desired vehicle trajectory waypoint to be sent to OA */
+
+	uORB::Publication<vehicle_trajectory_waypoint_s> _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)};	/**< trajectory waypoint desired publication */
+	uORB::Publication<position_controller_status_s> _pub_pos_control_status{ORB_ID(position_controller_status)};	/**< position controller status publication */
+	uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)};	/**< vehicle command do publication */
 
 	matrix::Vector3f _curr_wp = {}; /**< current position triplet */
 	matrix::Vector3f _position = {}; /**< current vehicle position */
@@ -126,11 +129,6 @@ class ObstacleAvoidance : public ModuleParams
 
 	bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
 
-	/**
-	 * Publishes vehicle trajectory waypoint desired.
-	 */
-	void _publishAvoidanceDesiredWaypoint();
-
 	/**
 	 * Publishes vehicle command.
 	 */
diff --git a/src/lib/systemlib/CMakeLists.txt b/src/lib/systemlib/CMakeLists.txt
index 62a3b1161920..4975dfcb123c 100644
--- a/src/lib/systemlib/CMakeLists.txt
+++ b/src/lib/systemlib/CMakeLists.txt
@@ -35,7 +35,7 @@ set(SRCS
 	conversions.c
 	cpuload.c
 	crc.c
-	mavlink_log.c
+	mavlink_log.cpp
 	otp.c
 	)
 
diff --git a/src/lib/systemlib/mavlink_log.c b/src/lib/systemlib/mavlink_log.cpp
similarity index 90%
rename from src/lib/systemlib/mavlink_log.c
rename to src/lib/systemlib/mavlink_log.cpp
index a83a6d680a82..142be78ced94 100644
--- a/src/lib/systemlib/mavlink_log.c
+++ b/src/lib/systemlib/mavlink_log.cpp
@@ -48,9 +48,6 @@
 #include <uORB/topics/mavlink_log.h>
 #include "mavlink_log.h"
 
-#define MAVLINK_LOG_QUEUE_SIZE 5
-
-
 __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...)
 {
 	// TODO: add compile check for maxlen
@@ -59,31 +56,23 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 		return;
 	}
 
-	if (mavlink_log_pub == NULL) {
+	if (mavlink_log_pub == nullptr) {
 		return;
 	}
 
-	struct mavlink_log_s log_msg;
-
+	mavlink_log_s log_msg;
 	log_msg.severity = severity;
-
 	log_msg.timestamp = hrt_absolute_time();
 
 	va_list ap;
-
 	va_start(ap, fmt);
-
 	vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap);
-
 	va_end(ap);
 
-	if (*mavlink_log_pub != NULL) {
+	if (*mavlink_log_pub != nullptr) {
 		orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg);
 
 	} else {
-		*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log),
-						       &log_msg,
-						       MAVLINK_LOG_QUEUE_SIZE);
+		*mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH);
 	}
 }
-
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 0349882b1a1d..87d008d584b6 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -93,7 +93,6 @@
 #include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
-#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
@@ -205,7 +204,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const ch
  */
 void *commander_low_prio_loop(void *arg);
 
-static void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub);
+static void answer_command(const vehicle_command_s &cmd, unsigned result,
+			   uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);
 
 static int power_button_state_notification_cb(board_power_button_state_notification_e request)
 {
@@ -249,7 +249,8 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
 
 static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN)
 {
-	vehicle_command_s vcmd = {};
+	vehicle_command_s vcmd{};
+
 	vcmd.timestamp = hrt_absolute_time();
 	vcmd.param1 = param1;
 	vcmd.param2 = param2;
@@ -262,9 +263,9 @@ static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2
 	vcmd.target_system = status.system_id;
 	vcmd.target_component = status.component_id;
 
-	orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
 
-	return (h != nullptr);
+	return vcmd_pub.publish(vcmd);
 }
 
 int commander_main(int argc, char *argv[])
@@ -577,7 +578,7 @@ Commander::~Commander()
 
 bool
 Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local,
-			  orb_advert_t *command_ack_pub, bool *changed)
+			  uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed)
 {
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
@@ -1078,13 +1079,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 	default:
 		/* Warn about unsupported commands, this makes sense because only commands
 		 * to this component ID (or all) are passed by mavlink. */
-		answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub);
+		answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
 		break;
 	}
 
 	if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
-		answer_command(cmd, cmd_result, *command_ack_pub);
+		answer_command(cmd, cmd_result, command_ack_pub);
 	}
 
 	return true;
@@ -1239,7 +1240,7 @@ Commander::run()
 	hrt_abstime last_disarmed_timestamp = 0;
 
 	/* command ack */
-	orb_advert_t command_ack_pub = nullptr;
+	uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
 	mission_init();
@@ -2152,7 +2153,7 @@ Commander::run()
 			cmd_sub.copy(&cmd);
 
 			/* handle it */
-			if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) {
+			if (handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) {
 				status_changed = true;
 			}
 		}
@@ -3260,7 +3261,8 @@ print_reject_arm(const char *msg)
 	}
 }
 
-void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub)
+void answer_command(const vehicle_command_s &cmd, unsigned result,
+		    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub)
 {
 	switch (result) {
 	case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
@@ -3288,20 +3290,16 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t
 	}
 
 	/* publish ACK */
-	vehicle_command_ack_s command_ack = {};
+	vehicle_command_ack_s command_ack{};
+
 	command_ack.timestamp = hrt_absolute_time();
 	command_ack.command = cmd.command;
 	command_ack.result = (uint8_t)result;
 	command_ack.target_system = cmd.source_system;
 	command_ack.target_component = cmd.source_component;
 
-	if (command_ack_pub != nullptr) {
-		orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
 
-	} else {
-		command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-						      vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-	}
+	command_ack_pub.publish(command_ack);
 }
 
 void *commander_low_prio_loop(void *arg)
@@ -3313,7 +3311,7 @@ void *commander_low_prio_loop(void *arg)
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 
 	/* command ack */
-	orb_advert_t command_ack_pub = nullptr;
+	uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	/* wakeup source(s) */
 	px4_pollfd_struct_t fds[1];
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index e29f54e44ada..47c9328c5668 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -45,8 +45,10 @@
 
 // publications
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
@@ -180,7 +182,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool _flight_termination_triggered{false};
 
 	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
-			    orb_advert_t *command_ack_pub, bool *changed);
+			    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed);
 
 	bool set_home_position();
 	bool set_home_position_alt_only();
@@ -258,12 +260,13 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	// Publications
 	uORB::Publication<vehicle_control_mode_s>		_control_mode_pub{ORB_ID(vehicle_control_mode)};
-	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};
 	uORB::Publication<vehicle_status_s>			_status_pub{ORB_ID(vehicle_status)};
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};
 	uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
 	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
 
+	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};
+
 };
 
 #endif /* COMMANDER_HPP_ */
diff --git a/src/modules/commander/arm_auth.cpp b/src/modules/commander/arm_auth.cpp
index 891f9c703809..4dbd79b79935 100644
--- a/src/modules/commander/arm_auth.cpp
+++ b/src/modules/commander/arm_auth.cpp
@@ -39,10 +39,10 @@
 #include <px4_config.h>
 
 #include <systemlib/mavlink_log.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
-static orb_advert_t handle_vehicle_command_pub;
 static orb_advert_t *mavlink_log_pub;
 static int command_ack_sub = -1;
 
@@ -78,17 +78,13 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = {
 
 static void arm_auth_request_msg_send()
 {
-	vehicle_command_s vcmd = {};
+	vehicle_command_s vcmd{};
 	vcmd.timestamp = hrt_absolute_time();
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST;
 	vcmd.target_system = arm_parameters.authorizer_system_id;
 
-	if (handle_vehicle_command_pub == nullptr) {
-		handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd);
-	}
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	vcmd_pub.publish(vcmd);
 }
 
 static uint8_t _auth_method_arm_req_check()
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index a0107297e771..d8e1554c769a 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -290,7 +290,7 @@ int led_init()
 	led_control.mode = led_control_s::MODE_OFF;
 	led_control.priority = 0;
 	led_control.timestamp = hrt_absolute_time();
-	led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
+	led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH);
 
 	/* first open normal LEDs */
 	DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp
index 752c7652a47f..1ef9c6589feb 100644
--- a/src/modules/events/send_event.cpp
+++ b/src/modules/events/send_event.cpp
@@ -195,20 +195,15 @@ void SendEvent::process_commands()
 void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result)
 {
 	/* publish ACK */
-	vehicle_command_ack_s command_ack = {};
+	vehicle_command_ack_s command_ack{};
 	command_ack.timestamp = hrt_absolute_time();
 	command_ack.command = cmd.command;
 	command_ack.result = (uint8_t)result;
 	command_ack.target_system = cmd.source_system;
 	command_ack.target_component = cmd.source_component;
 
-	if (_command_ack_pub != nullptr) {
-		orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
-
-	} else {
-		_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-						       vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-	}
+	uORB::PublicationQueued<vehicle_command_ack_s>	command_ack_pub{ORB_ID(vehicle_command_ack)};
+	command_ack_pub.publish(command_ack);
 }
 
 int SendEvent::print_usage(const char *reason)
@@ -275,7 +270,7 @@ int SendEvent::custom_command(int argc, char *argv[])
 			}
 		}
 
-		vehicle_command_s vcmd = {};
+		vehicle_command_s vcmd{};
 		vcmd.timestamp = hrt_absolute_time();
 		vcmd.param1 = (float)((gyro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
 		vcmd.param2 = NAN;
@@ -286,7 +281,8 @@ int SendEvent::custom_command(int argc, char *argv[])
 		vcmd.param7 = (float)((baro_calib || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
 		vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
 
-		orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+		uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+		vcmd_pub.publish(vcmd);
 
 	} else {
 		print_usage("unrecognized command");
diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h
index be3b9a49bd1c..805084cabd1c 100644
--- a/src/modules/events/send_event.h
+++ b/src/modules/events/send_event.h
@@ -40,6 +40,7 @@
 #include <px4_workqueue.h>
 #include <px4_module.h>
 #include <px4_module_params.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
@@ -132,9 +133,6 @@ class SendEvent : public ModuleBase<SendEvent>, public ModuleParams
 	/** @var _rc_loss_alarm Pointer to the RC loss alarm object. */
 	rc_loss::RC_Loss_Alarm *_rc_loss_alarm = nullptr;
 
-	/** @var _command_ack_pub The command ackowledgement topic. */
-	orb_advert_t _command_ack_pub = nullptr;
-
 	/** @note Declare local parameters using defined parameters. */
 	DEFINE_PARAMETERS(
 		/** @var _param_status_display Parameter to enable/disable the LED status display. */
diff --git a/src/modules/events/status_display.cpp b/src/modules/events/status_display.cpp
index b2c4fe5a65ec..118e342efc6a 100644
--- a/src/modules/events/status_display.cpp
+++ b/src/modules/events/status_display.cpp
@@ -102,12 +102,7 @@ void StatusDisplay::publish()
 {
 	_led_control.timestamp = hrt_absolute_time();
 
-	if (_led_control_pub != nullptr) {
-		orb_publish(ORB_ID(led_control), _led_control_pub, &_led_control);
-
-	} else {
-		_led_control_pub =  orb_advertise_queue(ORB_ID(led_control), &_led_control, LED_UORB_QUEUE_LENGTH);
-	}
+	_led_control_pub.publish(_led_control);
 }
 
 } /* namespace status */
diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h
index 696956416c63..8a6d35eb2e8d 100644
--- a/src/modules/events/status_display.h
+++ b/src/modules/events/status_display.h
@@ -44,7 +44,7 @@
 
 #include <drivers/drv_hrt.h>
 
-#include <uORB/uORB.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/led_control.h>
@@ -96,7 +96,9 @@ class StatusDisplay
 	bool _critical_battery = false;
 	int _old_nav_state = -1;
 	int _old_battery_status_warning = -1;
-	orb_advert_t _led_control_pub = nullptr;
+
+	uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
+
 	const events::SubscriberHandler &_subscriber_handler;
 };
 
diff --git a/src/modules/events/temperature_calibration/task.cpp b/src/modules/events/temperature_calibration/task.cpp
index d8c3b970654d..29283014b4f0 100644
--- a/src/modules/events/temperature_calibration/task.cpp
+++ b/src/modules/events/temperature_calibration/task.cpp
@@ -39,6 +39,7 @@
  * @author Beat Küng <beat-kueng@gmx.net>
  */
 
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <mathlib/mathlib.h>
 #include <px4_log.h>
@@ -92,7 +93,7 @@ class TemperatureCalibration
 private:
 	void publish_led_control(led_control_s &led_control);
 
-	orb_advert_t _led_control_pub = nullptr;
+	uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
 
 	bool	_force_task_exit = false;
 	int	_control_task = -1;		// task handle for task
@@ -349,13 +350,7 @@ int TemperatureCalibration::start()
 void TemperatureCalibration::publish_led_control(led_control_s &led_control)
 {
 	led_control.timestamp = hrt_absolute_time();
-
-	if (_led_control_pub == nullptr) {
-		_led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(led_control), _led_control_pub, &led_control);
-	}
+	_led_control_pub.publish(led_control);
 }
 
 int run_temperature_calibration(bool accel, bool baro, bool gyro)
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index f3f986dfebab..26d0a65db8cb 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -44,7 +44,7 @@
 #include <stdlib.h>
 #include <time.h>
 
-#include <uORB/uORB.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/uORBTopics.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/log_message.h>
@@ -935,7 +935,6 @@ void Logger::run()
 	}
 
 	int vehicle_command_sub = -1;
-	orb_advert_t vehicle_command_ack_pub = nullptr;
 
 	if (_writer.backend() & LogWriter::BackendMavlink) {
 		vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1042,7 +1041,7 @@ void Logger::run()
 
 		/* check for logging command from MAVLink (start/stop streaming) */
 		if (vehicle_command_sub >= 0) {
-			handle_vehicle_command_update(vehicle_command_sub, vehicle_command_ack_pub);
+			handle_vehicle_command_update(vehicle_command_sub);
 		}
 
 
@@ -1293,10 +1292,6 @@ void Logger::run()
 		_mavlink_log_pub = nullptr;
 	}
 
-	if (vehicle_command_ack_pub) {
-		orb_unadvertise(vehicle_command_ack_pub);
-	}
-
 	if (vehicle_command_sub != -1) {
 		orb_unsubscribe(vehicle_command_sub);
 	}
@@ -1402,7 +1397,7 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su
 	return bret;
 }
 
-void Logger::handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub)
+void Logger::handle_vehicle_command_update(int vehicle_command_sub)
 {
 	bool command_updated = false;
 	int ret = orb_check(vehicle_command_sub, &command_updated);
@@ -1413,23 +1408,19 @@ void Logger::handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t
 
 		if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
 			if ((int)(command.param1 + 0.5f) != 0) {
-				ack_vehicle_command(vehicle_command_ack_pub, &command,
-						    vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 
 			} else if (can_start_mavlink_log()) {
-				ack_vehicle_command(vehicle_command_ack_pub, &command,
-						    vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 				start_log_mavlink();
 
 			} else {
-				ack_vehicle_command(vehicle_command_ack_pub, &command,
-						    vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 			}
 
 		} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
 			stop_log_mavlink();
-			ack_vehicle_command(vehicle_command_ack_pub, &command,
-					    vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+			ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		}
 	}
 }
@@ -2339,7 +2330,7 @@ void Logger::write_changed_parameters(LogType type)
 	_writer.notify();
 }
 
-void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result)
+void Logger::ack_vehicle_command(vehicle_command_s *cmd, uint32_t result)
 {
 	vehicle_command_ack_s vehicle_command_ack = {};
 	vehicle_command_ack.timestamp = hrt_absolute_time();
@@ -2348,13 +2339,8 @@ void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_
 	vehicle_command_ack.target_system = cmd->source_system;
 	vehicle_command_ack.target_component = cmd->source_component;
 
-	if (vehicle_command_ack_pub == nullptr) {
-		vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
-					  vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
-	}
+	uORB::PublicationQueued<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	cmd_ack_pub.publish(vehicle_command_ack);
 }
 
 int Logger::print_usage(const char *reason)
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index e76146bc1dbf..4816e589dcf1 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -357,8 +357,8 @@ class Logger : public ModuleBase<Logger>
 	 */
 	bool start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type);
 
-	void handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub);
-	void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result);
+	void handle_vehicle_command_update(int vehicle_command_sub);
+	void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result);
 
 	/**
 	 * initialize the output for the process load, so that ~1 second later it will be written to the log
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 946aa8cce446..a018c5251142 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -49,8 +49,9 @@
 #endif
 
 #include <lib/ecl/geo/geo.h>
-#include <mathlib/mathlib.h>
-#include <version/version.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/version/version.h>
+#include <uORB/PublicationQueued.hpp>
 
 #include "mavlink_receiver.h"
 #include "mavlink_main.h"
@@ -2126,7 +2127,7 @@ Mavlink::task_main(int argc, char *argv[])
 	cmd_sub->subscribe_from_beginning(true);
 
 	/* command ack */
-	orb_advert_t command_ack_pub = nullptr;
+	uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
 
@@ -2263,7 +2264,7 @@ Mavlink::task_main(int argc, char *argv[])
 				}
 
 				// send positive command ack
-				vehicle_command_ack_s command_ack = {};
+				vehicle_command_ack_s command_ack{};
 				command_ack.timestamp = vehicle_cmd.timestamp;
 				command_ack.command = vehicle_cmd.command;
 				command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
@@ -2271,13 +2272,7 @@ Mavlink::task_main(int argc, char *argv[])
 				command_ack.target_system = vehicle_cmd.source_system;
 				command_ack.target_component = vehicle_cmd.source_component;
 
-				if (command_ack_pub != nullptr) {
-					orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
-
-				} else {
-					command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-									      vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-				}
+				command_ack_pub.publish(command_ack);
 			}
 		}
 
@@ -2542,12 +2537,7 @@ void Mavlink::publish_telemetry_status()
 
 	_tstatus.timestamp = hrt_absolute_time();
 
-	if (_telem_status_pub == nullptr) {
-		_telem_status_pub = orb_advertise_queue(ORB_ID(telemetry_status), &_tstatus, 3);
-
-	} else {
-		orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &_tstatus);
-	}
+	_telem_status_pub.publish(_tstatus);
 }
 
 void Mavlink::check_radio_config()
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5ab8d0fb9b7e..2aab99e98e7f 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -71,11 +71,11 @@
 #include <px4_posix.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/uthash/utlist.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/radio_status.h>
 #include <uORB/topics/telemetry_status.h>
-#include <uORB/uORB.h>
 
 #include "mavlink_command_sender.h"
 #include "mavlink_messages.h"
@@ -540,7 +540,8 @@ class Mavlink : public ModuleParams
 	bool			_first_heartbeat_sent{false};
 
 	orb_advert_t		_mavlink_log_pub{nullptr};
-	orb_advert_t		_telem_status_pub{nullptr};
+
+	uORB::PublicationQueued<telemetry_status_s>	_telem_status_pub{ORB_ID(telemetry_status)};
 
 	bool			_task_running{false};
 	static bool		_boot_complete;
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index b76d09813738..32faf3acd4e6 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -270,7 +270,6 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
 	PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system);
 }
 
-
 void
 MavlinkMissionManager::send_mission_current(uint16_t seq)
 {
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 1290c9a5f750..b058ae5dbb4a 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -50,13 +50,6 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) :
 {
 }
 
-MavlinkParametersManager::~MavlinkParametersManager()
-{
-	if (_uavcan_parameter_request_pub) {
-		orb_unadvertise(_uavcan_parameter_request_pub);
-	}
-}
-
 unsigned
 MavlinkParametersManager::get_size()
 {
@@ -91,12 +84,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				req.node_id = req_list.target_component;
 				req.param_index = 0;
 
-				if (_uavcan_parameter_request_pub == nullptr) {
-					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
-
-				} else {
-					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
-				}
+				_uavcan_parameter_request_pub.publish(req);
 			}
 
 			break;
@@ -163,12 +151,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 					req.int_value = val;
 				}
 
-				if (_uavcan_parameter_request_pub == nullptr) {
-					_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
-
-				} else {
-					orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
-				}
+				_uavcan_parameter_request_pub.publish(req);
 			}
 
 			break;
@@ -576,12 +559,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter()
 	if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) {
 		uavcan_parameter_request_s req = _uavcan_open_request_list->req;
 
-		if (_uavcan_parameter_request_pub == nullptr) {
-			_uavcan_parameter_request_pub = orb_advertise_queue(ORB_ID(uavcan_parameter_request), &req, 5);
-
-		} else {
-			orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
-		}
+		_uavcan_parameter_request_pub.publish(req);
 
 		_uavcan_waiting_for_request_response = true;
 	}
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index 6c82a42cd838..4219a8533215 100644
--- a/src/modules/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -45,7 +45,7 @@
 #include <parameters/param.h>
 
 #include "mavlink_bridge_header.h"
-#include <uORB/uORB.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rc_parameter_map.h>
 #include <uORB/topics/uavcan_parameter_request.h>
@@ -59,7 +59,7 @@ class MavlinkParametersManager
 {
 public:
 	explicit MavlinkParametersManager(Mavlink *mavlink);
-	~MavlinkParametersManager();
+	~MavlinkParametersManager() = default;
 
 	/**
 	 * Handle sending of messages. Call this regularly at a fixed frequency.
@@ -128,7 +128,8 @@ class MavlinkParametersManager
 	orb_advert_t _rc_param_map_pub{nullptr};
 	rc_parameter_map_s _rc_param_map{};
 
-	orb_advert_t _uavcan_parameter_request_pub{nullptr};
+	uORB::PublicationQueued<uavcan_parameter_request_s> _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)};
+
 	uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
 
 	uORB::Subscription _mavlink_parameter_sub{ORB_ID(parameter_update)};
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index a723b51f7b97..818d0c0a4528 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -98,20 +98,14 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 void
 MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
 {
-	vehicle_command_ack_s command_ack = {};
+	vehicle_command_ack_s command_ack{};
 	command_ack.timestamp = hrt_absolute_time();
 	command_ack.command = command;
 	command_ack.result = result;
 	command_ack.target_system = sysid;
 	command_ack.target_component = compid;
 
-	if (_command_ack_pub == nullptr) {
-		_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-						       vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
-	}
+	_cmd_ack_pub.publish(command_ack);
 }
 
 void
@@ -515,12 +509,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 		}
 
 		if (!send_ack) {
-			if (_cmd_pub == nullptr) {
-				_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, vehicle_command_s::ORB_QUEUE_LENGTH);
-
-			} else {
-				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vehicle_command);
-			}
+			_cmd_pub.publish(vehicle_command);
 		}
 	}
 
@@ -547,13 +536,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
 	command_ack.target_system = ack.target_system;
 	command_ack.target_component = ack.target_component;
 
-	if (_command_ack_pub == nullptr) {
-		_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-						       vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
-	}
+	_cmd_ack_pub.publish(command_ack);
 
 	// TODO: move it to the same place that sent the command
 	if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) {
@@ -687,7 +670,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	union px4_custom_mode custom_mode;
 	custom_mode.data = new_mode.custom_mode;
 
-	vehicle_command_s vcmd = {};
+	vehicle_command_s vcmd{};
 	vcmd.timestamp = hrt_absolute_time();
 
 	/* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
@@ -703,12 +686,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	vcmd.confirmation = true;
 	vcmd.from_external = true;
 
-	if (_cmd_pub == nullptr) {
-		_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
-	}
+	_cmd_pub.publish(vcmd);
 }
 
 void
@@ -2274,12 +2252,7 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
 
 	//PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc);
 
-	if (_transponder_report_pub == nullptr) {
-		_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
-	}
+	_transponder_report_pub.publish(t);
 }
 
 void
@@ -2343,13 +2316,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
 
 	// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
 	// provide these.
-
-	if (_transponder_report_pub == nullptr) {
-		_transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t);
-	}
+	_transponder_report_pub.publish(t);
 
 	_last_utm_global_pos_com = t.timestamp;
 }
@@ -2392,16 +2359,7 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
 	memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
 	       math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len));
 
-	orb_advert_t &pub = _gps_inject_data_pub;
-
-	if (pub == nullptr) {
-		pub = orb_advertise_queue(ORB_ID(gps_inject_data), &gps_inject_data_topic,
-					  _gps_inject_data_queue_size);
-
-	} else {
-		orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic);
-	}
-
+	_gps_inject_data_pub.publish(gps_inject_data_topic);
 }
 
 void
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 23982cd5904b..08144cb427e5 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -43,6 +43,7 @@
 
 #include <px4_module_params.h>
 
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
@@ -71,6 +72,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -212,6 +214,11 @@ class MavlinkReceiver : public ModuleParams
 	vehicle_land_detected_s _hil_land_detector {};
 	vehicle_control_mode_s _control_mode {};
 
+	uORB::PublicationQueued<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
+	uORB::PublicationQueued<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
+	uORB::PublicationQueued<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+
 	orb_advert_t _accel_pub{nullptr};
 	orb_advert_t _actuator_controls_pubs[4] {nullptr, nullptr, nullptr, nullptr};
 	orb_advert_t _airspeed_pub{nullptr};
@@ -219,9 +226,7 @@ class MavlinkReceiver : public ModuleParams
 	orb_advert_t _attitude_pub{nullptr};
 	orb_advert_t _baro_pub{nullptr};
 	orb_advert_t _battery_pub{nullptr};
-	orb_advert_t _cmd_pub{nullptr};
 	orb_advert_t _collision_report_pub{nullptr};
-	orb_advert_t _command_ack_pub{nullptr};
 	orb_advert_t _debug_array_pub{nullptr};
 	orb_advert_t _debug_key_value_pub{nullptr};
 	orb_advert_t _debug_value_pub{nullptr};
@@ -231,7 +236,6 @@ class MavlinkReceiver : public ModuleParams
 	orb_advert_t _flow_pub{nullptr};
 	orb_advert_t _follow_target_pub{nullptr};
 	orb_advert_t _global_pos_pub{nullptr};
-	orb_advert_t _gps_inject_data_pub{nullptr};
 	orb_advert_t _gps_pub{nullptr};
 	orb_advert_t _gyro_pub{nullptr};
 	orb_advert_t _hil_distance_sensor_pub{nullptr};
@@ -249,15 +253,12 @@ class MavlinkReceiver : public ModuleParams
 	orb_advert_t _rates_sp_pub{nullptr};
 	orb_advert_t _rc_pub{nullptr};
 	orb_advert_t _trajectory_waypoint_pub{nullptr};
-	orb_advert_t _transponder_report_pub{nullptr};
 	orb_advert_t _visual_odometry_pub{nullptr};
 
 	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
 	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 
-	static constexpr int _gps_inject_data_queue_size{6};
-
 	static constexpr unsigned int MOM_SWITCH_COUNT{8};
 
 	int _orb_class_instance{-1};
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d2c7cfb30038..a4caeca89bdd 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -47,6 +47,7 @@
 #include <commander/px4_custom_mode.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/parameter_update.h>
@@ -108,7 +109,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
 	Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
 
 	orb_advert_t	_att_sp_pub{nullptr};			/**< attitude setpoint publication */
-	orb_advert_t _pub_vehicle_command{nullptr};           /**< vehicle command publication */
+	uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)};	 /**< vehicle command publication */
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	orb_id_t _attitude_setpoint_id{nullptr};
@@ -1038,13 +1039,7 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state)
 	}
 
 	// publish the vehicle command
-	if (_pub_vehicle_command == nullptr) {
-		_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
-				       vehicle_command_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
-	}
+	_pub_vehicle_command.publish(command);
 }
 
 int MulticopterPositionControl::task_spawn(int argc, char *argv[])
diff --git a/src/modules/muorb/test/muorb_test_example.cpp b/src/modules/muorb/test/muorb_test_example.cpp
index d8af5f81e539..364b24981f03 100644
--- a/src/modules/muorb/test/muorb_test_example.cpp
+++ b/src/modules/muorb/test/muorb_test_example.cpp
@@ -43,6 +43,9 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <px4_defines.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/Subscription.hpp>
 
 px4::AppState MuorbTestExample::appState;
 
@@ -50,7 +53,7 @@ int MuorbTestExample::main()
 {
 	int rc;
 	appState.setRunning(true);
-	rc =  PingPongTest();
+	rc = PingPongTest();
 	appState.setRunning(false);
 	return rc;
 }
@@ -58,63 +61,37 @@ int MuorbTestExample::main()
 int  MuorbTestExample::DefaultTest()
 {
 	int i = 0;
-	orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status);
 
-	if (pub_id == nullptr) {
-		PX4_ERR("error publishing esc_status");
-		return -1;
-	}
-
-	orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc);
-
-	if (pub_id_vc == nullptr) {
-		PX4_ERR("error publishing vehicle_command");
-		return -1;
-	}
-
-	if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
-		PX4_ERR("[%d]Error publishing the vechile command message", i);
-		return -1;
-	}
-
-	int sub_vc = orb_subscribe(ORB_ID(vehicle_command));
-
-	if (sub_vc == PX4_ERROR) {
-		PX4_ERR("Error subscribing to vehicle_command topic");
-		return -1;
-	}
+	uORB::Subscription sub_vc{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	uORB::Publication<esc_status_s> pub_id{ORB_ID(esc_status)};
+	pub_id.publish(m_esc_status);
 
 	while (!appState.exitRequested() && i < 100) {
 
 		PX4_DEBUG("[%d]  Doing work...", i);
 
-		if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) {
+		if (!pub_id.publish(m_esc_status)) {
 			PX4_ERR("[%d]Error publishing the esc status message for iter", i);
 			break;
 		}
 
-		bool updated = false;
-
-		if (orb_check(sub_vc, &updated) == 0) {
-			if (updated) {
-				PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
-
-				if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) {
-					PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i);
-					break;
-				}
+		if (sub_vc.updated()) {
+			PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
 
-				if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
-					PX4_ERR("[%d]Error publishing the vechile command message", i);
-					break;
-				}
+			if (!sub_vc.copy(&m_vc)) {
+				PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i);
+				break;
+			}
 
-			} else {
-				PX4_DEBUG("[%d] VC topic is not updated", i);
+			if (!vcmd_pub.publish(m_vc)) {
+				PX4_ERR("[%d]Error publishing the vechile command message", i);
+				break;
 			}
 
 		} else {
 			PX4_ERR("[%d]Error checking the updated status for vechile command... ", i);
+			PX4_DEBUG("[%d] VC topic is not updated", i);
 			break;
 		}
 
@@ -127,51 +104,28 @@ int  MuorbTestExample::DefaultTest()
 int MuorbTestExample::PingPongTest()
 {
 	int i = 0;
-	orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc);
-
-	if (pub_id_vc == nullptr) {
-		PX4_ERR("error publishing vehicle_command");
-		return -1;
-	}
-
-	if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
-		PX4_ERR("[%d]Error publishing the vechile command message", i);
-		return -1;
-	}
-
-	int sub_esc_status = orb_subscribe(ORB_ID(esc_status));
-
-	if (sub_esc_status == PX4_ERROR) {
-		PX4_ERR("Error subscribing to esc_status topic");
-		return -1;
-	}
+	uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	uORB::Subscription sub_esc_status{ORB_ID(esc_status)};
 
 	while (!appState.exitRequested()) {
 
 		PX4_INFO("[%d]  Doing work...", i);
-		bool updated = false;
-
-		if (orb_check(sub_esc_status, &updated) == 0) {
-			if (updated) {
-				PX4_INFO("[%d]ESC status is updated... reading new value", i);
 
-				if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) {
-					PX4_ERR("[%d]Error calling orb copy for esc status... ", i);
-					break;
-				}
+		if (sub_esc_status.updated()) {
+			PX4_INFO("[%d]ESC status is updated... reading new value", i);
 
-				if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
-					PX4_ERR("[%d]Error publishing the vechile command message", i);
-					break;
-				}
+			if (!sub_esc_status.copy(&m_esc_status)) {
+				PX4_ERR("[%d]Error calling orb copy for esc status... ", i);
+				break;
+			}
 
-			} else {
-				PX4_INFO("[%d] esc status topic is not updated", i);
+			if (!vcmd_pub.publish(m_vc)) {
+				PX4_ERR("[%d]Error publishing the vechile command message", i);
+				break;
 			}
 
 		} else {
-			PX4_ERR("[%d]Error checking the updated status for esc status... ", i);
-			break;
+			PX4_INFO("[%d] esc status topic is not updated", i);
 		}
 
 		// sleep for 1 sec.
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 7643b2cad24e..b0549c8a28cb 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -60,6 +60,7 @@
 #include <lib/perf/perf_counter.h>
 #include <px4_module.h>
 #include <px4_module_params.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/home_position.h>
@@ -330,8 +331,9 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 	uORB::Publication<vehicle_roi_s>		_vehicle_roi_pub{ORB_ID(vehicle_roi)};
 
 	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
-	orb_advert_t	_vehicle_cmd_ack_pub{nullptr};
-	orb_advert_t	_vehicle_cmd_pub{nullptr};
+
+	uORB::PublicationQueued<vehicle_command_ack_s>	_vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::PublicationQueued<vehicle_command_s>	_vehicle_cmd_pub{ORB_ID(vehicle_command)};
 
 	// Subscriptions
 	home_position_s					_home_pos{};		/**< home position for RTL */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 8aae5510aede..3f9b6336ebc1 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -903,7 +903,7 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi
 	// float vel_e = get_global_position()->vel_e;
 	// float vel_d = get_global_position()->vel_d;
 
-	transponder_report_s tr = {};
+	transponder_report_s tr{};
 	tr.timestamp = hrt_absolute_time();
 	tr.icao_address = 1234;
 	tr.lat = lat; // Latitude, expressed as degrees
@@ -923,8 +923,8 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi
 		   transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields
 	tr.squawk = 6667;
 
-	orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH);
-	(void)orb_unadvertise(h);
+	uORB::PublicationQueued<transponder_report_s> tr_pub{ORB_ID(transponder_report)};
+	tr_pub.publish(tr);
 }
 
 void Navigator::check_traffic()
@@ -1142,12 +1142,7 @@ Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 		break;
 	}
 
-	if (_vehicle_cmd_pub != nullptr) {
-		orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd);
-
-	} else {
-		_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
-	}
+	_vehicle_cmd_pub.publish(*vcmd);
 }
 
 void
@@ -1165,13 +1160,7 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res
 	command_ack.result_param1 = 0;
 	command_ack.result_param2 = 0;
 
-	if (_vehicle_cmd_ack_pub != nullptr) {
-		orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack);
-
-	} else {
-		_vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-				       vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-	}
+	_vehicle_cmd_ack_pub.publish(command_ack);
 }
 
 int Navigator::print_usage(const char *reason)
diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp
index dd874fe9e344..bfa67ba0209d 100644
--- a/src/modules/sensors/voted_sensors_update.cpp
+++ b/src/modules/sensors/voted_sensors_update.cpp
@@ -951,12 +951,7 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
 					_info.enabled = true;
 					_info.ok = false;
 
-					if (_info_pub == nullptr) {
-						_info_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH);
-
-					} else {
-						orb_publish(ORB_ID(subsystem_info), _info_pub, &_info);
-					}
+					_info_pub.publish(_info);
 				}
 			}
 		}
diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h
index f905690506eb..d008430fed11 100644
--- a/src/modules/sensors/voted_sensors_update.h
+++ b/src/modules/sensors/voted_sensors_update.h
@@ -52,6 +52,7 @@
 #include <lib/ecl/validation/data_validator.h>
 #include <lib/ecl/validation/data_validator_group.h>
 
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_preflight.h>
 #include <uORB/topics/sensor_correction.h>
@@ -274,8 +275,8 @@ class VotedSensorsUpdate
 	bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */
 
 	/* subsystem info publication */
-	struct subsystem_info_s _info;
-	orb_advert_t _info_pub = nullptr;
+	subsystem_info_s _info{};
+	uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)};
 
 	uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; /**< accel driver device id for each uorb instance */
 	uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {};
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index c146c5e8531c..debdce3f4470 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -45,8 +45,7 @@ namespace uORB
 {
 
 /**
- * Base publication wrapper class, used in list traversal
- * of various publications.
+ * Base publication wrapper class
  */
 template<typename T>
 class Publication
@@ -61,7 +60,7 @@ class Publication
 	 * @param priority The priority for multi pub/sub, 0-based, -1 means
 	 * 	don't publish as multi
 	 */
-	Publication(const orb_metadata *meta, int priority = -1) : _meta(meta), _priority(priority) {}
+	Publication(const orb_metadata *meta, uint8_t priority = 0) : _meta(meta), _priority(priority) {}
 
 	~Publication() { orb_unadvertise(_handle); }
 
@@ -74,12 +73,7 @@ class Publication
 		bool updated = false;
 
 		if (_handle != nullptr) {
-			if (orb_publish(_meta, _handle, &data) != PX4_OK) {
-				PX4_ERR("%s publish fail", _meta->o_name);
-
-			} else {
-				updated = true;
-			}
+			updated = (orb_publish(_meta, _handle, &data) == PX4_OK);
 
 		} else {
 			orb_advert_t handle = nullptr;
@@ -106,13 +100,14 @@ class Publication
 
 protected:
 	const orb_metadata *_meta;
-	const int _priority;
 
 	orb_advert_t _handle{nullptr};
+
+	const uint8_t _priority;
 };
 
 /**
- * The publication base class as a list node.
+ * The publication class with data.
  */
 template<typename T>
 class PublicationData : public Publication<T>
diff --git a/src/modules/uORB/PublicationQueued.hpp b/src/modules/uORB/PublicationQueued.hpp
new file mode 100644
index 000000000000..d3db3331069a
--- /dev/null
+++ b/src/modules/uORB/PublicationQueued.hpp
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PublicationQueued.hpp
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include <px4_defines.h>
+
+namespace uORB
+{
+
+/**
+ * Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH)
+ */
+template<typename T>
+class PublicationQueued
+{
+public:
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from
+	 * 	the ORB_ID() macro) for the topic.
+	 */
+	PublicationQueued(const orb_metadata *meta) : _meta(meta) {}
+
+	~PublicationQueued() { orb_unadvertise(_handle); }
+
+	/**
+	 * Publish the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	bool publish(const T &data)
+	{
+		bool updated = false;
+
+		if (_handle != nullptr) {
+			updated = (orb_publish(_meta, _handle, &data) == PX4_OK);
+
+		} else {
+			orb_advert_t handle = orb_advertise_queue(_meta, &data, T::ORB_QUEUE_LENGTH);
+
+			if (handle != nullptr) {
+				_handle = handle;
+				updated = true;
+
+			} else {
+				PX4_ERR("%s advertise failed", _meta->o_name);
+			}
+		}
+
+		return updated;
+	}
+
+protected:
+	const orb_metadata *_meta;
+
+	orb_advert_t _handle{nullptr};
+};
+
+} // namespace uORB
diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp
index 91b4f2dc7c3b..dbceb7d15f0c 100644
--- a/src/modules/vmount/input_mavlink.cpp
+++ b/src/modules/vmount/input_mavlink.cpp
@@ -39,7 +39,7 @@
  */
 
 #include "input_mavlink.h"
-#include <uORB/uORB.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -333,7 +333,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 
 void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
 {
-	vehicle_command_ack_s vehicle_command_ack = {};
+	vehicle_command_ack_s vehicle_command_ack{};
 
 	vehicle_command_ack.timestamp = hrt_absolute_time();
 	vehicle_command_ack.command = cmd->command;
@@ -341,13 +341,8 @@ void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
 	vehicle_command_ack.target_system = cmd->source_system;
 	vehicle_command_ack.target_component = cmd->source_component;
 
-	if (_vehicle_command_ack_pub == nullptr) {
-		_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
-					   vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack);
-	}
+	uORB::PublicationQueued<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	cmd_ack_pub.publish(vehicle_command_ack);
 }
 
 void InputMavlinkCmdMount::print_status()
diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h
index 424e1a60ebd8..b3661466ec75 100644
--- a/src/modules/vmount/input_mavlink.h
+++ b/src/modules/vmount/input_mavlink.h
@@ -43,13 +43,11 @@
 #include "input_rc.h"
 #include <cstdint>
 
-#include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_roi.h>
 
 namespace vmount
 {
-
-
 /**
  ** class InputMavlinkROI
  ** Input based on the vehicle_roi topic
@@ -95,7 +93,6 @@ class InputMavlinkCmdMount : public InputBase
 	void _ack_vehicle_command(vehicle_command_s *cmd);
 
 	int _vehicle_command_sub = -1;
-	orb_advert_t _vehicle_command_ack_pub = nullptr;
 	bool _stabilize[3] = { false, false, false };
 
 	int32_t _mav_sys_id{1}; ///< our mavlink system id
diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp
index 4e10867de8ed..1d508be2b10f 100644
--- a/src/modules/vmount/output_mavlink.cpp
+++ b/src/modules/vmount/output_mavlink.cpp
@@ -54,16 +54,9 @@ OutputMavlink::OutputMavlink(const OutputConfig &output_config)
 {
 }
 
-OutputMavlink::~OutputMavlink()
-{
-	if (_vehicle_command_pub) {
-		orb_unadvertise(_vehicle_command_pub);
-	}
-}
-
 int OutputMavlink::update(const ControlData *control_data)
 {
-	vehicle_command_s vehicle_command = {};
+	vehicle_command_s vehicle_command{};
 	vehicle_command.timestamp = hrt_absolute_time();
 	vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
 	vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
@@ -82,18 +75,7 @@ int OutputMavlink::update(const ControlData *control_data)
 			vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
 		}
 
-		if (_vehicle_command_pub) {
-			orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
-
-		} else {
-			_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command,
-					       vehicle_command_s::ORB_QUEUE_LENGTH);
-		}
-
-	}
-
-	if (!_vehicle_command_pub) {
-		return 0;
+		_vehicle_command_pub.publish(vehicle_command);
 	}
 
 	_handle_position_update();
@@ -110,7 +92,7 @@ int OutputMavlink::update(const ControlData *control_data)
 	vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F;
 	vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F;
 
-	orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
+	_vehicle_command_pub.publish(vehicle_command);
 
 	_last_update = t;
 
diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h
index 24c2272afa8f..b4a6ce3f0d96 100644
--- a/src/modules/vmount/output_mavlink.h
+++ b/src/modules/vmount/output_mavlink.h
@@ -41,13 +41,11 @@
 
 #include "output.h"
 
-#include <uORB/uORB.h>
-
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/vehicle_command.h>
 
 namespace vmount
 {
-
-
 /**
  ** class OutputMavlink
  *  Output via vehicle_command topic
@@ -56,14 +54,15 @@ class OutputMavlink : public OutputBase
 {
 public:
 	OutputMavlink(const OutputConfig &output_config);
-	virtual ~OutputMavlink();
+	virtual ~OutputMavlink() = default;
 
 	virtual int update(const ControlData *control_data);
 
 	virtual void print_status();
 
 private:
-	orb_advert_t _vehicle_command_pub = nullptr;
+
+	uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 };
 
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index e535e78f507b..de638dde1449 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -49,6 +49,7 @@
 #include "vtol_att_control_main.h"
 #include <systemlib/mavlink_log.h>
 #include <matrix/matrix/math.hpp>
+#include <uORB/PublicationQueued.hpp>
 
 using namespace matrix;
 
@@ -193,20 +194,15 @@ VtolAttitudeControl::handle_command()
 		// This might not be optimal but is better than no response at all.
 
 		if (_vehicle_cmd.from_external) {
-			vehicle_command_ack_s command_ack = {};
+			vehicle_command_ack_s command_ack{};
 			command_ack.timestamp = hrt_absolute_time();
 			command_ack.command = _vehicle_cmd.command;
 			command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
 			command_ack.target_system = _vehicle_cmd.source_system;
 			command_ack.target_component = _vehicle_cmd.source_component;
 
-			if (_v_cmd_ack_pub == nullptr) {
-				_v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
-								     vehicle_command_ack_s::ORB_QUEUE_LENGTH);
-
-			} else {
-				orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack);
-			}
+			uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
+			command_ack_pub.publish(command_ack);
 		}
 	}
 }
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 4a381b3ab65c..8d4ddcb2bcf1 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -144,7 +144,6 @@ class VtolAttitudeControl
 	orb_advert_t	_actuators_0_pub{nullptr};		//input for the mixer (roll,pitch,yaw,thrust)
 	orb_advert_t	_mavlink_log_pub{nullptr};	// mavlink log uORB handle
 	orb_advert_t	_v_att_sp_pub{nullptr};
-	orb_advert_t	_v_cmd_ack_pub{nullptr};
 	orb_advert_t	_vtol_vehicle_status_pub{nullptr};
 	orb_advert_t 	_actuators_1_pub{nullptr};
 
diff --git a/src/platforms/common/px4_work_queue/WorkQueueManager.hpp b/src/platforms/common/px4_work_queue/WorkQueueManager.hpp
index d53813f8eeb4..e9259e24282d 100644
--- a/src/platforms/common/px4_work_queue/WorkQueueManager.hpp
+++ b/src/platforms/common/px4_work_queue/WorkQueueManager.hpp
@@ -50,12 +50,12 @@ namespace wq_configurations
 {
 static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
 
-static constexpr wq_config_t SPI1{"wq:SPI1", 1250, -1};
-static constexpr wq_config_t SPI2{"wq:SPI2", 1250, -2};
-static constexpr wq_config_t SPI3{"wq:SPI3", 1250, -3};
-static constexpr wq_config_t SPI4{"wq:SPI4", 1250, -4};
-static constexpr wq_config_t SPI5{"wq:SPI5", 1250, -5};
-static constexpr wq_config_t SPI6{"wq:SPI6", 1250, -6};
+static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -1};
+static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -2};
+static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -3};
+static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -4};
+static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -5};
+static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -6};
 
 static constexpr wq_config_t I2C1{"wq:I2C1", 1250, -7};
 static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8};
diff --git a/src/systemcmds/led_control/led_control.cpp b/src/systemcmds/led_control/led_control.cpp
index 60e987c88b03..bd07fbb2cf1d 100644
--- a/src/systemcmds/led_control/led_control.cpp
+++ b/src/systemcmds/led_control/led_control.cpp
@@ -45,9 +45,10 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_led.h>
 
-static void	usage();
+#include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/led_control.h>
 
-static orb_advert_t led_control_pub = nullptr;
+static void	usage();
 
 extern "C" {
 	__EXPORT int led_control_main(int argc, char *argv[]);
@@ -57,12 +58,8 @@ static void publish_led_control(led_control_s &led_control)
 {
 	led_control.timestamp = hrt_absolute_time();
 
-	if (led_control_pub == nullptr) {
-		led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
-
-	} else {
-		orb_publish(ORB_ID(led_control), led_control_pub, &led_control);
-	}
+	uORB::PublicationQueued<led_control_s> led_control_pub{ORB_ID(led_control)};
+	led_control_pub.publish(led_control);
 }
 
 static void run_led_test1()
diff --git a/src/systemcmds/motor_test/CMakeLists.txt b/src/systemcmds/motor_test/CMakeLists.txt
index 27605cef54bb..d2937e6a01c0 100644
--- a/src/systemcmds/motor_test/CMakeLists.txt
+++ b/src/systemcmds/motor_test/CMakeLists.txt
@@ -36,7 +36,7 @@ px4_add_module(
 	STACK_MAIN 4096
 	COMPILE_FLAGS
 	SRCS
-		motor_test.c
+		motor_test.cpp
 	DEPENDS
 	)
 
diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.cpp
similarity index 86%
rename from src/systemcmds/motor_test/motor_test.c
rename to src/systemcmds/motor_test/motor_test.cpp
index 76b2e1e15006..0426316212b9 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.cpp
@@ -38,48 +38,30 @@
  * Tool for drive testing
  */
 
+#include <drivers/drv_hrt.h>
 #include <px4_config.h>
 #include <px4_getopt.h>
 #include <px4_log.h>
 #include <px4_module.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_hrt.h>
+#include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/test_motor.h>
 
-
-
-
-__EXPORT int motor_test_main(int argc, char *argv[]);
+extern "C" __EXPORT int motor_test_main(int argc, char *argv[]);
 
 static void motor_test(unsigned channel, float value);
 static void usage(const char *reason);
 
-static orb_advert_t _test_motor_pub = NULL;
-
 void motor_test(unsigned channel, float value)
 {
-	struct test_motor_s _test_motor;
-
-	_test_motor.motor_number = channel;
-	_test_motor.timestamp = hrt_absolute_time();
-	_test_motor.value = value;
+	test_motor_s test_motor{};
+	test_motor.timestamp = hrt_absolute_time();
+	test_motor.motor_number = channel;
+	test_motor.value = value;
 
-	if (_test_motor_pub != NULL) {
-		/* publish test state */
-		orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
-
-	} else {
-		/* advertise and publish */
-		_test_motor_pub = orb_advertise_queue(ORB_ID(test_motor), &_test_motor, 4);
-	}
+	uORB::PublicationQueued<test_motor_s> test_motor_pub{ORB_ID(test_motor)};
+	test_motor_pub.publish(test_motor);
 
-	printf("motor %d set to %.2f\n", channel, (double)value);
+	PX4_INFO("motor %d set to %.2f", channel, (double)value);
 }
 
 static void usage(const char *reason)