vmount/navigator/mavlink: gimbal v2 changes

This is a collection of commits all having to do with changes in the
Mavlink gimbal v2 protocol as described in:
https://mavlink.io/en/services/gimbal_v2.html

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -243,7 +243,7 @@ then
 	sh etc/init.d-posix/rc.mavlink_override
 else
 	# GCS link
-	mavlink start -x -u $udp_gcs_port_local -r 4000000
+	mavlink start -x -u $udp_gcs_port_local -r 4000000 -f
 	mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local
 	mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local
 	mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local
@@ -255,7 +255,7 @@ else
 	mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
 
 	# API/Offboard link
-	mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
+	mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote
 
 	# Onboard link to camera
 	mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote

msg/CMakeLists.txt

@@ -68,6 +68,7 @@ set(msg_files
 	geofence_result.msg
 	gimbal_device_set_attitude.msg
 	gimbal_manager_set_attitude.msg
+	gimbal_manager_set_manual_control.msg
 	gimbal_device_attitude_status.msg
 	gimbal_device_information.msg
 	gimbal_manager_information.msg

msg/gimbal_device_information.msg

@@ -18,10 +18,10 @@ uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256
 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512
 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024
 
-float32 tilt_max # [rad]
-float32 tilt_min # [rad]
-float32 tilt_rate_max # [rad / s]
+float32 pitch_max # [rad]
+float32 pitch_min # [rad]
 
-float32 pan_max # [rad]
-float32 pan_min # [rad]
-float32 pan_rate_max # [rad / s]
+float32 yaw_max # [rad]
+float32 yaw_min # [rad]
+
+uint8 gimbal_device_compid

msg/gimbal_manager_set_attitude.msg

@@ -1,5 +1,8 @@
 uint64 timestamp						# time since system start (microseconds)
 
+uint8 origin_sysid
+uint8 origin_compid
+
 uint8 target_system
 uint8 target_component
 
@@ -8,10 +11,6 @@ uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2
 uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4
 uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8
 uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16
-uint32 GIMBAL_MANAGER_FLAGS_ANGULAR_VELOCITY_RELATIVE_TO_FOCAL_LENGTH = 1048576
-uint32 GIMBAL_MANAGER_FLAGS_NUDGE = 2097152
-uint32 GIMBAL_MANAGER_FLAGS_OVERRIDE = 4194304
-uint32 GIMBAL_MANAGER_FLAGS_NONE = 8388608
 
 uint32 flags
 uint8 gimbal_device_id

msg/gimbal_manager_set_manual_control.msg

@@ -0,0 +1,21 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint8 origin_sysid
+uint8 origin_compid
+
+uint8 target_system
+uint8 target_component
+
+uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1
+uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2
+uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4
+uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8
+uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16
+
+uint32 flags
+uint8 gimbal_device_id
+
+float32 pitch      # unitless -1..1, can be NAN
+float32 yaw        # unitless -1..1, can be NAN
+float32 pitch_rate # unitless -1..1, can be NAN
+float32 yaw_rate   # unitless -1..1, can be NAN

msg/gimbal_manager_status.msg

@@ -2,3 +2,7 @@ uint64 timestamp		# time since system start (microseconds)
 
 uint32 flags
 uint8 gimbal_device_id
+uint8 primary_control_sysid
+uint8 primary_control_compid
+uint8 secondary_control_sysid
+uint8 secondary_control_compid

msg/vehicle_command.msg

@@ -75,7 +75,9 @@ uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum|
 uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    # Request to send a single instance of the specified message
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530            # Set camera capture mode (photo, video, etc.)
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531                # Set camera zoom
-uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_ATTITUDE = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal attitude
+uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
+uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
+uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control
 uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-board camera triggering system
 uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
 uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data

src/modules/commander/Commander.cpp

@@ -1258,6 +1258,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE:
 	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM:
+	case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_FOCUS:
 	case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
 	case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
 	case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:

src/modules/mavlink/mavlink_messages.cpp

@@ -1482,6 +1482,111 @@ protected:
 	}
 };
 
+class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamAutopilotStateForGimbalDevice::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamAutopilotStateForGimbalDevice(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return _att_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Subscription _est_sub{ORB_ID(estimator_status)};
+	uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)};
+
+	/* do not allow top copying this class */
+	MavlinkStreamAutopilotStateForGimbalDevice(MavlinkStreamAutopilotStateForGimbalDevice &) = delete;
+	MavlinkStreamAutopilotStateForGimbalDevice &operator = (const MavlinkStreamAutopilotStateForGimbalDevice &) = delete;
+
+protected:
+	explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		if (_att_sub.advertised()) {
+
+			mavlink_autopilot_state_for_gimbal_device_t msg{};
+
+			{
+				vehicle_attitude_s att{};
+				_att_sub.copy(&att);
+				msg.time_boot_us = att.timestamp;
+				msg.q[0] = att.q[0];
+				msg.q[1] = att.q[1];
+				msg.q[2] = att.q[2];
+				msg.q[3] = att.q[3];
+				msg.q_estimated_delay_us = 0; // I don't know.
+			}
+
+			{
+				vehicle_local_position_s lpos{};
+				_lpos_sub.copy(&lpos);
+				msg.vx = lpos.vx;
+				msg.vy = lpos.vy;
+				msg.vz = lpos.vz;
+				msg.v_estimated_delay_us = 0; // I don't know.
+			}
+
+			{
+				vehicle_attitude_setpoint_s att_sp{};
+				_att_sp_sub.copy(&att_sp);
+				msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate;
+			}
+
+			{
+				estimator_status_s est{};
+				_est_sub.copy(&est);
+				msg.estimator_status = est.solution_status_flags;
+			}
+
+			{
+				vehicle_land_detected_s land_detected{};
+				_landed_sub.copy(&land_detected);
+
+				// Ignore take-off and landing states for now.
+				msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR;
+			}
+
+			mavlink_msg_autopilot_state_for_gimbal_device_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+
+		}
+
+		return false;
+	}
+};
+
+
 class MavlinkStreamSystemTime : public MavlinkStream
 {
 public:
@@ -3342,6 +3447,7 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamGimbalDeviceAttitudeStatus>(),
 	create_stream_list_item<MavlinkStreamGimbalManagerInformation>(),
 	create_stream_list_item<MavlinkStreamGimbalManagerStatus>(),
+	create_stream_list_item<MavlinkStreamAutopilotStateForGimbalDevice>(),
 	create_stream_list_item<MavlinkStreamGimbalDeviceSetAttitude>(),
 	create_stream_list_item<MavlinkStreamHomePosition>(),
 	create_stream_list_item<MavlinkStreamServoOutputRaw<0> >(),

src/modules/mavlink/mavlink_mission.cpp

@@ -1475,6 +1475,9 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_DO_DIGICAM_CONTROL:
 		case MAV_CMD_DO_MOUNT_CONFIGURE:
 		case MAV_CMD_DO_MOUNT_CONTROL:
+		case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+		case NAV_CMD_SET_CAMERA_FOCUS:
+		case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
 		case MAV_CMD_IMAGE_START_CAPTURE:
 		case MAV_CMD_IMAGE_STOP_CAPTURE:
 		case MAV_CMD_VIDEO_START_CAPTURE:
@@ -1574,12 +1577,15 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		case NAV_CMD_DO_CONTROL_VIDEO:
 		case NAV_CMD_DO_MOUNT_CONFIGURE:
 		case NAV_CMD_DO_MOUNT_CONTROL:
+		case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+		case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
 		case NAV_CMD_DO_SET_ROI:
 		case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
 		case NAV_CMD_OBLIQUE_SURVEY:
 		case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 		case NAV_CMD_SET_CAMERA_MODE:
 		case NAV_CMD_SET_CAMERA_ZOOM:
+		case NAV_CMD_SET_CAMERA_FOCUS:
 		case NAV_CMD_DO_VTOL_TRANSITION:
 			break;
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -283,6 +283,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_gimbal_manager_set_attitude(msg);
 		break;
 
+	case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL:
+		handle_message_gimbal_manager_set_manual_control(msg);
+		break;
+
 	case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
 		handle_message_gimbal_device_information(msg);
 		break;
@@ -2946,16 +2950,39 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg)
+{
+	mavlink_gimbal_manager_set_manual_control_t set_manual_control_msg;
+	mavlink_msg_gimbal_manager_set_manual_control_decode(msg, &set_manual_control_msg);
+
+	gimbal_manager_set_manual_control_s set_manual_control{};
+	set_manual_control.timestamp = hrt_absolute_time();
+	set_manual_control.origin_sysid = msg->sysid;
+	set_manual_control.origin_compid = msg->compid;
+	set_manual_control.target_system = set_manual_control_msg.target_system;
+	set_manual_control.target_component = set_manual_control_msg.target_component;
+	set_manual_control.flags = set_manual_control_msg.flags;
+	set_manual_control.gimbal_device_id = set_manual_control_msg.gimbal_device_id;
+
+	set_manual_control.pitch = set_manual_control_msg.pitch;
+	set_manual_control.yaw = set_manual_control_msg.yaw;
+	set_manual_control.pitch_rate = set_manual_control_msg.pitch_rate;
+	set_manual_control.yaw_rate = set_manual_control_msg.yaw_rate;
+
+	_gimbal_manager_set_manual_control_pub.publish(set_manual_control);
+}
 
 void
 MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg)
 {
-
 	mavlink_gimbal_manager_set_attitude_t set_attitude_msg;
 	mavlink_msg_gimbal_manager_set_attitude_decode(msg, &set_attitude_msg);
 
 	gimbal_manager_set_attitude_s gimbal_attitude{};
 	gimbal_attitude.timestamp = hrt_absolute_time();
+	gimbal_attitude.origin_sysid = msg->sysid;
+	gimbal_attitude.origin_compid = msg->compid;
 	gimbal_attitude.target_system = set_attitude_msg.target_system;
 	gimbal_attitude.target_component = set_attitude_msg.target_component;
 	gimbal_attitude.flags = set_attitude_msg.flags;
@@ -2969,7 +2996,6 @@ MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *m
 	gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z;
 
 	_gimbal_manager_set_attitude_pub.publish(gimbal_attitude);
-
 }
 
 void
