vmount/navigator/mavlink: gimbal v2 changes

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

src/modules/vmount/common.h

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

src/modules/vmount/input.h

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

src/modules/vmount/input_mavlink.cpp

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

src/modules/vmount/input_mavlink.h

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

src/modules/vmount/input_rc.cpp

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

src/modules/vmount/input_test.cpp

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

src/modules/vmount/output.cpp

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

src/modules/vmount/output.h

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

src/modules/vmount/output_mavlink.cpp

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

src/modules/vmount/output_mavlink.h

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

src/modules/vmount/output_rc.cpp

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

src/modules/vmount/vmount.cpp

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

src/modules/vmount/vmount_params.c

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