feat(mavlink): receive external gimbal manager information/status

PX4 today only produces GIMBAL_MANAGER_INFORMATION/STATUS (as its own gimbal
manager); it has no way to learn about a gimbal manager running on another
component. To interoperate with an external gimbal manager (e.g. a smart
camera-gimbal that runs its own manager), PX4 needs to discover it and track
who currently controls it.

Decode incoming GIMBAL_MANAGER_INFORMATION and GIMBAL_MANAGER_STATUS from
other components into the new external_gimbal_manager_information and
external_gimbal_manager_status uORB topics, carrying the manager's sysid and
compid from the message frame. Messages originating from our own
system/component are ignored so we don't ingest our own streamed manager
messages.

This is the awareness layer that a gimbal-manager client output builds on.

Signed-off-by: Julian Oes <julian@oes.ch>

msg/CMakeLists.txt

@@ -88,6 +88,8 @@ set(msg_files
 	EstimatorStatus.msg
 	EstimatorStatusFlags.msg
 	versioned/Event.msg
+	ExternalGimbalManagerInformation.msg
+	ExternalGimbalManagerStatus.msg
 	FigureEightStatus.msg
 	FailsafeFlags.msg
 	FailureDetectorStatus.msg

msg/ExternalGimbalManagerInformation.msg

@@ -0,0 +1,21 @@
+# Information about a gimbal manager running on another component (not us),
+# decoded from an incoming GIMBAL_MANAGER_INFORMATION message. Used to discover
+# external gimbal managers and their capabilities.
+
+uint64 timestamp						# time since system start (microseconds)
+
+uint8 manager_sysid						# system id of the external gimbal manager
+uint8 manager_compid					# component id of the external gimbal manager
+
+uint32 cap_flags						# GIMBAL_MANAGER_CAP_FLAGS bitmask
+
+uint8 gimbal_device_id					# gimbal device managed by this manager
+
+float32 roll_min # [rad]
+float32 roll_max # [rad]
+
+float32 pitch_min # [rad]
+float32 pitch_max # [rad]
+
+float32 yaw_min # [rad]
+float32 yaw_max # [rad]

msg/ExternalGimbalManagerStatus.msg

@@ -0,0 +1,15 @@
+# Status of a gimbal manager running on another component (not us), decoded from
+# an incoming GIMBAL_MANAGER_STATUS message. Used to track which component
+# currently controls an external gimbal manager.
+
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 manager_sysid		# system id of the external gimbal manager
+uint8 manager_compid	# component id of the external gimbal manager
+
+uint32 flags
+uint8 gimbal_device_id
+uint8 primary_control_sysid
+uint8 primary_control_compid
+uint8 secondary_control_sysid
+uint8 secondary_control_compid

src/modules/mavlink/mavlink_receiver.cpp

@@ -364,6 +364,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_gimbal_device_attitude_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION:
+		handle_message_gimbal_manager_information(msg);
+		break;
+
+	case MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS:
+		handle_message_gimbal_manager_status(msg);
+		break;
+
 #if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used
 
 	case MAVLINK_MSG_ID_SET_VELOCITY_LIMITS:
@@ -3694,6 +3702,59 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 	_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
 }
 
+void
+MavlinkReceiver::handle_message_gimbal_manager_information(mavlink_message_t *msg)
+{
+	// Ignore our own gimbal manager: PX4 streams this itself from the autopilot
+	// component, and we only care about external gimbal managers here.
+	if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+		return;
+	}
+
+	mavlink_gimbal_manager_information_t information_msg;
+	mavlink_msg_gimbal_manager_information_decode(msg, &information_msg);
+
+	external_gimbal_manager_information_s information{};
+	information.timestamp = hrt_absolute_time();
+	information.manager_sysid = msg->sysid;
+	information.manager_compid = msg->compid;
+	information.cap_flags = information_msg.cap_flags;
+	information.gimbal_device_id = information_msg.gimbal_device_id;
+	information.roll_min = information_msg.roll_min;
+	information.roll_max = information_msg.roll_max;
+	information.pitch_min = information_msg.pitch_min;
+	information.pitch_max = information_msg.pitch_max;
+	information.yaw_min = information_msg.yaw_min;
+	information.yaw_max = information_msg.yaw_max;
+
+	_external_gimbal_manager_information_pub.publish(information);
+}
+
+void
+MavlinkReceiver::handle_message_gimbal_manager_status(mavlink_message_t *msg)
+{
+	// Ignore our own gimbal manager (see handle_message_gimbal_manager_information).
+	if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+		return;
+	}
+
+	mavlink_gimbal_manager_status_t status_msg;
+	mavlink_msg_gimbal_manager_status_decode(msg, &status_msg);
+
+	external_gimbal_manager_status_s status{};
+	status.timestamp = hrt_absolute_time();
+	status.manager_sysid = msg->sysid;
+	status.manager_compid = msg->compid;
+	status.flags = status_msg.flags;
+	status.gimbal_device_id = status_msg.gimbal_device_id;
+	status.primary_control_sysid = status_msg.primary_control_sysid;
+	status.primary_control_compid = status_msg.primary_control_compid;
+	status.secondary_control_sysid = status_msg.secondary_control_sysid;
+	status.secondary_control_compid = status_msg.secondary_control_compid;
+
+	_external_gimbal_manager_status_pub.publish(status);
+}
+
 void MavlinkReceiver::handle_message_open_drone_id_basic_id(mavlink_message_t *msg)
 {
 	mavlink_open_drone_id_basic_id_t odid_module {};

src/modules/mavlink/mavlink_receiver.h

@@ -83,6 +83,9 @@
 #include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/gimbal_device_attitude_status.h>
 #include <uORB/topics/rtcm_data.h>
+#include <uORB/topics/external_gimbal_manager_information.h>
+#include <uORB/topics/external_gimbal_manager_status.h>
+#include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/irlock_report.h>
@@ -238,6 +241,8 @@ private:
 	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
 	void handle_message_gimbal_device_information(mavlink_message_t *msg);
 	void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg);
+	void handle_message_gimbal_manager_information(mavlink_message_t *msg);
+	void handle_message_gimbal_manager_status(mavlink_message_t *msg);
 	void handle_message_global_position_sensor(mavlink_message_t *msg);
 #if defined(MAVLINK_MSG_ID_RANGING_BEACON)
 	void handle_message_ranging_beacon(mavlink_message_t *msg);
@@ -348,6 +353,8 @@ private:
 	uORB::Publication<gimbal_manager_set_manual_control_s>	_gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};
 	uORB::Publication<gimbal_device_information_s>		_gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
 	uORB::Publication<gimbal_device_attitude_status_s>	_gimbal_device_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
+	uORB::Publication<external_gimbal_manager_information_s>	_external_gimbal_manager_information_pub{ORB_ID(external_gimbal_manager_information)};
+	uORB::Publication<external_gimbal_manager_status_s>	_external_gimbal_manager_status_pub{ORB_ID(external_gimbal_manager_status)};
 	uORB::Publication<irlock_report_s>			_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<landing_target_pose_s>		_landing_target_pose_pub{ORB_ID(landing_target_pose)};
 	uORB::Publication<log_message_s>			_log_message_pub{ORB_ID(log_message)};