@@ -2994,13 +3020,13 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
 	gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version;
 	gimbal_information.capability_flags = gimbal_device_info_msg.cap_flags;
 
-	gimbal_information.tilt_max = gimbal_device_info_msg.tilt_max;
-	gimbal_information.tilt_min = gimbal_device_info_msg.tilt_min;
-	gimbal_information.tilt_rate_max = gimbal_device_info_msg.tilt_rate_max;
+	gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max;
+	gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min;
+
+	gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
+	gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
 
-	gimbal_information.pan_max = gimbal_device_info_msg.pan_max;
-	gimbal_information.pan_min = gimbal_device_info_msg.pan_min;
-	gimbal_information.pan_rate_max = gimbal_device_info_msg.pan_rate_max;
+	gimbal_information.gimbal_device_compid = msg->compid;
 
 	_gimbal_device_information_pub.publish(gimbal_information);
 

src/modules/mavlink/mavlink_receiver.h

@@ -68,6 +68,7 @@
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/generator_status.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
+#include <uORB/topics/gimbal_manager_set_manual_control.h>
 #include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/home_position.h>
@@ -205,6 +206,7 @@ private:
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_onboard_computer_status(mavlink_message_t *msg);
 	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
+	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
 	void handle_message_gimbal_device_information(mavlink_message_t *msg);
 
 #if !defined(CONSTRAINED_FLASH)
@@ -357,6 +359,9 @@ private:
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
 	uORB::Publication<follow_target_s>			_follow_target_pub{ORB_ID(follow_target)};
+	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
+	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<irlock_report_s>			_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
 	uORB::Publication<gimbal_device_information_s>		_gimbal_device_information_pub{ORB_ID(gimbal_device_information)};

src/modules/navigator/mission.cpp

@@ -146,13 +146,15 @@ void
 Mission::on_inactivation()
 {
 	// Disable camera trigger
-	vehicle_command_s cmd = {};
+	vehicle_command_s cmd {};
 	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
 	// Pause trigger
 	cmd.param1 = -1.0f;
 	cmd.param3 = 1.0f;
 	_navigator->publish_vehicle_cmd(&cmd);
 
+	_navigator->release_gimbal_control();
+
 	if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
 	}

src/modules/navigator/mission_block.cpp

@@ -96,6 +96,8 @@ MissionBlock::is_mission_item_reached()
 	case NAV_CMD_DO_CONTROL_VIDEO:
 	case NAV_CMD_DO_MOUNT_CONFIGURE:
 	case NAV_CMD_DO_MOUNT_CONTROL:
+	case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+	case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
 	case NAV_CMD_DO_SET_ROI:
 	case NAV_CMD_DO_SET_ROI_LOCATION:
 	case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
@@ -105,6 +107,7 @@ MissionBlock::is_mission_item_reached()
 	case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
 	case NAV_CMD_SET_CAMERA_MODE:
 	case NAV_CMD_SET_CAMERA_ZOOM:
+	case NAV_CMD_SET_CAMERA_FOCUS:
 		return true;
 
 	case NAV_CMD_DO_VTOL_TRANSITION:
@@ -472,7 +475,10 @@ MissionBlock::issue_command(const mission_item_s &item)
 	} else {
 		_action_start = hrt_absolute_time();
 
-		// mission_item -> vehicle_command
+		// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
+		if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
+			_navigator->acquire_gimbal_control();
+		}
 
 		// we're expecting a mission command item here so assign the "raw" inputs to the command
 		// (MAV_FRAME_MISSION mission item)

src/modules/navigator/mission_feasibility_checker.cpp

@@ -262,6 +262,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
 		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
 		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
+		    missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
+		    missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
@@ -271,6 +273,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
+		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
 		    missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
 
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1),
@@ -389,6 +392,8 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 					  missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
 					  missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
 					  missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
+					  missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
+					  missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
@@ -398,6 +403,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 					  missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
+					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
 					  missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION);
 		}
 	}

src/modules/navigator/navigation.h

@@ -86,6 +86,9 @@ enum NAV_CMD {
 	NAV_CMD_OBLIQUE_SURVEY = 260,
 	NAV_CMD_SET_CAMERA_MODE = 530,
 	NAV_CMD_SET_CAMERA_ZOOM = 531,
+	NAV_CMD_SET_CAMERA_FOCUS = 532,
+	NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000,
+	NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001,
 	NAV_CMD_IMAGE_START_CAPTURE = 2000,
 	NAV_CMD_IMAGE_STOP_CAPTURE = 2001,
 	NAV_CMD_DO_TRIGGER_CONTROL = 2003,

src/modules/navigator/navigator.h

@@ -302,6 +302,9 @@ public:
 
 	bool		force_vtol();
 
+	void		acquire_gimbal_control();
+	void		release_gimbal_control();
+
 private:
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,	/**< loiter radius for fixedwing */
@@ -403,7 +406,6 @@ private:
 	float _mission_cruising_speed_fw{-1.0f};
 	float _mission_throttle{NAN};
 
-
 	bool _mission_landing_in_progress{false};	// this flag gets set if the mission is currently executing on a landing pattern
 	// if mission mode is inactive, this flag will be cleared after 2 seconds
 

src/modules/navigator/navigator_main.cpp

@@ -1374,6 +1374,30 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res
 	_vehicle_cmd_ack_pub.publish(command_ack);
 }
 
