introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

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::Run()
 {
-	CameraCapture *dev = reinterpret_cast<CameraCapture *>(arg);
-
-	dev->cycle();
-}
-
-void
-CameraCapture::cycle()
-{
-
-	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
-			};
+			vehicle_command_ack_s command_ack{};
 
-			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);
+			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;
 
-			} else {
-				orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack);
-
-			}
+			_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 @@ err_out:
 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) {

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

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

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

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

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

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

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

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

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