diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 25f4f0c734..ab36952df7 100644
--- a/msg/CMakeLists.txt
+++ b/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
diff --git a/msg/gimbal_device_attitude_status.msg b/msg/gimbal_device_attitude_status.msg
new file mode 100644
index 0000000000..de2d7e892b
--- /dev/null
+++ b/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
diff --git a/msg/gimbal_device_information.msg b/msg/gimbal_device_information.msg
new file mode 100644
index 0000000000..47d9cee6a2
--- /dev/null
+++ b/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]
diff --git a/msg/gimbal_device_set_attitude.msg b/msg/gimbal_device_set_attitude.msg
new file mode 100644
index 0000000000..f224a23441
--- /dev/null
+++ b/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
diff --git a/msg/gimbal_manager_information.msg b/msg/gimbal_manager_information.msg
new file mode 100644
index 0000000000..bb8b25de1c
--- /dev/null
+++ b/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]
diff --git a/msg/gimbal_manager_set_attitude.msg b/msg/gimbal_manager_set_attitude.msg
new file mode 100644
index 0000000000..6ec9e76141
--- /dev/null
+++ b/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
diff --git a/msg/gimbal_manager_status.msg b/msg/gimbal_manager_status.msg
new file mode 100644
index 0000000000..f672bc1121
--- /dev/null
+++ b/msg/gimbal_manager_status.msg
@@ -0,0 +1,4 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint32 flags
+uint8 gimbal_device_id
diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml
index 9727a1886e..b57bd5c8a0 100644
--- a/msg/tools/uorb_rtps_message_ids.yaml
+++ b/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
diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg
index 9b73b96faa..0989cd96d4 100644
--- a/msg/vehicle_command.msg
+++ b/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
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a8fbf7454f..0814e14b19 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/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);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 170b83868f..82eba94c69 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/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> >(),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 56a63bac3b..8f252a3cf4 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/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
  */
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 1c8171054c..f252707e6b 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/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)};
diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp
index 570dd2d086..710bbab523 100644
--- a/src/modules/vmount/input_mavlink.cpp
+++ b/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)
-	: _stabilize {stabilize, stabilize, stabilize}
+InputMavlinkCmdMount::InputMavlinkCmdMount()
 {
 	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;
-					_stabilize[2] = (int)(vehicle_command.param4 + 0.5f) == 1;
+
+					_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),
@@ -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 */
diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h
index b3661466ec..2a70284294 100644
--- a/src/modules/vmount/input_mavlink.h
+++ b/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 */
diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp
index b917914807..df1311270b 100644
--- a/src/modules/vmount/input_rc.cpp
+++ b/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)
-	: _do_stabilization(do_stabilization)
+InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
 {
 	_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];
 		}
diff --git a/src/modules/vmount/input_rc.h b/src/modules/vmount/input_rc.h
index 14c39c4053..89cf3a6034 100644
--- a/src/modules/vmount/input_rc.h
+++ b/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;
 
diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp
index 2654b8500e..ef8cc947bc 100644
--- a/src/modules/vmount/input_test.cpp
+++ b/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;
diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp
index 706190eab9..82c2cdc929 100644
--- a/src/modules/vmount/output.cpp
+++ b/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 {
diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h
index 5b34a6c4d5..7356ec9edc 100644
--- a/src/modules/vmount/output.h
+++ b/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);
 
diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp
index 9babd00497..2ed277ff4b 100644
--- a/src/modules/vmount/output_mavlink.cpp
+++ b/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.param3 = control_data->stabilize_axis[1];
-		vehicle_command.param4 = control_data->stabilize_axis[2];
+		vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f;
+		vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f;
+		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 */
diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h
index 5ace7f6582..8c0a4808a0 100644
--- a/src/modules/vmount/output_mavlink.h
+++ b/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);
-	virtual ~OutputMavlink() = default;
+	OutputMavlinkV1(const OutputConfig &output_config);
+	virtual ~OutputMavlinkV1() = default;
 
 	virtual int update(const ControlData *control_data);
 
 	virtual void print_status();
 
 private:
-
-	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<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 */
diff --git a/src/modules/vmount/output_rc.cpp b/src/modules/vmount/output_rc.cpp
index 196772a6fc..a829d4cb3b 100644
--- a/src/modules/vmount/output_rc.cpp
+++ b/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 */
diff --git a/src/modules/vmount/output_rc.h b/src/modules/vmount/output_rc.h
index 340e913cc7..52839cac69 100644
--- a/src/modules/vmount/output_rc.h
+++ b/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;
 };
diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp
index d853d3bd0c..406cf74aff 100644
--- a/src/modules/vmount/vmount.cpp
+++ b/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
-				thread_data.output_obj = new OutputMavlink(output_config);
+			case 1: //MAVLink v1 gimbal protocol
+				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) {
diff --git a/src/modules/vmount/vmount_params.c b/src/modules/vmount/vmount_params.c
index 556987360e..6d067da61b 100644
--- a/src/modules/vmount/vmount_params.c
+++ b/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 3 MAVLINK_DO_MOUNT
+* @value 2 MAVLINK_ROI (protocol v1)
+* @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);