Fix camera trigger via MAVLink when camera capture feedback is enabled

- camera_trigger module always publishes the camera_trigger msg (independent of the camera feedback)
- Use camera_trigger msg and set the feedback flag
- Subscribing modules determine if the message is relevant based on the feedback message

msg/camera_trigger.msg

@@ -4,4 +4,6 @@ uint64 timestamp_utc # UTC timestamp
 uint32 seq		# Image sequence number
 bool feedback	# Trigger feedback from camera
 
+uint32 ORB_QUEUE_LENGTH = 2
+
 # TOPICS camera_trigger

src/drivers/camera_trigger/camera_trigger.cpp

@@ -200,10 +200,8 @@ private:
 	param_t			_p_min_interval;
 	param_t			_p_distance;
 	param_t			_p_interface;
-	param_t 		_p_cam_cap_fback;
 
 	trigger_mode_t		_trigger_mode{TRIGGER_MODE_NONE};
-	int32_t _cam_cap_fback{0};
 
 	camera_interface_mode_t	_camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO};
 	CameraInterface		*_camera_interface{nullptr};  ///< instance of camera interface
@@ -266,7 +264,6 @@ CameraTrigger::CameraTrigger() :
 	_p_activation_time = param_find("TRIG_ACT_TIME");
 	_p_mode = param_find("TRIG_MODE");
 	_p_interface = param_find("TRIG_INTERFACE");
-	_p_cam_cap_fback = param_find("CAM_CAP_FBACK");
 
 	param_get(_p_activation_time, &_activation_time);
 	param_get(_p_interval, &_interval);
@@ -275,10 +272,6 @@ CameraTrigger::CameraTrigger() :
 	param_get(_p_mode, (int32_t *)&_trigger_mode);
 	param_get(_p_interface, (int32_t *)&_camera_interface_mode);
 
-	if (_p_cam_cap_fback != PARAM_INVALID) {
-		param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
-	}
-
 	switch (_camera_interface_mode) {
 #ifdef __PX4_NUTTX
 
@@ -320,10 +313,7 @@ CameraTrigger::CameraTrigger() :
 	// Advertise critical publishers here, because we cannot advertise in interrupt context
 	camera_trigger_s trigger{};
 
-	if (!_cam_cap_fback) {
+	_trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH);
-		_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
-
-	}
 }
 
 CameraTrigger::~CameraTrigger()
@@ -835,30 +825,26 @@ CameraTrigger::engage(void *arg)
 		return;
 	}
 
-	// Publish only if  _cam_cap_fback is disabled, otherwise, it is published over camera_capture driver
+	pps_capture_s pps_capture;
-	if (!trig->_cam_cap_fback) {
 
-		pps_capture_s pps_capture;
+	if (trig->_pps_capture_sub.update(&pps_capture)) {
-
+		trig->_rtc_drift_time = pps_capture.rtc_drift_time;
-		if (trig->_pps_capture_sub.update(&pps_capture)) {
-			trig->_rtc_drift_time = pps_capture.rtc_drift_time;
-		}
-
-		// Send camera trigger message. This messages indicates that we sent
-		// the camera trigger request. Does not guarantee capture.
-		camera_trigger_s trigger{};
-
-		timespec tv{};
-		px4_clock_gettime(CLOCK_REALTIME, &tv);
-		trigger.timestamp_utc = ts_to_abstime(&tv) + trig->_rtc_drift_time;
-
-		trigger.seq = trig->_trigger_seq;
-		trigger.feedback = false;
-		trigger.timestamp = hrt_absolute_time();
-
-		orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
 	}
 
+	// Send camera trigger message. This messages indicates that we sent
+	// the camera trigger request. Does not guarantee capture.
+	camera_trigger_s trigger{};
+
+	timespec tv{};
+	px4_clock_gettime(CLOCK_REALTIME, &tv);
+	trigger.timestamp_utc = ts_to_abstime(&tv) + trig->_rtc_drift_time;
+
+	trigger.seq = trig->_trigger_seq;
+	trigger.feedback = false;
+	trigger.timestamp = hrt_absolute_time();
+
+	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
+
 	// increment frame count
 	trig->_trigger_seq++;
 }

src/modules/camera_feedback/CameraFeedback.cpp

@@ -37,6 +37,11 @@ CameraFeedback::CameraFeedback() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
+	_p_cam_cap_fback = param_find("CAM_CAP_FBACK");
+
+	if (_p_cam_cap_fback != PARAM_INVALID) {
+		param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
+	}
 }
 
 bool
@@ -63,7 +68,7 @@ CameraFeedback::Run()
 
 	camera_trigger_s trig{};
 
-	if (_trigger_sub.update(&trig)) {
+	while (_trigger_sub.update(&trig)) {
 
 		// update geotagging subscriptions
 		vehicle_global_position_s gpos{};
@@ -77,7 +82,12 @@ CameraFeedback::Run()
 		    att.timestamp == 0) {
 
 			// reject until we have valid data
-			return;
+			continue;
+		}
+
+		if ((_cam_cap_fback >= 1) && !trig.feedback) {
+			// Ignore triggers that are not feedback when camera capture feedback is enabled
+			continue;
 		}
 
 		camera_capture_s capture{};

src/modules/camera_feedback/CameraFeedback.hpp

@@ -83,4 +83,7 @@ private:
 	uORB::Subscription	_att_sub{ORB_ID(vehicle_attitude)};
 
 	uORB::Publication<camera_capture_s>	_capture_pub{ORB_ID(camera_capture)};
+
+	param_t _p_cam_cap_fback;
+	int32_t _cam_cap_fback{0};
 };

src/modules/mavlink/streams/CAMERA_TRIGGER.hpp

@@ -77,8 +77,8 @@ private:
 		camera_trigger_s camera_trigger;
 
 		if ((_mavlink->get_free_tx_buf() >= get_size()) && _camera_trigger_sub.update(&camera_trigger)) {
-			/* ensure that only active trigger events are sent */
+			/* ensure that only active trigger events are sent and ignore camera capture feedback messages*/
-			if (camera_trigger.timestamp > 0) {
+			if (camera_trigger.timestamp > 0 && !camera_trigger.feedback) {
 				mavlink_camera_trigger_t msg{};
 				msg.time_usec = camera_trigger.timestamp;
 				msg.seq = camera_trigger.seq;