Support for gimbal v2 protocol

- add command to request a message
- add gimbal attitude message

mavlink_receiver handle GIMBAL_MANAGER_SET_ATTITUDE

first implementation of new vmount input MavlinkGimbalV2
- setup class
- decode gimbal_manager_set_attitude in ControlData

add gimbal information message

add gimbal manager information and vehicle command ack

mavlink messages: add stream for GIMBAL_MANAGER_INFORMATION

mavlink_receiver: handle GIMBAL_DEVICE_INFORMATION

remove mavlink cmd handling from vmount input MavlinkGimbalV2

complete gimbal manager:
- send out fake gimbal_device_information for dummy gimbals
- complete ROI handling with nudging

small fixes

fix typos

cleanup
- gimbal device information
- flags lock
- check sanity of string

add support for CMD_DO_GIMBAL_MANAGER_ATTITUDE

stream GimbalDeviceAttitudeStatus for dummy gimbals
- add uROB gimbal_attitude_status
- fill status in vmount output_rc for dummy gimbals not able to send the
status themselves
- stream mavlink GimbalDeviceAttitudeStatus

better handle the request for gimbal infomation request

clean up

bring gimbal information back on vmount init

add new gimbal message to mavlink normal stream

fix publication of gimbal device information

rename gimbal_attitude_status to gimbal_device_attitude_status

stream gimbal_manager_status at 5Hz

mavlink: send information only on request

Sending the information message once on request should now work and we
don't need to keep publishing it.

mavlink: debug output for now

make sure to copy over control data

mavlink: add missing copyright header, pragma once

mavlink: address review comments

mavlink: handle stream not updated

Our answer does not just depend on whether the stream was found but
whether we actually were able to send out an update.

mavlink: remove outdated comment

vmount: add option for yaw stabilization only

The stabilize flag is used for gimbals which do not have an internal IMU
and need the autopilot's attitude in order to do stabilization. These
gimbals are probably quite rare by now but it makes sense to keep the
functionality given it can e.g. be used by simple servo gimbals for
sensors other than highres cameras.

The stabilize flag can also be re-used for gimbals which are capable of
stabilizing pitch and roll but not absolute yaw (e.g. locked to North).
For such gimbals we can now set the param MNT_DO_STAB to 2.

We still support configuring which axes are stabilized by the
MAVLink command DO_MOUNT_CONFIGURE, however, this is generally not
recommended anymore.

vmount: fix incorrect check for bit flag

mavlink_messages: remove debug message

Signed-off-by: Claudio Micheli <claudio@auterion.com>

use device id

remove debug print

gimbal attitude fix mistake

clang tidy fix

split:
- gimbal_attitude -> gimbal_device_set_attitude, gimbal_manager_set_attitude
- gimbal_information -> gimbal_device_informatio, gimbal_manager_information

add gimbal protocol messages to rtps msg ids

support set attitude for gimbal directly speaking mavlink

clean up gimbal urob messages

vmount: address a few small review comments

vmount: split output into v1 and v2 protocol

This way we can continue to support the MAVLink v1 protocol. Also, we
don't send the old vehicle commands when actually using the new v2
protocol.

vmount: config via ctor instead of duplicate param

vmount: use loop to poll all topics

Otherwise we might give up too soon and miss some data, or run too fast
based on commands that have nothing to do with the gimbal.

typhoon_h480: use gimbal v2 protocol, use yaw stab

Let's by default use the v2 protocol with typhoon_h480 and enable yaw
lock mode by stabilizing yaw.

msg/CMakeLists.txt