+void
+Navigator::acquire_gimbal_control()
+{
+	vehicle_command_s vcmd = {};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+	vcmd.param1 = _vstatus.system_id;
+	vcmd.param2 = _vstatus.component_id;
+	vcmd.param3 = -1.0f; // Leave unchanged.
+	vcmd.param4 = -1.0f; // Leave unchanged.
+	publish_vehicle_cmd(&vcmd);
+}
+
+void
+Navigator::release_gimbal_control()
+{
+	vehicle_command_s vcmd = {};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
+	vcmd.param1 = -3.0f; // Remove control if it had it.
+	vcmd.param2 = -3.0f; // Remove control if it had it.
+	vcmd.param3 = -1.0f; // Leave unchanged.
+	vcmd.param4 = -1.0f; // Leave unchanged.
+	publish_vehicle_cmd(&vcmd);
+}
+
 int Navigator::print_usage(const char *reason)
 {
 	if (reason) {

src/modules/vmount/common.h

@@ -41,11 +41,9 @@
 
 #include <stdint.h>
 
-
 namespace vmount
 {
 
-
 /**
  * @struct ControlData
  * This defines the common API between an input and an output of the vmount driver.
@@ -58,7 +56,6 @@ struct ControlData {
 		Neutral = 0,      /**< move to neutral position */
 		Angle,            /**< control the roll, pitch & yaw angle directly */
 		LonLat            /**< control via lon, lat */
-		//TODO: add more, like smooth curve, ... ?
 	};
 
 
@@ -66,13 +63,15 @@ struct ControlData {
 
 	union TypeData {
 		struct TypeAngle {
-			float angles[3];         /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] or rad/s (if is angular rate) */
+			float q[4]; /**< attitude quaternion */
+			float angular_velocity[3]; // angular velocity
 
+			// according to DO_MOUNT_CONFIGURE
 			enum class Frame : uint8_t {
-				AngleBodyFrame = 0, /**< Angle in body frame. */
-				AngularRate,        /**< Angular rate in rad/s. */
-				AngleAbsoluteFrame  /**< Angle in absolute frame. */
-			} frames[3];             /**< Frame of given angle. */
+				AngleBodyFrame = 0, /**< Follow mode, angle relative to vehicle (usually default for yaw axis). */
+				AngularRate = 1, /**< Angular rate set only, for compatibility with MAVLink v1 protocol. */
+				AngleAbsoluteFrame = 2/**< Lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch). */
+			} frames[3]; /**< Mode. */
 		} angle;
 
 		struct TypeLonLat {
@@ -89,10 +88,8 @@ struct ControlData {
 
 	bool stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis
 	 	 	 	 	 	 	 	 	 	 (if the output supports it, this can also be done externally) */
-
 	bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */
 
 };
 
-
 } /* namespace vmount */

src/modules/vmount/input.h

@@ -52,7 +52,8 @@ namespace vmount
 class InputBase
 {
 public:
-	virtual ~InputBase() {}
+	InputBase() = default;
+	virtual ~InputBase() = default;
 
 	/**
 	 * Wait for an input update, with a timeout.

src/modules/vmount/input_mavlink.cpp

@@ -44,6 +44,8 @@
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
+#include <uORB/topics/gimbal_manager_set_manual_control.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
 #include <px4_platform_common/defines.h>
@@ -176,17 +178,6 @@ void InputMavlinkROI::print_status()
 
 InputMavlinkCmdMount::InputMavlinkCmdMount()
 {
-	param_t handle = param_find("MAV_SYS_ID");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_mav_sys_id);
-	}
-
-	handle = param_find("MAV_COMP_ID");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_mav_comp_id);
-	}
 }
 
 InputMavlinkCmdMount::~InputMavlinkCmdMount()
@@ -273,24 +264,30 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 						*control_data = &_control_data;
 						break;
 
-					case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
+					case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: {
 							_control_data.type = ControlData::Type::Angle;
 							_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 							_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 							_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
 							// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
-						_control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F;
+							const float roll = vehicle_command.param2 * M_DEG_TO_RAD_F;
 							// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
-						_control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F;
+							const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F;
 							// both specs have yaw on channel 2
-						_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
+							float yaw = vehicle_command.param3 * M_DEG_TO_RAD_F;
 
-						// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
-						if (_control_data.type_data.angle.angles[2] > M_PI_F) {
-							_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
-						}
+							matrix::Eulerf euler(roll, pitch, yaw);
+
+							matrix::Quatf q(euler);
+							q.copyTo(_control_data.type_data.angle.q);
+
+							_control_data.type_data.angle.angular_velocity[0] = NAN;
+							_control_data.type_data.angle.angular_velocity[1] = NAN;
+							_control_data.type_data.angle.angular_velocity[2] = NAN;
 
 							*control_data = &_control_data;
+						}
 						break;
 
 					case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
@@ -374,25 +371,14 @@ void InputMavlinkCmdMount::print_status()
 	PX4_INFO("Input: Mavlink (CMD_MOUNT)");
 }
 
-InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device)
+InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id,
+		float &mnt_rate_pitch, float &mnt_rate_yaw) :
+	_mav_sys_id(mav_sys_id),
+	_mav_comp_id(mav_comp_id),
+	_mnt_rate_pitch(mnt_rate_pitch),
+	_mnt_rate_yaw(mnt_rate_yaw)
 {
-	param_t handle = param_find("MAV_SYS_ID");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_mav_sys_id);
-	}
-
-	handle = param_find("MAV_COMP_ID");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_mav_comp_id);
-	}
-
-	if (has_v2_gimbal_device) {
-		/* smart gimbal: ask GIMBAL_DEVICE_INFORMATION to it */
-		_request_gimbal_device_information();
-
-	} else {
+	if (!has_v2_gimbal_device) {
 		/* dumb gimbal or MAVLink v1 protocol gimbal: fake GIMBAL_DEVICE_INFORMATION */
 		_stream_gimbal_manager_information();
 	}
@@ -415,6 +401,10 @@ InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
 	if (_vehicle_command_sub >= 0) {
 		orb_unsubscribe(_vehicle_command_sub);
 	}
+
+	if (_gimbal_manager_set_manual_control_sub >= 0) {
+		orb_unsubscribe(_gimbal_manager_set_manual_control_sub);
+	}
 }
 
 
@@ -447,6 +437,10 @@ int InputMavlinkGimbalV2::initialize()
 		return -errno;
 	}
 
+	if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) {
+		return -errno;
+	}
+
 	// rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode,
 	// it will publish vehicle_command's as well, causing the input poll() in here to return
 	// immediately, which in turn will cause an output update and thus a busy loop.
@@ -457,17 +451,21 @@ int InputMavlinkGimbalV2::initialize()
 
 void InputMavlinkGimbalV2::_stream_gimbal_manager_status()
 {
+	gimbal_device_attitude_status_s gimbal_device_attitude_status{};
 
 	if (_gimbal_device_attitude_status_sub.updated()) {
-		_gimbal_device_attitude_status_sub.copy(&_gimbal_device_attitude_status);
+		_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status);
 	}
 
 	gimbal_manager_status_s gimbal_manager_status{};
 	gimbal_manager_status.timestamp = hrt_absolute_time();
-	gimbal_manager_status.flags = _gimbal_device_attitude_status.device_flags;
+	gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags;
 	gimbal_manager_status.gimbal_device_id = 0;
