introduce uORB::PublicationQueued and transition most orb_advertise_queue usage

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

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

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

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

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

msg/telemetry_status.msg

@@ -55,3 +55,5 @@ float32 rate_rx
 
 float32 rate_tx
 float32 rate_txerr
+
+uint8 ORB_QUEUE_LENGTH = 3

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

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

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

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

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,41 +193,25 @@ 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
 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,18 +305,17 @@ 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;
 }
 
 void
 CameraCapture::stop()
 {
+	ScheduleClear();
 
-	work_cancel(LPWORK, &_work);
 	work_cancel(HPWORK, &_work_publisher);
 
 	if (camera_capture::g_camera_capture != nullptr) {