@@ -66,6 +66,12 @@ set(msg_files
 	follow_target.msg
 	generator_status.msg
 	geofence_result.msg
+	gimbal_device_set_attitude.msg
+	gimbal_manager_set_attitude.msg
+	gimbal_device_attitude_status.msg
+	gimbal_device_information.msg
+	gimbal_manager_information.msg
+	gimbal_manager_status.msg
 	gps_dump.msg
 	gps_inject_data.msg
 	heater_status.msg

msg/gimbal_device_attitude_status.msg

@@ -0,0 +1,18 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint8 target_system
+uint8 target_component
+uint16 device_flags
+
+uint16 DEVICE_FLAGS_RETRACT = 1
+uint16 DEVICE_FLAGS_NEUTRAL = 2
+uint16 DEVICE_FLAGS_ROLL_LOCK = 4
+uint16 DEVICE_FLAGS_PITCH_LOCK = 8
+uint16 DEVICE_FLAGS_YAW_LOCK = 16
+
+float32[4] q
+float32 angular_velocity_x
+float32 angular_velocity_y
+float32 angular_velocity_z
+
+uint32 failure_flags

msg/gimbal_device_information.msg

@@ -0,0 +1,27 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint8[32] vendor_name
+uint8[32] model_name
+uint32 firmware_version
+
+uint16 capability_flags
+
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS = 4
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW = 8
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK = 16
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS = 32
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW = 64
+uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK = 128
+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 pan_max # [rad]
+float32 pan_min # [rad]
+float32 pan_rate_max # [rad / s]

msg/gimbal_device_set_attitude.msg

@@ -0,0 +1,17 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint8 target_system
+uint8 target_component
+
+uint16 flags
+uint32 GIMBAL_DEVICE_FLAGS_RETRACT = 1
+uint32 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2
+uint32 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4
+uint32 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8
+uint32 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16
+
+float32[4] q
+
+float32 angular_velocity_x
+float32 angular_velocity_y
+float32 angular_velocity_z

msg/gimbal_manager_information.msg

@@ -0,0 +1,13 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint32 capability_flags
+
+uint8 gimbal_device_id
+
+float32 tilt_max # [rad]
+float32 tilt_min # [rad]
+float32 tilt_rate_max # [rad / s]
+
+float32 pan_max # [rad]
+float32 pan_min # [rad]
+float32 pan_rate_max # [rad / s]

msg/gimbal_manager_set_attitude.msg

@@ -0,0 +1,23 @@
+uint64 timestamp						# time since system start (microseconds)
+
+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 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
+
+float32[4] q
+
+float32 angular_velocity_x
+float32 angular_velocity_y
+float32 angular_velocity_z

msg/gimbal_manager_status.msg

@@ -0,0 +1,4 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint32 flags
+uint8 gimbal_device_id

msg/tools/uorb_rtps_message_ids.yaml

@@ -315,6 +315,18 @@ rtps:
     id: 149
   - msg: heater_status
     id: 150
+  - msg: gimbal_device_attitude_status
+    id: 151
+  - msg: gimbal_device_information
+    id: 152
+  - msg: gimbal_device_set_attitude
+    id: 153
+  - msg: gimbal_manager_information
+    id: 154
+  - msg: gimbal_manager_set_attitude
+    id: 155
+  - msg: gimbal_manager_status
+    id: 156
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 170

msg/vehicle_command.msg

@@ -66,12 +66,16 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 ==
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260			# Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
+uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal
+
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm|
 uint16 VEHICLE_CMD_INJECT_FAILURE = 420			# Inject artificial failure for testing purposes
 uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
+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_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/mavlink/mavlink_main.cpp

@@ -1587,6 +1587,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ESTIMATOR_STATUS", 0.5f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
+		configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
+		configure_stream_local("GIMBAL_MANAGER_STATUS", 5.0f);
+		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
 		configure_stream_local("GPS2_RAW", 1.0f);
 		configure_stream_local("GPS_RAW_INT", 1.0f);
 		configure_stream_local("GPS_STATUS", 1.0f);
@@ -1642,6 +1645,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("COLLISION", unlimited_rate);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
+		configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
+		configure_stream_local("GIMBAL_MANAGER_STATUS", 5.0f);
+		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);

src/modules/mavlink/mavlink_messages.cpp

@@ -69,6 +69,10 @@
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/geofence_result.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
+#include <uORB/topics/gimbal_device_information.h>
+#include <uORB/topics/gimbal_device_set_attitude.h>
+#include <uORB/topics/gimbal_manager_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_baro.h>
@@ -1867,6 +1871,295 @@ protected:
 	}
 };
 