+	gimbal_manager_status.primary_control_sysid = _sys_id_primary_control;
+	gimbal_manager_status.primary_control_compid = _comp_id_primary_control;
+	gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control
+	gimbal_manager_status.secondary_control_compid = 0; // TODO: support secondary control
 	_gimbal_manager_status_pub.publish(gimbal_manager_status);
-
 }
 
 void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
@@ -489,33 +487,14 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
 					      gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS |
 					      gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK;
 
-	gimbal_device_info.tilt_max = M_PI_F / 2;
-	gimbal_device_info.tilt_min = -M_PI_F / 2;
-	gimbal_device_info.tilt_rate_max = 1;
-	gimbal_device_info.pan_max = M_PI_F;
-	gimbal_device_info.pan_min = -M_PI_F;
-	gimbal_device_info.pan_rate_max = 1;
+	gimbal_device_info.pitch_max = M_PI_F / 2;
+	gimbal_device_info.pitch_min = -M_PI_F / 2;
+	gimbal_device_info.yaw_max = M_PI_F;
+	gimbal_device_info.yaw_min = -M_PI_F;
 
 	_gimbal_device_info_pub.publish(gimbal_device_info);
 }
 
-void InputMavlinkGimbalV2::_request_gimbal_device_information()
-{
-	vehicle_command_s vehicle_cmd{};
-	vehicle_cmd.timestamp = hrt_absolute_time();
-	vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE;
-	vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION;
-	vehicle_cmd.target_system = 0;
-	vehicle_cmd.target_component = 0;
-	vehicle_cmd.source_system = _mav_sys_id;
-	vehicle_cmd.source_component = _mav_comp_id;
-	vehicle_cmd.confirmation = 0;
-	vehicle_cmd.from_external = false;
-
-	uORB::PublicationQueued<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
-	vehicle_command_pub.publish(vehicle_cmd);
-}
-
 int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
 {
 	_stream_gimbal_manager_status();
@@ -523,7 +502,7 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
 	// Default to no change, set if we receive anything.
 	*control_data = nullptr;
 
-	const int num_poll = 4;
+	const int num_poll = 5;
 	px4_pollfd_struct_t polls[num_poll];
 	polls[0].fd = 		_gimbal_manager_set_attitude_sub;
 	polls[0].events = 	POLLIN;
@@ -533,6 +512,8 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
 	polls[2].events = 	POLLIN;
 	polls[3].fd = 		_vehicle_command_sub;
 	polls[3].events = 	POLLIN;
+	polls[4].fd = 		_gimbal_manager_set_manual_control_sub;
+	polls[4].events = 	POLLIN;
 
 	int poll_timeout = (int)timeout_ms;
 
@@ -560,13 +541,20 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
 				gimbal_manager_set_attitude_s set_attitude;
 				orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude);
 
-				const float pitch = matrix::Eulerf(matrix::Quatf(set_attitude.q)).phi(); // rad
-				const float roll = matrix::Eulerf(matrix::Quatf(set_attitude.q)).theta();
-				const float yaw = matrix::Eulerf(matrix::Quatf(set_attitude.q)).psi();
+				if (set_attitude.origin_sysid == _sys_id_primary_control &&
+				    set_attitude.origin_compid == _comp_id_primary_control) {
+					const matrix::Quatf q(set_attitude.q);
+					const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x,
+										set_attitude.angular_velocity_y,
+										set_attitude.angular_velocity_z);
 
-				_set_control_data_from_set_attitude(set_attitude.flags, pitch, set_attitude.angular_velocity_y, yaw,
-								    set_attitude.angular_velocity_z, roll, set_attitude.angular_velocity_x);
+					_set_control_data_from_set_attitude(set_attitude.flags, q, angular_velocity);
 					*control_data = &_control_data;
+
+				} else {
+					PX4_WARN("Ignoring gimbal_manager_set_attitude from %d/%d",
+						 set_attitude.origin_sysid, set_attitude.origin_compid);
+				}
 			}
 
 			if (polls[1].revents & POLLIN) {
@@ -642,16 +630,233 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
 					continue;
 				}
 
-				if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_ATTITUDE) {
-					_set_control_data_from_set_attitude((uint32_t)vehicle_command.param5, vehicle_command.param3, vehicle_command.param1,
-									    vehicle_command.param3, vehicle_command.param2);
+				if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
+					// FIXME: Remove me later. For now, we support this for legacy missions
+					//        using gimbal v1 protocol.
+
+					switch ((int)vehicle_command.param7) {
+					case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
+						_control_data.gimbal_shutter_retract = true;
+
+					/* FALLTHROUGH */
+
+					case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
+						_control_data.type = ControlData::Type::Neutral;
+
 						*control_data = &_control_data;
-					_ack_vehicle_command(&vehicle_command);
+						break;
+
+					case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: {
+							_control_data.type = ControlData::Type::Angle;
+							_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+							_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+							_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+							// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
+							const float roll = vehicle_command.param2 * M_DEG_TO_RAD_F;
+							// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
+							const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F;
+							// both specs have yaw on channel 2
+							float yaw = vehicle_command.param3 * M_DEG_TO_RAD_F;
+
+							// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
+							if (yaw > M_PI_F) {
+								yaw -= 2 * M_PI_F;
+							}
+
+							matrix::Eulerf euler(roll, pitch, yaw);
+
+							matrix::Quatf q(euler);
+							q.copyTo(_control_data.type_data.angle.q);
+
+							_control_data.type_data.angle.angular_velocity[0] = NAN;
+							_control_data.type_data.angle.angular_velocity[1] = NAN;
+							_control_data.type_data.angle.angular_velocity[2] = NAN;
+
+							*control_data = &_control_data;
+						}
+						break;
+
+					case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
+						break;
+
+					case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
+						control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4);
+
+						*control_data = &_control_data;
+						break;
+					}
+
+					_ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
+
+					_control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1;
+					_control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1;
+					_control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1;
+
+
+					const int params[] = {
+						(int)((float)vehicle_command.param5 + 0.5f),
+						(int)((float)vehicle_command.param6 + 0.5f),
+						(int)(vehicle_command.param7 + 0.5f)
+					};
+
+					for (int i = 0; i < 3; ++i) {
+
+						if (params[i] == 0) {
+							_control_data.type_data.angle.frames[i] =
+								ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+						} else if (params[i] == 1) {
+							_control_data.type_data.angle.frames[i] =
+								ControlData::TypeData::TypeAngle::Frame::AngularRate;
+
+						} else if (params[i] == 2) {
+							_control_data.type_data.angle.frames[i] =
+								ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+
+						} else {
+							// Not supported, fallback to body angle.
+							_control_data.type_data.angle.frames[i] =
+								ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+						}
+					}
+
+					_control_data.type = ControlData::Type::Neutral; //always switch to neutral position
+
+					*control_data = &_control_data;
+					_ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) {
+
+					const int param_sys_id = roundf(vehicle_command.param1);
+					const int param_comp_id = roundf(vehicle_command.param2);
+
+					uint8_t new_sys_id_primary_control = [&]() {
+						if (param_sys_id >= 0 && param_sys_id < 256) {
+							// Valid new sysid.
+							return (uint8_t)param_sys_id;
+
+						} else if (param_sys_id == -1) {
+							// leave unchanged
+							return _sys_id_primary_control;
+
+						} else if (param_sys_id == -2) {
+							// set itself
+							return _mav_sys_id;
+
+						} else if (param_sys_id == -3) {
+							// release control if in control
+							if (_sys_id_primary_control == vehicle_command.source_system) {
+								return (uint8_t)0;
+
+							} else {
+								return _sys_id_primary_control;
+							}
+
+						} else {
+							PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE");
+							return _sys_id_primary_control;
+						}
+					}();
+
+					uint8_t new_comp_id_primary_control = [&]() {
+						if (param_comp_id >= 0 && param_comp_id < 256) {
+							// Valid new compid.
+							return (uint8_t)param_comp_id;
+
+						} else if (param_comp_id == -1) {
+							// leave unchanged
+							return _comp_id_primary_control;
+
+						} else if (param_comp_id == -2) {
+							// set itself
+							return _mav_comp_id;
+
+						} else if (param_comp_id == -3) {
+							// release control if in control
+							if (_comp_id_primary_control == vehicle_command.source_component) {
+								return (uint8_t)0;
+
+							} else {
+								return _comp_id_primary_control;
+							}
+
+						} else {
+							PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE");
+							return _comp_id_primary_control;
+						}
+					}();
+
+
+					if (new_sys_id_primary_control != _sys_id_primary_control ||
+					    new_comp_id_primary_control != _comp_id_primary_control) {
+						PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d",
+							 _sys_id_primary_control, _comp_id_primary_control,
+							 new_sys_id_primary_control, new_comp_id_primary_control);
+						_sys_id_primary_control = new_sys_id_primary_control;
+						_comp_id_primary_control = new_comp_id_primary_control;
+					}
+
+					// TODO: support secondary control
+					// TODO: support gimbal device id for multiple gimbals
+
+					_ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) {
+
+					if (vehicle_command.source_system == _sys_id_primary_control &&
+					    vehicle_command.source_component == _comp_id_primary_control) {
+
+						const matrix::Eulerf euler(0.0f, vehicle_command.param1 * M_DEG_TO_RAD_F, vehicle_command.param2 * M_DEG_TO_RAD_F);
+						const matrix::Quatf q(euler);
+						const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, vehicle_command.param4);
+						const uint32_t flags = vehicle_command.param5;
+
+						// TODO: support gimbal device id for multiple gimbals
+
+						_set_control_data_from_set_attitude(flags, q, angular_velocity);
+						*control_data = &_control_data;
+						_ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+					} else {
+						PX4_ERR("GIMBAL_MANAGER_PITCHYAW denied, not in control");
+						_ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+					}
 
 				} else {
 					exit_loop = false;
 				}
 			}
