introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

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

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 @@ private:
 	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 @@ private:
 
 	// 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_ */

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

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

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

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 @@ private:
 	/** @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. */

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 */

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 @@ private:
 	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;
 };
 

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 @@ public:
 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)

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)

src/modules/logger/logger.h

@@ -357,8 +357,8 @@ private:
 	 */
 	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

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

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 @@ private:
 	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;

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

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;
@@ -577,12 +560,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;
 	}

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 @@ protected:
 	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)};

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

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 @@ private:
 	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 @@ private:
 	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 @@ private:
 	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 @@ private:
 	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};

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 @@ private:
 	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[])

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 (sub_vc.updated()) {
+			PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
 
-		if (orb_check(sub_vc, &updated) == 0) {
-			if (updated) {
-				PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
+			if (!sub_vc.copy(&m_vc)) {
+				PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i);
+				break;
+			}
 
-				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 (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) {
-					PX4_ERR("[%d]Error publishing the vechile command message", 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 (sub_esc_status.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.copy(&m_esc_status)) {
+				PX4_ERR("[%d]Error calling orb copy for esc status... ", i);
+				break;
+			}
 
-				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;
-				}
-
-			} 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.

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 @@ private:
 	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 */

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)

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

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 @@ private:
 	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] = {};

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 @@ public:
 	 * @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 @@ public:
 		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 @@ public:
 
 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>

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

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

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

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;
 

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

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

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -144,7 +144,6 @@ private:
 	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};