+class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamGimbalDeviceAttitudeStatus::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "GIMBAL_DEVICE_ATTITUDE_STATUS";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamGimbalDeviceAttitudeStatus(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return _gimbal_device_attitude_status_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN +
+		       MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
+
+	/* do not allow top copying this class */
+	MavlinkStreamGimbalDeviceAttitudeStatus(MavlinkStreamGimbalDeviceAttitudeStatus &) = delete;
+	MavlinkStreamGimbalDeviceAttitudeStatus &operator = (const MavlinkStreamGimbalDeviceAttitudeStatus &) = delete;
+
+protected:
+	explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		gimbal_device_attitude_status_s gimbal_device_attitude_status{};
+
+		if (_gimbal_device_attitude_status_sub.update(&gimbal_device_attitude_status)) {
+			mavlink_gimbal_device_attitude_status_t msg{};
+
+			msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
+			msg.q[0] = gimbal_device_attitude_status.q[0];
+			msg.q[1] = gimbal_device_attitude_status.q[1];
+			msg.q[2] = gimbal_device_attitude_status.q[2];
+			msg.q[3] = gimbal_device_attitude_status.q[3];
+			msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x;
+			msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y;
+			msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z;
+			msg.failure_flags = gimbal_device_attitude_status.failure_flags;
+			msg.flags = gimbal_device_attitude_status.device_flags;
+
+			mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+class MavlinkStreamGimbalManagerInformation : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamGimbalManagerInformation::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "GIMBAL_MANAGER_INFORMATION";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamGimbalManagerInformation(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return _gimbal_device_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN +
+				MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+	}
+
+private:
+	uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
+
+	/* do not allow top copying this class */
+	MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete;
+	MavlinkStreamGimbalManagerInformation &operator = (const MavlinkStreamGimbalManagerInformation &) = delete;
+
+protected:
+	explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		gimbal_device_information_s gimbal_device_information;
+
+		if (_gimbal_device_information_sub.advertised() && _gimbal_device_information_sub.copy(&gimbal_device_information)) {
+			// send out gimbal_manager_info with info from gimbal_device_information
+			mavlink_gimbal_manager_information_t msg{};
+			msg.time_boot_ms = gimbal_device_information.timestamp / 1000;
+			msg.gimbal_device_id = 0;
+			msg.cap_flags = gimbal_device_information.capability_flags;
+
+			msg.tilt_max = gimbal_device_information.tilt_max;
+			msg.tilt_min = gimbal_device_information.tilt_min;
+			msg.tilt_rate_max = gimbal_device_information.tilt_rate_max;
+
+			msg.pan_max = gimbal_device_information.pan_max;
+			msg.pan_min = gimbal_device_information.pan_min;
+			msg.pan_rate_max = gimbal_device_information.pan_rate_max;
+
+			mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+
+		}
+
+		return false;
+	}
+};
+
+class MavlinkStreamGimbalManagerStatus : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamGimbalManagerStatus::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "GIMBAL_MANAGER_STATUS";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamGimbalManagerStatus(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return _gimbal_manager_status_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN +
+				MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+	}
+
+private:
+	uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)};
+
+	/* do not allow top copying this class */
+	MavlinkStreamGimbalManagerStatus(MavlinkStreamGimbalManagerStatus &) = delete;
+	MavlinkStreamGimbalManagerStatus &operator = (const MavlinkStreamGimbalManagerStatus &) = delete;
+
+protected:
+	explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		gimbal_manager_status_s gimbal_manager_status;
+
+		if (_gimbal_manager_status_sub.advertised() && _gimbal_manager_status_sub.copy(&gimbal_manager_status)) {
+			// send out gimbal_manager_info with info from gimbal_device_information
+			mavlink_gimbal_manager_status_t msg{};
+			msg.time_boot_ms = gimbal_manager_status.timestamp / 1000;
+			msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id;
+			msg.flags = gimbal_manager_status.flags;
+
+			mavlink_msg_gimbal_manager_status_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+
+		}
+
+		return false;
+	}
+};
+
+
+
+class MavlinkStreamGimbalDeviceSetAttitude : public MavlinkStream
+{
+public:
+	const char *get_name() const override
+	{
+		return MavlinkStreamGimbalDeviceSetAttitude::get_name_static();
+	}
+
+	static constexpr const char *get_name_static()
+	{
+		return "GIMBAL_DEVICE_SET_ATTITUDE";
+	}
+
+	static constexpr uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE;
+	}
+
+	uint16_t get_id() override
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamGimbalDeviceSetAttitude(mavlink);
+	}
+
+	unsigned get_size() override
+	{
+		return _gimbal_device_set_attitude_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN +
+				MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+	}
+
+private:
+	uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)};
+
+	/* do not allow top copying this class */
+	MavlinkStreamGimbalDeviceSetAttitude(MavlinkStreamGimbalDeviceSetAttitude &) = delete;
+	MavlinkStreamGimbalDeviceSetAttitude &operator = (const MavlinkStreamGimbalDeviceSetAttitude &) = delete;
+
+protected:
+	explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	bool send(const hrt_abstime t) override
+	{
+		gimbal_device_set_attitude_s gimbal_device_set_attitude;
+
+		if (_gimbal_device_set_attitude_sub.advertised() && _gimbal_device_set_attitude_sub.copy(&gimbal_device_set_attitude)) {
+			// send out gimbal_manager_info with info from gimbal_device_information
+			mavlink_gimbal_device_set_attitude_t msg{};
+			msg.flags = gimbal_device_set_attitude.flags;
+			msg.target_system = gimbal_device_set_attitude.target_system;
+			msg.target_component = gimbal_device_set_attitude.target_component;
+
+			msg.q[0] = gimbal_device_set_attitude.q[0];
+			msg.q[1] = gimbal_device_set_attitude.q[1];
+			msg.q[2] = gimbal_device_set_attitude.q[2];
+			msg.q[3] = gimbal_device_set_attitude.q[3];
+
+			msg.angular_velocity_x = gimbal_device_set_attitude.angular_velocity_x;
+			msg.angular_velocity_y = gimbal_device_set_attitude.angular_velocity_y;
+			msg.angular_velocity_z = gimbal_device_set_attitude.angular_velocity_z;
+
+			mavlink_msg_gimbal_device_set_attitude_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+
+		}
+
+		return false;
+	}
+};
+
 class MavlinkStreamCameraTrigger : public MavlinkStream
 {
 public:
@@ -3046,6 +3339,10 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamEstimatorStatus>(),
 	create_stream_list_item<MavlinkStreamVibration>(),
 	create_stream_list_item<MavlinkStreamAttPosMocap>(),
+	create_stream_list_item<MavlinkStreamGimbalDeviceAttitudeStatus>(),
+	create_stream_list_item<MavlinkStreamGimbalManagerInformation>(),
+	create_stream_list_item<MavlinkStreamGimbalManagerStatus>(),
+	create_stream_list_item<MavlinkStreamGimbalDeviceSetAttitude>(),
 	create_stream_list_item<MavlinkStreamHomePosition>(),
 	create_stream_list_item<MavlinkStreamServoOutputRaw<0> >(),
 	create_stream_list_item<MavlinkStreamServoOutputRaw<1> >(),

src/modules/mavlink/mavlink_receiver.cpp

@@ -279,6 +279,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		break;
 #endif // !CONSTRAINED_FLASH
 
+	case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE:
+		handle_message_gimbal_manager_set_attitude(msg);
+		break;
+
+	case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
+		handle_message_gimbal_device_information(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -2938,6 +2946,66 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
 	}
 }
 
+
+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.target_system = set_attitude_msg.target_system;
+	gimbal_attitude.target_component = set_attitude_msg.target_component;
+	gimbal_attitude.flags = set_attitude_msg.flags;
+	gimbal_attitude.gimbal_device_id = set_attitude_msg.gimbal_device_id;
+
+	matrix::Quatf q(set_attitude_msg.q);
+	q.copyTo(gimbal_attitude.q);
+
+	gimbal_attitude.angular_velocity_x = set_attitude_msg.angular_velocity_x;
+	gimbal_attitude.angular_velocity_y = set_attitude_msg.angular_velocity_y;
+	gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z;
+
+	_gimbal_manager_set_attitude_pub.publish(gimbal_attitude);
+
+}
+
+void
+MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg)
+{
+
+	mavlink_gimbal_device_information_t gimbal_device_info_msg;
+	mavlink_msg_gimbal_device_information_decode(msg, &gimbal_device_info_msg);
+
+	gimbal_device_information_s gimbal_information{};
+	gimbal_information.timestamp = hrt_absolute_time();
+
+	static_assert(sizeof(gimbal_information.vendor_name) == sizeof(gimbal_device_info_msg.vendor_name),
+		      "vendor_name length doesn't match");
+	static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name),
+		      "model_name length doesn't match");
+	memcpy(gimbal_information.vendor_name, gimbal_device_info_msg.vendor_name, sizeof(gimbal_information.vendor_name));
+	memcpy(gimbal_information.model_name, gimbal_device_info_msg.model_name, sizeof(gimbal_information.model_name));
+	gimbal_device_info_msg.vendor_name[sizeof(gimbal_device_info_msg.vendor_name) - 1] = '\0';
+	gimbal_device_info_msg.model_name[sizeof(gimbal_device_info_msg.model_name) - 1] = '\0';
+
+	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.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_device_information_pub.publish(gimbal_information);
+
+}
+
 /**
  * Receive data from UART/UDP
  */