+
+			if (polls[4].revents & POLLIN) {
+				gimbal_manager_set_manual_control_s set_manual_control;
+				orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub, &set_manual_control);
+
+				if (set_manual_control.origin_sysid == _sys_id_primary_control &&
+				    set_manual_control.origin_compid == _comp_id_primary_control) {
+
+					const matrix::Quatf q =
+						(PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) ?
+						matrix::Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) :
+						matrix::Quatf(NAN, NAN, NAN, NAN);
+
+					const matrix::Vector3f angular_velocity =
+						(PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) ?
+						matrix::Vector3f(0.0f,
+								 _mnt_rate_pitch * M_DEG_TO_RAD_F * set_manual_control.pitch_rate,
+								 _mnt_rate_yaw * M_DEG_TO_RAD_F * set_manual_control.yaw_rate) :
+						matrix::Vector3f(NAN, NAN, NAN);
+
+					_set_control_data_from_set_attitude(set_manual_control.flags, q, angular_velocity);
+					*control_data = &_control_data;
+
+				} else {
+					PX4_WARN("Ignoring gimbal_manager_set_manual_control from %d/%d",
+						 set_manual_control.origin_sysid, set_manual_control.origin_compid);
+				}
+			}
 		}
 	}
 
@@ -671,25 +876,30 @@ void InputMavlinkGimbalV2::_transform_lon_lat_to_angle(const double roi_lon, con
 	_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 	_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 
-	_control_data.type_data.angle.angles[0] = 0.f;
+	const float roll = 0.0f;
+	float pitch = 0.0f;
+	float yaw = 0.0f;
 
 	// interface: use fixed pitch value > -pi otherwise consider ROI altitude
 	if (_control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
-		_control_data.type_data.angle.angles[1] = _control_data.type_data.lonlat.pitch_fixed_angle;
+		pitch = _control_data.type_data.lonlat.pitch_fixed_angle;
 
 	} else {
-		_control_data.type_data.angle.angles[1] = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position);
+		pitch = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position);
 	}
 
-	_control_data.type_data.angle.angles[2] = get_bearing_to_next_waypoint(vlat, vlon, roi_lat,
-			roi_lon) - vehicle_global_position.yaw;
+	yaw = get_bearing_to_next_waypoint(vlat, vlon, roi_lat, roi_lon) - vehicle_local_position.yaw;
 
 	// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
-	_control_data.type_data.angle.angles[1] += _control_data.type_data.lonlat.pitch_angle_offset;
-	_control_data.type_data.angle.angles[2] += _control_data.type_data.lonlat.yaw_angle_offset;
+	pitch += _control_data.type_data.lonlat.pitch_angle_offset;
+	yaw += _control_data.type_data.lonlat.yaw_angle_offset;
 
 	// make sure yaw is wrapped correctly for the output
-	_control_data.type_data.angle.angles[2] = wrap_pi(_control_data.type_data.angle.angles[2]);
+	yaw = wrap_pi(yaw);
+
+	matrix::Eulerf euler(roll, pitch, yaw);
+	matrix::Quatf q(euler);
+	q.copyTo(_control_data.type_data.angle.q);
 }
 
 float InputMavlinkGimbalV2::_calculate_pitch(double lon, double lat, float altitude,
@@ -718,81 +928,48 @@ void InputMavlinkGimbalV2::_read_lat_lon_alt_from_position_setpoint_sub(double &
 	alt_sp = (double)position_setpoint_triplet.current.alt;
 }
 
-void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const float pitch_angle,
-		const float pitch_rate, const float yaw_angle, const float yaw_rate, float roll_angle, float roll_rate)
+void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q,
+		const matrix::Vector3f &angular_velocity)
 {
-
 	if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) {
 		// not implemented in ControlData
 	} else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) {
 		_control_data.type = ControlData::Type::Neutral;
 
-	} else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NONE) != 0) {
-		// don't do anything
 	} else {
 		_control_data.type = ControlData::Type::Angle;
-		_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-		_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-		_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-
-		if (_is_roi_set && (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NUDGE) != 0) {
-			// add set_attitude.q to existing tracking angle or ROI
-			// track message not yet implemented
-			_control_data.type_data.angle.angles[0] += pitch_angle;
-			_control_data.type_data.angle.angles[1] += roll_angle;
-			_control_data.type_data.angle.angles[2] += yaw_angle;
-
-		} else {
-			_control_data.type_data.angle.angles[0] = pitch_angle;
-			_control_data.type_data.angle.angles[1] = roll_angle;
-			_control_data.type_data.angle.angles[2] = yaw_angle;
-		}
 
-		if (_is_roi_set && (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_OVERRIDE) != 0) {
-			// overides tracking or ROI angle with set_attitude.q, respects flag GIMBAL_MANAGER_FLAGS_YAW_LOCK
-			_control_data.type_data.angle.angles[0] = pitch_angle;
-			_control_data.type_data.angle.angles[1] = roll_angle;
-			_control_data.type_data.angle.angles[2] = yaw_angle;
-		}
+		_control_data.type_data.angle.q[0] = q(0);
+		_control_data.type_data.angle.q[1] = q(1);
+		_control_data.type_data.angle.q[2] = q(2);
+		_control_data.type_data.angle.q[3] = q(3);
 
-		if (PX4_ISFINITE(roll_rate)) { //roll
-			_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
-			_control_data.type_data.angle.angles[0] = roll_rate; //rad/s
-		}
+		_control_data.type_data.angle.angular_velocity[0] = angular_velocity(0);
+		_control_data.type_data.angle.angular_velocity[1] = angular_velocity(1);
+		_control_data.type_data.angle.angular_velocity[2] = angular_velocity(2);
 
-		if (PX4_ISFINITE(pitch_rate)) { //pitch
-			_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
-			_control_data.type_data.angle.angles[1] = pitch_rate; //rad/s
-		}
+		_control_data.type_data.angle.frames[0] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK)
+				? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame
+				: ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 
-		if (PX4_ISFINITE(yaw_rate)) { //yaw
-			_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
-			_control_data.type_data.angle.angles[2] = yaw_rate; //rad/s
-		}
+		_control_data.type_data.angle.frames[1] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK)
+				? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame
+				: ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 
-		if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) {
-			// stay horizontal with the horizon
-			_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-		}
-
-		if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) {
-			_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-		}
-
-		if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) {
-			_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-		}
+		_control_data.type_data.angle.frames[2] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK)
+				? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame
+				: ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 	}
 }
 
 //TODO move this one to input.cpp such that it can be shared across functions
