mavlink: use dynamic camera comp id instead of hardcoded value

msg/CMakeLists.txt

@@ -47,6 +47,7 @@ set(msg_files
 	baro_bias_estimate.msg
 	battery_status.msg
 	camera_capture.msg
+	camera_status.msg
 	camera_trigger.msg
 	cellular_status.msg
 	collision_constraints.msg

msg/camera_status.msg

@@ -0,0 +1,4 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 active_sys_id		# mavlink system id of the currently active camera
+uint8 active_comp_id 	# mavlink component id of currently active camera

msg/tools/uorb_rtps_message_ids.yaml

@@ -347,6 +347,8 @@ rtps:
     id: 161
   - msg: internal_combustion_engine_status
     id: 162
+  - msg: camera_status
+    id: 163
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 180

src/modules/mavlink/mavlink_receiver.cpp

@@ -2001,6 +2001,8 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 		if (same_system || hb.type == MAV_TYPE_GCS) {
 
+			camera_status_s camera_status{};
+
 			switch (hb.type) {
 			case MAV_TYPE_ANTENNA_TRACKER:
 				_heartbeat_type_antenna_tracker = now;
@@ -2024,6 +2026,10 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 			case MAV_TYPE_CAMERA:
 				_heartbeat_type_camera = now;
+				camera_status.timestamp = now;
+				camera_status.active_comp_id = msg->compid;
+				camera_status.active_sys_id = msg->sysid;
+				_camera_status_pub.publish(camera_status);
 				break;
 
 			default:

src/modules/mavlink/mavlink_receiver.h

@@ -62,6 +62,7 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/camera_status.h>
 #include <uORB/topics/cellular_status.h>
 #include <uORB/topics/collision_report.h>
 #include <uORB/topics/differential_pressure.h>
@@ -266,6 +267,7 @@ private:
 	uORB::Publication<actuator_controls_s>			_actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)};
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
+	uORB::Publication<camera_status_s>					_camera_status_pub{ORB_ID(camera_status)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};

src/modules/mavlink/streams/CAMERA_TRIGGER.hpp

@@ -34,6 +34,7 @@
 #ifndef CAMERA_TRIGGER_HPP
 #define CAMERA_TRIGGER_HPP
 
+#include <uORB/topics/camera_status.h>
 #include <uORB/topics/camera_trigger.h>
 
 class MavlinkStreamCameraTrigger : public MavlinkStream
@@ -63,6 +64,12 @@ private:
 	explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
 	uORB::Subscription _camera_trigger_sub{ORB_ID(camera_trigger)};
+	uORB::Subscription _camera_status_sub{ORB_ID(camera_status)};
+	camera_status_s _camera_status = {
+		0,	//timestamp
+		0,	//target_sys_id
+		MAV_COMP_ID_CAMERA // active_comp_id
+	};
 
 	bool send() override
 	{
@@ -76,6 +83,7 @@ private:
 				msg.seq = camera_trigger.seq;
 				mavlink_msg_camera_trigger_send_struct(_mavlink->get_channel(), &msg);
 
+				_camera_status_sub.update(&_camera_status);
 
 				vehicle_command_s vcmd{};
 				vcmd.timestamp = hrt_absolute_time();
@@ -88,7 +96,7 @@ private:
 				vcmd.param7 = NAN;
 				vcmd.command = MAV_CMD_IMAGE_START_CAPTURE;
 				vcmd.target_system = mavlink_system.sysid;
-				vcmd.target_component = MAV_COMP_ID_CAMERA;
+				vcmd.target_component = _camera_status.active_comp_id;
 
 				MavlinkCommandSender::instance().handle_vehicle_command(vcmd, _mavlink->get_channel());
 
@@ -97,8 +105,8 @@ private:
 				/* send MAV_CMD_DO_DIGICAM_CONTROL*/
 				mavlink_command_long_t command_long_msg{};
 
-				command_long_msg.target_system = 0; // 0 for broadcast
-				command_long_msg.target_component = MAV_COMP_ID_CAMERA;
+				command_long_msg.target_system = _camera_status.active_sys_id;
+				command_long_msg.target_component = _camera_status.active_comp_id;
 				command_long_msg.command = MAV_CMD_DO_DIGICAM_CONTROL;
 				command_long_msg.confirmation = 0;
 				command_long_msg.param1 = NAN;