src/modules/mavlink/mavlink_receiver.h

@@ -67,6 +67,8 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/generator_status.h>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
+#include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/input_rc.h>
@@ -201,6 +203,9 @@ private:
 	void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
 	void handle_message_utm_global_position(mavlink_message_t *msg);
 	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_device_information(mavlink_message_t *msg);
 
 #if !defined(CONSTRAINED_FLASH)
 	void handle_message_debug(mavlink_message_t *msg);
@@ -353,6 +358,8 @@ private:
 	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<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)};
 	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)};
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};

src/modules/vmount/input_mavlink.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -40,6 +40,7 @@
 
 #include "input_mavlink.h"
 #include <uORB/Publication.hpp>
+#include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -49,6 +50,9 @@
 #include <px4_platform_common/posix.h>
 #include <errno.h>
 #include <math.h>
+#include <matrix/matrix/math.hpp>
+
+using matrix::wrap_pi;
 
 namespace vmount
 {
@@ -137,11 +141,6 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_
 			}
 
 			_cur_roi_mode = vehicle_roi.mode;
-
-			//set all other control data fields to defaults
-			for (int i = 0; i < 3; ++i) {
-				_control_data.stabilize_axis[i] = false;
-			}
 		}
 
 		// check whether the position setpoint got updated
@@ -175,8 +174,7 @@ void InputMavlinkROI::print_status()
 }
 
 
-InputMavlinkCmdMount::InputMavlinkCmdMount(bool stabilize)
+InputMavlinkCmdMount::InputMavlinkCmdMount()
-	: _stabilize {stabilize, stabilize, stabilize}
 {
 	param_t handle = param_find("MAV_SYS_ID");
 
@@ -259,10 +257,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 					continue;
 				}
 
-				for (int i = 0; i < 3; ++i) {
-					_control_data.stabilize_axis[i] = _stabilize[i];
-				}
-
 				_control_data.gimbal_shutter_retract = false;
 
 				if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) {
@@ -312,9 +306,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
 					_ack_vehicle_command(&vehicle_command);
 
 				} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
-					_stabilize[0] = (int)(vehicle_command.param2 + 0.5f) == 1;
+
-					_stabilize[1] = (int)(vehicle_command.param3 + 0.5f) == 1;
+					_control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1;
-					_stabilize[2] = (int)(vehicle_command.param4 + 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),
@@ -378,5 +374,430 @@ void InputMavlinkCmdMount::print_status()
 	PX4_INFO("Input: Mavlink (CMD_MOUNT)");
 }
 
+InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device)
+{
+	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 {
+		/* dumb gimbal or MAVLink v1 protocol gimbal: fake GIMBAL_DEVICE_INFORMATION */
+		_stream_gimbal_manager_information();
+	}
+}
+
+InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
+{
+	if (_vehicle_roi_sub >= 0) {
+		orb_unsubscribe(_vehicle_roi_sub);
+	}
+
+	if (_position_setpoint_triplet_sub >= 0) {
+		orb_unsubscribe(_position_setpoint_triplet_sub);
+	}
+
+	if (_gimbal_manager_set_attitude_sub >= 0) {
+		orb_unsubscribe(_gimbal_manager_set_attitude_sub);
+	}
+
+	if (_vehicle_command_sub >= 0) {
+		orb_unsubscribe(_vehicle_command_sub);
+	}
+}
+
+
+void InputMavlinkGimbalV2::print_status()
+{
+	PX4_INFO("Input: Mavlink (Gimbal V2)");
+}
+
+int InputMavlinkGimbalV2::initialize()
+{
+	_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
+
+	if (_vehicle_roi_sub < 0) {
+		return -errno;
+	}
+
+	_position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+
+	if (_position_setpoint_triplet_sub < 0) {
+		return -errno;
+	}
+
+	_gimbal_manager_set_attitude_sub  = orb_subscribe(ORB_ID(gimbal_manager_set_attitude));
+
+	if (_gimbal_manager_set_attitude_sub < 0) {
+		return -errno;
+	}
+
+	if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 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.
+	orb_set_interval(_vehicle_command_sub, 10);
+
+	return 0;
+}
+
+void InputMavlinkGimbalV2::_stream_gimbal_manager_status()
+{
+
+	if (_gimbal_device_attitude_status_sub.updated()) {
+		_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.gimbal_device_id = 0;
+	_gimbal_manager_status_pub.publish(gimbal_manager_status);
+
+}
+
+void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
+{
+	gimbal_device_information_s gimbal_device_info;
+	gimbal_device_info.timestamp = hrt_absolute_time();
+	const char vendor_name[] = "PX4";
+	const char model_name[] = "AUX gimbal";
+
+	strncpy((char *)gimbal_device_info.vendor_name, vendor_name, sizeof(gimbal_device_info.vendor_name));
+	strncpy((char *)gimbal_device_info.model_name, model_name, sizeof(gimbal_device_info.model_name));
+
+	gimbal_device_info.firmware_version = 0;
+	gimbal_device_info.capability_flags = 0;
+	gimbal_device_info.capability_flags = gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL |
+					      gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK |
+					      gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS |
+					      gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK |
+					      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_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();
+
+	// Default to no change, set if we receive anything.
+	*control_data = nullptr;
+
+	const int num_poll = 4;
+	px4_pollfd_struct_t polls[num_poll];
+	polls[0].fd = 		_gimbal_manager_set_attitude_sub;
+	polls[0].events = 	POLLIN;
+	polls[1].fd = 		_vehicle_roi_sub;
+	polls[1].events = 	POLLIN;
+	polls[2].fd = 		_position_setpoint_triplet_sub;
+	polls[2].events = 	POLLIN;
+	polls[3].fd = 		_vehicle_command_sub;
+	polls[3].events = 	POLLIN;
+
+	int poll_timeout = (int)timeout_ms;
+
+	bool exit_loop = false;
+
+	while (!exit_loop && poll_timeout >= 0) {
+		hrt_abstime poll_start = hrt_absolute_time();
+
+		int ret = px4_poll(polls, num_poll, poll_timeout);
+
+		if (ret < 0) {
+			return -errno;
+		}
+
+		poll_timeout -= (hrt_absolute_time() - poll_start) / 1000;
+
+		// if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout
+		exit_loop = true;
+
+		if (ret == 0) {
+			// Timeout control_data already null.
+
+		} else {
+			if (polls[0].revents & POLLIN) {
+				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();
+
+				_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);
+				*control_data = &_control_data;
+			}
+
+			if (polls[1].revents & POLLIN) {
+				vehicle_roi_s vehicle_roi;
+				orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi);
+
+				_control_data.gimbal_shutter_retract = false;
+
+				if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
+
+					_control_data.type = ControlData::Type::Neutral;
+					*control_data = &_control_data;
+					_is_roi_set = false;
+					_cur_roi_mode = vehicle_roi.mode;
+
+				} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
+					double lat, lon, alt = 0.;
+					_read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt);
+					_control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
+
+					_control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset;
+					_control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset;
+					_control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset;
+
+					_transform_lon_lat_to_angle(lon, lat, alt);
+
+					*control_data = &_control_data;
+					_is_roi_set = true;
+					_cur_roi_mode = vehicle_roi.mode;
+
+				} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
+
+					_transform_lon_lat_to_angle(vehicle_roi.lon, vehicle_roi.lat, (double)vehicle_roi.alt);
+					*control_data = &_control_data;
+					_is_roi_set = true;
+					_cur_roi_mode = vehicle_roi.mode;
+
+				} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
+					//TODO is this even suported?
+					exit_loop = false;
+
+				} else {
+					exit_loop = false;
+				}
+			}
+
+			// check whether the position setpoint got updated
+			if (polls[2].revents & POLLIN) {
+				if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
+					double lat, lon, alt = 0.;
+					_read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt);
+					_transform_lon_lat_to_angle(lon, lat, alt);
+					*control_data = &_control_data;
+
+				} else { // must do an orb_copy() in *every* case
+					position_setpoint_triplet_s position_setpoint_triplet;
+					orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
+					exit_loop = false;
+				}
+			}
+
+			if (polls[3].revents & POLLIN) {
+				vehicle_command_s vehicle_command;
+				orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
+
+				// Process only if the command is for us or for anyone (component id 0).
+				const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id) || (vehicle_command.target_system == 0);
+				const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) ||
+							     (vehicle_command.target_component == 0));
+
+				if (!sysid_correct || !compid_correct) {
+					exit_loop = false;
+					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);
+					*control_data = &_control_data;
+					_ack_vehicle_command(&vehicle_command);
+
+				} else {
+					exit_loop = false;
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+void InputMavlinkGimbalV2::_transform_lon_lat_to_angle(const double roi_lon, const double roi_lat,
+		const double roi_alt)
+{
+	vehicle_global_position_s vehicle_global_position;
+	_vehicle_global_position_sub.copy(&vehicle_global_position);
+	const double &vlat = vehicle_global_position.lat;
+	const double &vlon = vehicle_global_position.lon;
+
+	_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;
+
+	_control_data.type_data.angle.angles[0] = 0.f;
+
+	// 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;
+
+	} else {
+		_control_data.type_data.angle.angles[1] = _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;
+
+	// 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;
+
+	// 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]);
+}
+
+float InputMavlinkGimbalV2::_calculate_pitch(double lon, double lat, float altitude,
+		const vehicle_global_position_s &global_position)
+{
+	if (!map_projection_initialized(&_projection_reference)) {
+		map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
+	}
+
+	float x1, y1, x2, y2;
+	map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
+	map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
+	float dx = x1 - x2, dy = y1 - y2;
+	float target_distance = sqrtf(dx * dx + dy * dy);
+	float z = altitude - global_position.alt;
+
+	return atan2f(z, target_distance);
+}
+
+void InputMavlinkGimbalV2::_read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp)
+{
+	position_setpoint_triplet_s position_setpoint_triplet;
+	orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
+	lon_sp = position_setpoint_triplet.current.lon;
+	lat_sp = position_setpoint_triplet.current.lat;
+	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)
+{
+
+	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;
+		}
+
+		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
+		}
+
+		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
+		}
+
+		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
+		}
+
+		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;
+		}
+	}
+}
+
+//TODO move this one to input.cpp such that it can be shared across functions
+void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd)
+{
+	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.target_system = cmd->source_system;
+	vehicle_command_ack.target_component = cmd->source_component;
+
+	uORB::PublicationQueued<vehicle_command_ack_s> cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	cmd_ack_pub.publish(vehicle_command_ack);
+}
 
 } /* namespace vmount */