-void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd)
+void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t result)
 {
 	vehicle_command_ack_s vehicle_command_ack{};
 
 	vehicle_command_ack.timestamp = hrt_absolute_time();
 	vehicle_command_ack.command = cmd->command;
-	vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+	vehicle_command_ack.result = result;
 	vehicle_command_ack.target_system = cmd->source_system;
 	vehicle_command_ack.target_component = cmd->source_component;
 

src/modules/vmount/input_mavlink.h

@@ -45,7 +45,6 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/gimbal_manager_set_attitude.h>
 #include <uORB/topics/gimbal_device_attitude_status.h>
 #include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/gimbal_manager_status.h>
@@ -110,7 +109,8 @@ private:
 class InputMavlinkGimbalV2 : public InputBase
 {
 public:
-	InputMavlinkGimbalV2(bool has_v2_gimbal_device);
+	InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch,
+			     float &mnt_rate_yaw);
 	virtual ~InputMavlinkGimbalV2();
 
 	virtual void print_status();
@@ -125,20 +125,26 @@ private:
 	float _calculate_pitch(double lon, double lat, float altitude,
 			       const vehicle_global_position_s &global_position);
 	void _read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp);
-	void _set_control_data_from_set_attitude(const uint32_t flags, const float pitch_angle, const float pitch_rate,
-			const float yaw_angle, const float yaw_rate, float roll_angle = 0, float roll_rate = 0);
-	void _ack_vehicle_command(vehicle_command_s *cmd);
+	void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q,
+			const matrix::Vector3f &angular_velocity);
+	void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result);
 	void _stream_gimbal_manager_information();
-	void _request_gimbal_device_information();
 	void _stream_gimbal_manager_status();
 
 	int _vehicle_roi_sub = -1;
 	int _gimbal_manager_set_attitude_sub = -1;
+	int _gimbal_manager_set_manual_control_sub = -1;
 	int _position_setpoint_triplet_sub = -1;
 	int _vehicle_command_sub = -1;
 
-	int32_t _mav_sys_id{1}; ///< our mavlink system id
-	int32_t _mav_comp_id{1}; ///< our mavlink component id
+	uint8_t _mav_sys_id{1}; ///< our mavlink system id
+	uint8_t _mav_comp_id{1}; ///< our mavlink component id
+
+	uint8_t _sys_id_primary_control{0};
+	uint8_t _comp_id_primary_control{0};
+
+	float &_mnt_rate_pitch;
+	float &_mnt_rate_yaw;
 
 	bool _is_roi_set{false};
 
@@ -148,9 +154,6 @@ private:
 	uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
 	map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
 	uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
-
-	gimbal_device_attitude_status_s _gimbal_device_attitude_status{};
-
 };
 
 } /* namespace vmount */

src/modules/vmount/input_rc.cpp

@@ -41,6 +41,7 @@
 
 #include <math.h>
 #include <errno.h>
+#include <matrix/matrix/math.hpp>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/defines.h>
 
@@ -130,10 +131,14 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
 
 		_first_time = false;
 
+		matrix::Eulerf euler(new_aux_values[0] * M_PI_F, new_aux_values[1] * M_PI_F,
+				     new_aux_values[2] * M_PI_F);
+		matrix::Quatf q(euler);
+		q.copyTo(control_data.type_data.angle.q);
+
 		for (int i = 0; i < 3; ++i) {
+			// We always use follow mode with RC input for now.
 			control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
-			control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F;
-
 			_last_set_aux_values[i] = new_aux_values[i];
 		}
 

src/modules/vmount/input_test.cpp

@@ -42,6 +42,7 @@
 #include <math.h>
 
 #include <px4_platform_common/posix.h>
+#include <lib/matrix/matrix/math.hpp>
 
 
 namespace vmount
@@ -69,9 +70,16 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool
 	_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
 	_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 
-	for (int i = 0; i < 3; ++i) {
-		_control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F;
-	}
+	matrix::Eulerf euler(
+		_angles[0] * M_DEG_TO_RAD_F,
+		_angles[1] * M_DEG_TO_RAD_F,
+		_angles[2] * M_DEG_TO_RAD_F);
+	matrix::Quatf q(euler);
+
+	_control_data.type_data.angle.q[0] = q(0);
+	_control_data.type_data.angle.q[1] = q(1);
+	_control_data.type_data.angle.q[2] = q(2);
+	_control_data.type_data.angle.q[3] = q(3);
 
 	_control_data.gimbal_shutter_retract = false;
 	*control_data = &_control_data;

src/modules/vmount/output.cpp

