Use gimbal attitude for the camera feedback when available

The CameraFeedback module used only the vehicle attitude for the camera orientation so far. With this change, the gimbal_device_attitude_status is used to compute the global camera orientation when a gimbal is used.

msg/CameraCapture.msg

@@ -5,5 +5,5 @@ float64 lat					# Latitude in degrees (WGS84)
 float64 lon					# Longitude in degrees (WGS84)
 float32 alt					# Altitude (AMSL)
 float32 ground_distance			# Altitude above ground (meters)
-float32[4] q					# Attitude of the camera, zero rotation is facing towards front of vehicle
+float32[4] q					# Attitude of the camera relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude
 int8 result					# 1 for success, 0 for failure, -1 if camera does not provide feedback

src/modules/camera_feedback/CameraFeedback.cpp

@@ -33,6 +33,8 @@
 
 #include "CameraFeedback.hpp"
 
+using namespace time_literals;
+
 CameraFeedback::CameraFeedback() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -112,11 +114,39 @@ CameraFeedback::Run()
 		}
 
 		// Fill attitude data
-		// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
+		gimbal_device_attitude_status_s gimbal{};
+
+		if (_gimbal_sub.copy(&gimbal) && (hrt_elapsed_time(&gimbal.timestamp) < 1_s)) {
+			if (gimbal.device_flags & gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK) {
+				// Gimbal yaw angle is absolute angle relative to North
+				capture.q[0] = gimbal.q[0];
+				capture.q[1] = gimbal.q[1];
+				capture.q[2] = gimbal.q[2];
+				capture.q[3] = gimbal.q[3];
+
+			} else {
+				// Gimbal quaternion frame is in the Earth frame rotated so that the x-axis is pointing
+				// forward (yaw relative to vehicle). Get heading from the vehicle attitude and combine it
+				// with the gimbal orientation.
+				const matrix::Eulerf euler_vehicle(matrix::Quatf(att.q));
+				const matrix::Quatf q_heading(matrix::Eulerf(0.0f, 0.0f, euler_vehicle(2)));
+				matrix::Quatf q_gimbal(gimbal.q);
+				q_gimbal = q_heading * q_gimbal;
+
+				capture.q[0] = q_gimbal(0);
+				capture.q[1] = q_gimbal(1);
+				capture.q[2] = q_gimbal(2);
+				capture.q[3] = q_gimbal(3);
+			}
+
+		} else {
+			// No gimbal orientation, use vehicle attitude
 			capture.q[0] = att.q[0];
 			capture.q[1] = att.q[1];
 			capture.q[2] = att.q[2];
 			capture.q[3] = att.q[3];
+		}
+
 		capture.result = 1;
 
 		_capture_pub.publish(capture);

src/modules/camera_feedback/CameraFeedback.hpp

@@ -55,6 +55,7 @@
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
 
 class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
 {
@@ -81,6 +82,7 @@ private:
 
 	uORB::Subscription	_gpos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription	_att_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription	_gimbal_sub{ORB_ID(gimbal_device_attitude_status)};
 
 	uORB::Publication<camera_capture_s>	_capture_pub{ORB_ID(camera_capture)};
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -3128,7 +3128,7 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 	mavlink_msg_gimbal_device_attitude_status_decode(msg, &gimbal_device_attitude_status_msg);
 
 	gimbal_device_attitude_status_s gimbal_attitude_status{};
-	gimbal_attitude_status.timestamp = static_cast<uint64_t>(gimbal_device_attitude_status_msg.time_boot_ms) * 1000;
+	gimbal_attitude_status.timestamp = hrt_absolute_time();
 	gimbal_attitude_status.target_system = gimbal_device_attitude_status_msg.target_system;
 	gimbal_attitude_status.target_component = gimbal_device_attitude_status_msg.target_component;
 	gimbal_attitude_status.device_flags = gimbal_device_attitude_status_msg.flags;