src/modules/vmount/input_mavlink.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -43,8 +43,17 @@
 #include "input_rc.h"
 #include <cstdint>
 
+#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>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_roi.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <lib/ecl/geo/geo.h>
+
 
 namespace vmount
 {
@@ -69,7 +78,7 @@ private:
 
 	int _vehicle_roi_sub = -1;
 	int _position_setpoint_triplet_sub = -1;
-	uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
+	uint8_t _cur_roi_mode {vehicle_roi_s::ROI_NONE};
 };
 
 
@@ -80,7 +89,7 @@ private:
 class InputMavlinkCmdMount : public InputBase
 {
 public:
-	InputMavlinkCmdMount(bool stabilize);
+	InputMavlinkCmdMount();
 	virtual ~InputMavlinkCmdMount();
 
 	virtual void print_status();
@@ -93,11 +102,55 @@ private:
 	void _ack_vehicle_command(vehicle_command_s *cmd);
 
 	int _vehicle_command_sub = -1;
-	bool _stabilize[3] = { false, false, false };
 
 	int32_t _mav_sys_id{1}; ///< our mavlink system id
 	int32_t _mav_comp_id{1}; ///< our mavlink component id
 };
 
+class InputMavlinkGimbalV2 : public InputBase
+{
+public:
+	InputMavlinkGimbalV2(bool has_v2_gimbal_device);
+	virtual ~InputMavlinkGimbalV2();
+
+	virtual void print_status();
+
+protected:
+	virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
+	virtual int initialize();
+
+private:
+
+	void _transform_lon_lat_to_angle(const double roi_lon, const double roi_lat, const double roi_alt);
+	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 _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 _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
+
+	bool _is_roi_set{false};
+
+	uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Publication<gimbal_device_information_s> _gimbal_device_info_pub{ORB_ID(gimbal_device_information)};
+	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

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -49,8 +49,7 @@ namespace vmount
 {
 
 
-InputRC::InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
+InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
-	: _do_stabilization(do_stabilization)
 {
 	_aux_channels[0] = aux_channel_roll;
 	_aux_channels[1] = aux_channel_pitch;
@@ -134,7 +133,6 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
 		for (int i = 0; i < 3; ++i) {
 			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;
-			control_data.stabilize_axis[i] = _do_stabilization;
 
 			_last_set_aux_values[i] = new_aux_values[i];
 		}

src/modules/vmount/input_rc.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -57,12 +57,11 @@ class InputRC : public InputBase
 public:
 
 	/**
-	 * @param do_stabilization
 	 * @param aux_channel_roll   which aux channel to use for roll (set to 0 to use a fixed angle of 0)
 	 * @param aux_channel_pitch
 	 * @param aux_channel_yaw
 	 */
-	InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw);
+	InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw);
 	virtual ~InputRC();
 
 	virtual void print_status();
@@ -81,7 +80,6 @@ protected:
 	float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
 
 private:
-	const bool _do_stabilization;
 	int _aux_channels[3];
 	int _manual_control_setpoint_sub = -1;
 

src/modules/vmount/input_test.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -71,8 +71,6 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool
 
 	for (int i = 0; i < 3; ++i) {
 		_control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F;
-
-		_control_data.stabilize_axis[i] = true;
 	}
 
 	_control_data.gimbal_shutter_retract = false;

src/modules/vmount/output.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -74,6 +74,13 @@ void OutputBase::publish()
 	_mount_orientation_pub.publish(mount_orientation);
 }
 