@@ -108,22 +108,28 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
 
 	switch (control_data->type) {
 	case ControlData::Type::Angle:
+
+		{
+			matrix::Quatf q(control_data->type_data.angle.q);
+			matrix::Eulerf euler(q);
+
 			for (int i = 0; i < 3; ++i) {
 				switch (control_data->type_data.angle.frames[i]) {
 				case ControlData::TypeData::TypeAngle::Frame::AngularRate:
-				_angle_speeds[i] = control_data->type_data.angle.angles[i];
 					break;
 
 				case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame:
 					_absolute_angle[i] = false;
-				_angle_setpoints[i] = control_data->type_data.angle.angles[i];
 					break;
 
 				case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame:
 					_absolute_angle[i] = true;
-				_angle_setpoints[i] = control_data->type_data.angle.angles[i];
 					break;
 				}
+
+				_angle_speeds[i] = control_data->type_data.angle.angular_velocity[i];
+				_angle_setpoints[i] = euler(i);
+			}
 		}
 
 		break;

src/modules/vmount/output.h

@@ -67,8 +67,8 @@ struct OutputConfig {
 	float roll_offset;	/**< Offset for roll channel in radians */
 	float yaw_offset;	/**< Offset for yaw channel in radians */
 
-	uint32_t mavlink_sys_id;	/**< Mavlink target system id for mavlink output */
-	uint32_t mavlink_comp_id;
+	uint32_t mavlink_sys_id_v1;	/**< Mavlink target system id for mavlink output only for v1 */
+	uint32_t mavlink_comp_id_v1;
 };
 
 

src/modules/vmount/output_mavlink.cpp

@@ -59,8 +59,8 @@ int OutputMavlinkV1::update(const ControlData *control_data)
 {
 	vehicle_command_s vehicle_command{};
 	vehicle_command.timestamp = hrt_absolute_time();
-	vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
-	vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
+	vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id_v1;
+	vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1;
 
 	if (control_data) {
 		//got new command
@@ -118,13 +118,26 @@ void OutputMavlinkV1::print_status()
 	PX4_INFO("Output: MAVLink gimbal protocol v1");
 }
 
-OutputMavlinkV2::OutputMavlinkV2(const OutputConfig &output_config)
-	: OutputBase(output_config)
+OutputMavlinkV2::OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config)
+	: OutputBase(output_config),
+	  _mav_sys_id(mav_sys_id),
+	  _mav_comp_id(mav_comp_id)
 {
 }
 
 int OutputMavlinkV2::update(const ControlData *control_data)
 {
+	_check_for_gimbal_device_information();
+
+	hrt_abstime t = hrt_absolute_time();
+
+	if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) {
+		_request_gimbal_device_information();
+		_last_gimbal_device_checked = t;
+
+	} else {
+		// Only start sending attitude setpoints once a device has been discovered.
+
 		if (control_data) {
 			//got new command
 			_set_angle_setpoints(control_data);
@@ -133,14 +146,40 @@ int OutputMavlinkV2::update(const ControlData *control_data)
 
 		_handle_position_update();
 
-	hrt_abstime t = hrt_absolute_time();
 		_calculate_output_angles(t);
-
 		_last_update = t;
+	}
 
 	return 0;
 }
 
+void OutputMavlinkV2::_request_gimbal_device_information()
+{
+	vehicle_command_s vehicle_cmd{};
+	vehicle_cmd.timestamp = hrt_absolute_time();
+	vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE;
+	vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION;
+	vehicle_cmd.target_system = 0;
+	vehicle_cmd.target_component = 0;
+	vehicle_cmd.source_system = _mav_sys_id;
+	vehicle_cmd.source_component = _mav_comp_id;
+	vehicle_cmd.confirmation = 0;
+	vehicle_cmd.from_external = false;
+
+	uORB::PublicationQueued<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
+	vehicle_command_pub.publish(vehicle_cmd);
+}
+
+void OutputMavlinkV2::_check_for_gimbal_device_information()
+{
+	gimbal_device_information_s gimbal_device_information;
+
+	if (_gimbal_device_information_sub.update(&gimbal_device_information)) {
+		_gimbal_device_found = true;
+		_gimbal_device_compid = gimbal_device_information.gimbal_device_compid;
+	}
+}
+
 void OutputMavlinkV2::print_status()
 {
 	PX4_INFO("Output: MAVLink gimbal protocol v2");
@@ -150,35 +189,29 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *con
 {
 	gimbal_device_set_attitude_s set_attitude{};
 	set_attitude.timestamp = hrt_absolute_time();
-	set_attitude.target_system = (uint8_t)_config.mavlink_sys_id;
-	set_attitude.target_component = (uint8_t)_config.mavlink_comp_id;
-
-	matrix::Eulerf euler(control_data->type_data.angle.angles[0], control_data->type_data.angle.angles[1],
-			     control_data->type_data.angle.angles[2]);
-	matrix::Quatf q(euler);
-
-	set_attitude.q[0] = q(0);
-	set_attitude.q[1] = q(1);
-	set_attitude.q[2] = q(2);
-	set_attitude.q[3] = q(3);
-
-
-	if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
-		set_attitude.angular_velocity_x = control_data->type_data.angle.angles[0]; //roll
-	}
-
-	if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
-		set_attitude.angular_velocity_y = control_data->type_data.angle.angles[1]; //pitch
-
-	}
+	set_attitude.target_system = (uint8_t)_mav_sys_id;
+	set_attitude.target_component = _gimbal_device_compid;
 
-	if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
-		set_attitude.angular_velocity_z = control_data->type_data.angle.angles[2];
-	}
-
-	if (control_data->type == ControlData::Type::Neutral) {
+	switch (control_data->type) {
+	case ControlData::Type::Neutral:
 		set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_NEUTRAL;
-	}
+		set_attitude.angular_velocity_x = NAN;
+		set_attitude.angular_velocity_y = NAN;
+		set_attitude.angular_velocity_z = NAN;
+		set_attitude.q[0] = NAN;
+		set_attitude.q[1] = NAN;
+		set_attitude.q[2] = NAN;
+		set_attitude.q[3] = NAN;
+		break;
+
+	case ControlData::Type::Angle:
+		set_attitude.angular_velocity_x = control_data->type_data.angle.angular_velocity[0];
+		set_attitude.angular_velocity_y = control_data->type_data.angle.angular_velocity[1];
+		set_attitude.angular_velocity_z = control_data->type_data.angle.angular_velocity[2];
+		set_attitude.q[0] = control_data->type_data.angle.q[0];
+		set_attitude.q[1] = control_data->type_data.angle.q[1];
+		set_attitude.q[2] = control_data->type_data.angle.q[2];
+		set_attitude.q[3] = control_data->type_data.angle.q[3];
 
 		if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
 			set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK;
@@ -192,8 +225,14 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *con
 			set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK;
 		}
 
-	_gimbal_device_set_attitude_pub.publish(set_attitude);
+		break;
+
+	case ControlData::Type::LonLat:
+		// FIXME: needs conversion to angle.
+		break;
+	}
 
+	_gimbal_device_set_attitude_pub.publish(set_attitude);
 }
 
 } /* namespace vmount */

src/modules/vmount/output_mavlink.h

@@ -44,6 +44,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/gimbal_device_set_attitude.h>
+#include <uORB/topics/gimbal_device_information.h>
 
 
 namespace vmount
@@ -69,7 +70,7 @@ private:
 class OutputMavlinkV2 : public OutputBase
 {
 public:
-	OutputMavlinkV2(const OutputConfig &output_config);
+	OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config);
 	virtual ~OutputMavlinkV2() = default;
 
 	virtual int update(const ControlData *control_data);
@@ -78,7 +79,17 @@ public:
 
 private:
 	void _publish_gimbal_device_set_attitude(const ControlData *control_data);
+	void _request_gimbal_device_information();
+	void _check_for_gimbal_device_information();
+
 	uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
+	uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
+
+	int32_t _mav_sys_id{1}; ///< our mavlink system id
+	int32_t _mav_comp_id{1}; ///< our mavlink component id
+	uint8_t _gimbal_device_compid{0};
+	hrt_abstime _last_gimbal_device_checked{0};
+	bool _gimbal_device_found {false};
 };
 
 } /* namespace vmount */

src/modules/vmount/output_rc.cpp

@@ -100,10 +100,7 @@ void OutputRC::_stream_device_attitude_status()
 
 	matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]);
 	matrix::Quatf q(euler);
-	attitude_status.q[0] = q(0);
-	attitude_status.q[1] = q(1);
-	attitude_status.q[2] = q(2);
-	attitude_status.q[3] = q(3);
+	q.copyTo(attitude_status.q);
 
 	attitude_status.failure_flags = 0;
 	_attitude_status_pub.publish(attitude_status);

src/modules/vmount/vmount.cpp

