Support for gimbal v2 protocol

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

mavlink_receiver handle GIMBAL_MANAGER_SET_ATTITUDE

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

add gimbal information message

add gimbal manager information and vehicle command ack

mavlink messages: add stream for GIMBAL_MANAGER_INFORMATION

mavlink_receiver: handle GIMBAL_DEVICE_INFORMATION

remove mavlink cmd handling from vmount input MavlinkGimbalV2

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

small fixes

fix typos

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

add support for CMD_DO_GIMBAL_MANAGER_ATTITUDE

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

better handle the request for gimbal infomation request

clean up

bring gimbal information back on vmount init

add new gimbal message to mavlink normal stream

fix publication of gimbal device information

rename gimbal_attitude_status to gimbal_device_attitude_status

stream gimbal_manager_status at 5Hz

mavlink: send information only on request

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

mavlink: debug output for now

make sure to copy over control data

mavlink: add missing copyright header, pragma once

mavlink: address review comments

mavlink: handle stream not updated

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

mavlink: remove outdated comment

vmount: add option for yaw stabilization only

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

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

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

vmount: fix incorrect check for bit flag

mavlink_messages: remove debug message

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

use device id

remove debug print

gimbal attitude fix mistake

clang tidy fix

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

add gimbal protocol messages to rtps msg ids

support set attitude for gimbal directly speaking mavlink

clean up gimbal urob messages

vmount: address a few small review comments

vmount: split output into v1 and v2 protocol

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

vmount: config via ctor instead of duplicate param

vmount: use loop to poll all topics

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

typhoon_h480: use gimbal v2 protocol, use yaw stab

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

src/modules/mavlink/mavlink_main.cpp

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

src/modules/mavlink/mavlink_messages.cpp

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

src/modules/mavlink/mavlink_receiver.cpp

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

src/modules/mavlink/mavlink_receiver.h

@@ -67,6 +67,8 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/generator_status.h>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
+#include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/input_rc.h>
@@ -201,6 +203,9 @@ private:
 	void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
 	void handle_message_utm_global_position(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
+	void handle_message_onboard_computer_status(mavlink_message_t *msg);
+	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
+	void handle_message_gimbal_device_information(mavlink_message_t *msg);
 
 #if !defined(CONSTRAINED_FLASH)
 	void handle_message_debug(mavlink_message_t *msg);
@@ -353,6 +358,8 @@ private:
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};
 	uORB::Publication<follow_target_s>			_follow_target_pub{ORB_ID(follow_target)};
 	uORB::Publication<irlock_report_s>			_irlock_report_pub{ORB_ID(irlock_report)};
+	uORB::Publication<gimbal_manager_set_attitude_s>	_gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
+	uORB::Publication<gimbal_device_information_s>		_gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
 	uORB::Publication<landing_target_pose_s>		_landing_target_pose_pub{ORB_ID(landing_target_pose)};
 	uORB::Publication<log_message_s>			_log_message_pub{ORB_ID(log_message)};
 	uORB::Publication<obstacle_distance_s>			_obstacle_distance_pub{ORB_ID(obstacle_distance)};