+void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)
+{
+	_stabilize[0] = roll_stabilize;
+	_stabilize[1] = pitch_stabilize;
+	_stabilize[2] = yaw_stabilize;
+}
+
 float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 				   const vehicle_global_position_s &global_position)
 {
@@ -96,18 +103,26 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
 	_cur_control_data = control_data;
 
 	for (int i = 0; i < 3; ++i) {
-		_stabilize[i] = control_data->stabilize_axis[i];
 		_angle_speeds[i] = 0.f;
 	}
 
 	switch (control_data->type) {
 	case ControlData::Type::Angle:
 		for (int i = 0; i < 3; ++i) {
-			if (control_data->type_data.angle.frames[i] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
+			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;
 
-			} else {
+			case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame:
+				_absolute_angle[i] = true;
 				_angle_setpoints[i] = control_data->type_data.angle.angles[i];
+				break;
 			}
 		}
 
@@ -188,13 +203,23 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t)
 	vehicle_attitude_s vehicle_attitude{};
 	matrix::Eulerf euler;
 
-	if (_stabilize[0] || _stabilize[1] || _stabilize[2]) {
+	// We only need to apply additional compensation if the required angle is
+	// absolute (world frame) as well as the gimbal is not capable of doing that
+	// calculation. (Most gimbals stabilize at least roll and pitch
+	// and only need compensation for yaw, if at all.)
+	bool compensate[3];
+
+	for (int i = 0; i < 3; ++i) {
+		compensate[i] = _stabilize[i] && _absolute_angle[i];
+	}
+
+	if (compensate[0] || compensate[1] || compensate[2]) {
 		_vehicle_attitude_sub.copy(&vehicle_attitude);
 		euler = matrix::Quatf(vehicle_attitude.q);
 	}
 
 	for (int i = 0; i < 3; ++i) {
-		if (_stabilize[i]) {
+		if (compensate[i]) {
 			_angle_outputs[i] = _angle_setpoints[i] - euler(i);
 
 		} else {

src/modules/vmount/output.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -97,6 +97,8 @@ public:
 	/** Publish _angle_outputs as a mount_orientation message. */
 	void publish();
 
+	void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
+
 protected:
 	float _calculate_pitch(double lon, double lat, float altitude,
 			       const vehicle_global_position_s &global_position);
@@ -116,6 +118,10 @@ protected:
 	float _angle_speeds[3] = { 0.f, 0.f, 0.f };
 	bool _stabilize[3] = { false, false, false };
 
+	// Pitch and role are by default aligned with the horizon.
+	// Yaw follows the vehicle (not lock/absolute mode).
+	bool _absolute_angle[3] = {true, true, false };
+
 	/** calculate the _angle_outputs (with speed) and stabilize if needed */
 	void _calculate_output_angles(const hrt_abstime &t);
 

src/modules/vmount/output_mavlink.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -41,6 +41,7 @@
 #include "output_mavlink.h"
 
 #include <math.h>
+#include <matrix/matrix/math.hpp>
 
 #include <uORB/topics/vehicle_command.h>
 #include <px4_platform_common/defines.h>
@@ -49,12 +50,12 @@
 namespace vmount
 {
 
-OutputMavlink::OutputMavlink(const OutputConfig &output_config)
+OutputMavlinkV1::OutputMavlinkV1(const OutputConfig &output_config)
 	: OutputBase(output_config)
 {
 }
 
-int OutputMavlink::update(const ControlData *control_data)
+int OutputMavlinkV1::update(const ControlData *control_data)
 {
 	vehicle_command_s vehicle_command{};
 	vehicle_command.timestamp = hrt_absolute_time();
@@ -83,9 +84,9 @@ int OutputMavlink::update(const ControlData *control_data)
 			vehicle_command.param7 = static_cast<float>(control_data->type_data.angle.frames[2]);
 		}
 
-		vehicle_command.param2 = control_data->stabilize_axis[0];
+		vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f;
-		vehicle_command.param3 = control_data->stabilize_axis[1];
+		vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f;
-		vehicle_command.param4 = control_data->stabilize_axis[2];
+		vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f;
 
 		_vehicle_command_pub.publish(vehicle_command);
 	}
@@ -112,10 +113,87 @@ int OutputMavlink::update(const ControlData *control_data)
 	return 0;
 }
 
-void OutputMavlink::print_status()
+void OutputMavlinkV1::print_status()
 {
-	PX4_INFO("Output: Mavlink");
+	PX4_INFO("Output: MAVLink gimbal protocol v1");
 }
 
-} /* namespace vmount */
+OutputMavlinkV2::OutputMavlinkV2(const OutputConfig &output_config)
+	: OutputBase(output_config)
+{
+}
+
+int OutputMavlinkV2::update(const ControlData *control_data)
+{
+	if (control_data) {
+		//got new command
+		_set_angle_setpoints(control_data);
+		_publish_gimbal_device_set_attitude(control_data);
+	}
+
+	_handle_position_update();
 
+	hrt_abstime t = hrt_absolute_time();
+	_calculate_output_angles(t);
+
+	_last_update = t;
+
+	return 0;
+}
+
+void OutputMavlinkV2::print_status()
+{
+	PX4_INFO("Output: MAVLink gimbal protocol v2");
+}
+
+void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *control_data)
+{
+	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
+
+	}
+
+	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) {
+		set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_NEUTRAL;
+	}
+
+	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;
+	}
+
+	if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
+		set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK;
+	}
+
+	if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
+		set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK;
+	}
+
+	_gimbal_device_set_attitude_pub.publish(set_attitude);
+
+}
+
+} /* namespace vmount */

src/modules/vmount/output_mavlink.h

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -43,6 +43,8 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/gimbal_device_set_attitude.h>
+
 
 namespace vmount
 {
@@ -50,20 +52,33 @@ namespace vmount
  ** class OutputMavlink
  *  Output via vehicle_command topic
  */
-class OutputMavlink : public OutputBase
+class OutputMavlinkV1 : public OutputBase
 {
 public:
-	OutputMavlink(const OutputConfig &output_config);
+	OutputMavlinkV1(const OutputConfig &output_config);
-	virtual ~OutputMavlink() = default;
+	virtual ~OutputMavlinkV1() = default;
 
 	virtual int update(const ControlData *control_data);
 
 	virtual void print_status();
 
 private:
-
+	uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
-	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 };
 
+class OutputMavlinkV2 : public OutputBase
+{
+public:
+	OutputMavlinkV2(const OutputConfig &output_config);
+	virtual ~OutputMavlinkV2() = default;
+
+	virtual int update(const ControlData *control_data);
+
+	virtual void print_status();
+
+private:
+	void _publish_gimbal_device_set_attitude(const ControlData *control_data);
+	uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
+};
 
 } /* namespace vmount */

src/modules/vmount/output_rc.cpp

@@ -42,7 +42,7 @@
 
 #include <uORB/topics/actuator_controls.h>
 #include <px4_platform_common/defines.h>
-
+#include <matrix/matrix/math.hpp>
 
 namespace vmount
 {
@@ -73,6 +73,8 @@ int OutputRC::update(const ControlData *control_data)
 	actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale;
 	actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value;
 
+	_stream_device_attitude_status();
+
 	_actuator_controls_pub.publish(actuator_controls);
 
 	_last_update = t;
@@ -85,5 +87,26 @@ void OutputRC::print_status()
 	PX4_INFO("Output: AUX");
 }
 