@@ -88,8 +88,8 @@ static volatile ThreadData *g_thread_data = nullptr;
 struct Parameters {
 	int32_t mnt_mode_in;
 	int32_t mnt_mode_out;
-	int32_t mnt_mav_sysid;
-	int32_t mnt_mav_compid;
+	int32_t mnt_mav_sys_id_v1;
+	int32_t mnt_mav_comp_id_v1;
 	float mnt_ob_lock_mode;
 	float mnt_ob_norm_mode;
 	int32_t mnt_man_pitch;
@@ -102,6 +102,10 @@ struct Parameters {
 	float mnt_off_pitch;
 	float mnt_off_roll;
 	float mnt_off_yaw;
+	int32_t mav_sys_id;
+	int32_t mav_comp_id;
+	float mnt_rate_pitch;
+	float mnt_rate_yaw;
 
 	bool operator!=(const Parameters &p)
 	{
@@ -109,8 +113,8 @@ struct Parameters {
 #pragma GCC diagnostic ignored "-Wfloat-equal"
 		return mnt_mode_in != p.mnt_mode_in ||
 		       mnt_mode_out != p.mnt_mode_out ||
-		       mnt_mav_sysid != p.mnt_mav_sysid ||
-		       mnt_mav_compid != p.mnt_mav_compid ||
+		       mnt_mav_sys_id_v1 != p.mnt_mav_sys_id_v1 ||
+		       mnt_mav_comp_id_v1 != p.mnt_mav_comp_id_v1 ||
 		       fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f ||
 		       fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f ||
 		       mnt_man_pitch != p.mnt_man_pitch ||
@@ -122,7 +126,9 @@ struct Parameters {
 		       mnt_range_yaw != p.mnt_range_yaw ||
 		       mnt_off_pitch != p.mnt_off_pitch ||
 		       mnt_off_roll != p.mnt_off_roll ||
-		       mnt_off_yaw != p.mnt_off_yaw;
+		       mnt_off_yaw != p.mnt_off_yaw ||
+		       mav_sys_id != p.mav_sys_id ||
+		       mav_comp_id != p.mav_comp_id;
 #pragma GCC diagnostic pop
 
 	}
@@ -131,8 +137,8 @@ struct Parameters {
 struct ParameterHandles {
 	param_t mnt_mode_in;
 	param_t mnt_mode_out;
-	param_t mnt_mav_sysid;
-	param_t mnt_mav_compid;
+	param_t mnt_mav_sys_id_v1;
+	param_t mnt_mav_comp_id_v1;
 	param_t mnt_ob_lock_mode;
 	param_t mnt_ob_norm_mode;
 	param_t mnt_man_pitch;
@@ -145,6 +151,10 @@ struct ParameterHandles {
 	param_t mnt_off_pitch;
 	param_t mnt_off_roll;
 	param_t mnt_off_yaw;
+	param_t mav_sys_id;
+	param_t mav_comp_id;
+	param_t mnt_rate_pitch;
+	param_t mnt_rate_yaw;
 };
 
 
@@ -229,8 +239,8 @@ static int vmount_thread_main(int argc, char *argv[])
 			output_config.pitch_offset = params.mnt_off_pitch * M_DEG_TO_RAD_F;
 			output_config.roll_offset = params.mnt_off_roll * M_DEG_TO_RAD_F;
 			output_config.yaw_offset = params.mnt_off_yaw * M_DEG_TO_RAD_F;
-			output_config.mavlink_sys_id = params.mnt_mav_sysid;
-			output_config.mavlink_comp_id = params.mnt_mav_compid;
+			output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1;
+			output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1;
 
 			bool alloc_failed = false;
 			thread_data.input_objs_len = 1;
@@ -271,7 +281,11 @@ static int vmount_thread_main(int argc, char *argv[])
 					break;
 
 				case 4: //MAVLINK_V2
-					thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2);
+					thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2,
+							params.mav_sys_id,
+							params.mav_comp_id,
+							params.mnt_rate_pitch,
+							params.mnt_rate_yaw);
 					break;
 
 				default:
@@ -302,7 +316,7 @@ static int vmount_thread_main(int argc, char *argv[])
 				break;
 
 			case 2: //MAVLink v2 gimbal protocol
-				thread_data.output_obj = new OutputMavlinkV2(output_config);
+				thread_data.output_obj = new OutputMavlinkV2(params.mav_sys_id, params.mav_comp_id, output_config);
 
 				if (!thread_data.output_obj) { alloc_failed = true; }
 
@@ -375,9 +389,9 @@ static int vmount_thread_main(int argc, char *argv[])
 				break;
 			}
 
-			//only publish the mount orientation if the mode is not mavlink
-			//if the gimbal speaks mavlink it publishes its own orientation
-			if (params.mnt_mode_out != 1) { // 1 = MAVLINK
+			// Only publish the mount orientation if the mode is not mavlink v1 or v2
+			// If the gimbal speaks mavlink it publishes its own orientation.
+			if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2
 				thread_data.output_obj->publish();
 			}
 
@@ -550,8 +564,8 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
 	Parameters prev_params = params;
 	param_get(param_handles.mnt_mode_in, &params.mnt_mode_in);
 	param_get(param_handles.mnt_mode_out, &params.mnt_mode_out);
-	param_get(param_handles.mnt_mav_sysid, &params.mnt_mav_sysid);
-	param_get(param_handles.mnt_mav_compid, &params.mnt_mav_compid);
+	param_get(param_handles.mnt_mav_sys_id_v1, &params.mnt_mav_sys_id_v1);
+	param_get(param_handles.mnt_mav_comp_id_v1, &params.mnt_mav_comp_id_v1);
 	param_get(param_handles.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
 	param_get(param_handles.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
 	param_get(param_handles.mnt_man_pitch, &params.mnt_man_pitch);
@@ -564,6 +578,10 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
 	param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
 	param_get(param_handles.mnt_off_roll, &params.mnt_off_roll);
 	param_get(param_handles.mnt_off_yaw, &params.mnt_off_yaw);
+	param_get(param_handles.mav_sys_id, &params.mav_sys_id);
+	param_get(param_handles.mav_comp_id, &params.mav_comp_id);
+	param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
+	param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
 
 	got_changes = prev_params != params;
 }
@@ -572,8 +590,8 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 {
 	param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
 	param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
-	param_handles.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
-	param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID");
+	param_handles.mnt_mav_sys_id_v1 = param_find("MNT_MAV_SYSID");
+	param_handles.mnt_mav_comp_id_v1 = param_find("MNT_MAV_COMPID");
 	param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
 	param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
 	param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
@@ -586,11 +604,15 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH");
 	param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL");
 	param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW");
+	param_handles.mav_sys_id = param_find("MAV_SYS_ID");
+	param_handles.mav_comp_id = param_find("MAV_COMP_ID");
+	param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH");
+	param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW");
 
 	if (param_handles.mnt_mode_in == PARAM_INVALID ||
 	    param_handles.mnt_mode_out == PARAM_INVALID ||
-	    param_handles.mnt_mav_sysid == PARAM_INVALID ||
-	    param_handles.mnt_mav_compid == PARAM_INVALID ||
+	    param_handles.mnt_mav_sys_id_v1 == PARAM_INVALID ||
+	    param_handles.mnt_mav_comp_id_v1 == PARAM_INVALID ||
 	    param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
 	    param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
 	    param_handles.mnt_man_pitch == PARAM_INVALID ||
@@ -602,7 +624,12 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	    param_handles.mnt_range_yaw == PARAM_INVALID ||
 	    param_handles.mnt_off_pitch == PARAM_INVALID ||
 	    param_handles.mnt_off_roll == PARAM_INVALID ||
-	    param_handles.mnt_off_yaw == PARAM_INVALID) {
+	    param_handles.mnt_off_yaw == PARAM_INVALID ||
+	    param_handles.mav_sys_id == PARAM_INVALID ||
+	    param_handles.mav_comp_id == PARAM_INVALID ||
+	    param_handles.mnt_rate_pitch == PARAM_INVALID ||
+	    param_handles.mnt_rate_yaw == PARAM_INVALID
+	   ) {
 		return false;
 	}
 

src/modules/vmount/vmount_params.c

@@ -235,3 +235,23 @@ PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f);
 * @group Mount
 */
 PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f);
+
+/**
+ * Angular pitch rate for manual input in degrees/second.
+ * Full stick input [-1..1] translats to [-pitch rate..pitch rate].
+ *
+ * @min 1.0
+ * @max 90.0
+ * @group Mount
+ */
+PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f);
+
+/**
+ * Angular yaw rate for manual input in degrees/second.
+ * Full stick input [-1..1] translats to [-yaw rate..yaw rate].
+ *
+ * @min 1.0
+ * @max 90.0
+ * @group Mount
+ */
+PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);