From 90fe4b7a541dbf49e70d9b59f5fa863870e95c6c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 2 Jul 2021 16:33:04 +0200
Subject: [PATCH] vmount: publish gimbal v2 protocol for v1 gimbals

This enables the use case where the gimbal v2 protocol is used
between the ground station and the drone, and the gimbal v1 protocol is
used between the drone and the gimbal.
---
 src/modules/vmount/output_mavlink.cpp | 23 +++++++++++++++++++++++
 src/modules/vmount/output_mavlink.h   |  3 +++
 2 files changed, 26 insertions(+)

diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp
index c8ea6555de..38732cab9f 100644
--- a/src/modules/vmount/output_mavlink.cpp
+++ b/src/modules/vmount/output_mavlink.cpp
@@ -108,11 +108,34 @@ int OutputMavlinkV1::update(const ControlData *control_data)
 
 	_vehicle_command_pub.publish(vehicle_command);
 
+	_stream_device_attitude_status();
+
 	_last_update = t;
 
 	return 0;
 }
 
+void OutputMavlinkV1::_stream_device_attitude_status()
+{
+	// This enables the use case where the gimbal v2 protocol is used
+	// between the ground station and the drone, and the gimbal v1 protocol is
+	// used between the drone and the gimbal.
+	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;
+
+	matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]);
+	matrix::Quatf q(euler);
+	q.copyTo(attitude_status.q);
+
+	attitude_status.failure_flags = 0;
+	_attitude_status_pub.publish(attitude_status);
+}
+
 void OutputMavlinkV1::print_status()
 {
 	PX4_INFO("Output: MAVLink gimbal protocol v1");
diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h
index 89eb4a67c4..d44f48b3e9 100644
--- a/src/modules/vmount/output_mavlink.h
+++ b/src/modules/vmount/output_mavlink.h
@@ -45,6 +45,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/gimbal_device_set_attitude.h>
 #include <uORB/topics/gimbal_device_information.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
 
 
 namespace vmount
@@ -64,7 +65,9 @@ public:
 	virtual void print_status();
 
 private:
+	void _stream_device_attitude_status();
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+	uORB::Publication <gimbal_device_attitude_status_s>	_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
 };
 
 class OutputMavlinkV2 : public OutputBase