-} /* namespace vmount */
+void OutputRC::_stream_device_attitude_status()
+{
+	gimbal_device_attitude_status_s attitude_status{};
+	attitude_status.timestamp = hrt_absolute_time();
+	attitude_status.target_system = 0;
+	attitude_status.target_component = 0;
+	attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK |
+				       gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK;
+
+	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);
+
+	attitude_status.failure_flags = 0;
+	_attitude_status_pub.publish(attitude_status);
+}
 
+} /* namespace vmount */

src/modules/vmount/output_rc.h

@@ -43,6 +43,7 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
 
 namespace vmount
 {
@@ -63,7 +64,10 @@ public:
 	virtual void print_status();
 
 private:
+	void _stream_device_attitude_status();
+
 	uORB::Publication <actuator_controls_s>	_actuator_controls_pub{ORB_ID(actuator_controls_2)};
+	uORB::Publication <gimbal_device_attitude_status_s>	_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
 
 	bool _retract_gimbal = true;
 };

src/modules/vmount/vmount.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -243,20 +243,22 @@ static int vmount_thread_main(int argc, char *argv[])
 				case 0:
 
 					// Automatic
-					thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab);
+					thread_data.input_objs[0] = new InputMavlinkCmdMount();
 					thread_data.input_objs[1] = new InputMavlinkROI();
 
 					// RC is on purpose last here so that if there are any mavlink
 					// messages, they will take precedence over RC.
 					// This logic is done further below while update() is called.
-					thread_data.input_objs[2] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch,
+					thread_data.input_objs[2] = new InputRC(params.mnt_man_roll,
+										params.mnt_man_pitch,
 										params.mnt_man_yaw);
 					thread_data.input_objs_len = 3;
 
 					break;
 
 				case 1: //RC
-					thread_data.input_objs[0] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch,
+					thread_data.input_objs[0] = new InputRC(params.mnt_man_roll,
+										params.mnt_man_pitch,
 										params.mnt_man_yaw);
 					break;
 
@@ -265,7 +267,11 @@ static int vmount_thread_main(int argc, char *argv[])
 					break;
 
 				case 3: //MAVLINK_DO_MOUNT
-					thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab);
+					thread_data.input_objs[0] = new InputMavlinkCmdMount();
+					break;
+
+				case 4: //MAVLINK_V2
+					thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2);
 					break;
 
 				default:
@@ -288,8 +294,15 @@ static int vmount_thread_main(int argc, char *argv[])
 
 				break;
 
-			case 1: //MAVLINK
+			case 1: //MAVLink v1 gimbal protocol
-				thread_data.output_obj = new OutputMavlink(output_config);
+				thread_data.output_obj = new OutputMavlinkV1(output_config);
+
+				if (!thread_data.output_obj) { alloc_failed = true; }
+
+				break;
+
+			case 2: //MAVLink v2 gimbal protocol
+				thread_data.output_obj = new OutputMavlinkV2(output_config);
 
 				if (!thread_data.output_obj) { alloc_failed = true; }
 
@@ -318,6 +331,16 @@ static int vmount_thread_main(int argc, char *argv[])
 				thread_should_exit = true;
 				break;
 			}
+
+			if (params.mnt_do_stab == 1) {
+				thread_data.output_obj->set_stabilize(true, true, true);
+
+			} else if (params.mnt_do_stab == 2) {
+				thread_data.output_obj->set_stabilize(false, false, true);
+
+			} else {
+				thread_data.output_obj->set_stabilize(false, false, false);
+			}
 		}
 
 		if (thread_data.input_objs_len > 0) {

src/modules/vmount/vmount_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -48,8 +48,9 @@
 * @value -1 DISABLED
 * @value 0 AUTO
 * @value 1 RC
-* @value 2 MAVLINK_ROI
+* @value 2 MAVLINK_ROI (protocol v1)
-* @value 3 MAVLINK_DO_MOUNT
+* @value 3 MAVLINK_DO_MOUNT (protocol v1)
+* @value 4 MAVlink gimbal protocol v2
 * @min -1
 * @max 3
 * @group Mount
@@ -65,9 +66,10 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
 * to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID)
 *
 * @value 0 AUX
-* @value 1 MAVLINK
+* @value 1 MAVLink gimbal protocol v1
+* @value 2 MAVLink gimbal protocol v2
 * @min 0
-* @max 1
+* @max 2
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);
@@ -75,7 +77,7 @@ PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);
 /**
 * Mavlink System ID of the mount
 *
-* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this target ID.
+* If MNT_MODE_OUT is MAVLink gimbal protocol v1, mount configure/control commands will be sent with this target ID.
 *
 * @group Mount
 */
@@ -84,7 +86,7 @@ PARAM_DEFINE_INT32(MNT_MAV_SYSID, 1);
 /**
 * Mavlink Component ID of the mount
 *
-* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this component ID.
+* If MNT_MODE_OUT is MAVLink protocol v2, mount configure/control commands will be sent with this component ID.
 *
 * @group Mount
 */
@@ -162,9 +164,14 @@ PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);
 
 /**
 * Stabilize the mount (set to true for servo gimbal, false for passthrough).
-* Does not affect MAVLINK_ROI input.
+* (This is required for a gimbal which is not capable of stabilizing itself
+* and relies on the IMU's attitude estimation.)
 *
-* @boolean
+* @value 0 Disable
+* @value 1 Stabilize all axis
+* @value 2 Stabilize yaw for absolute/lock mode.
+* @min 0
+* @max 2
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_DO_STAB, 0);