Simulation: add gimbal simulation for gz-sim

ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal

@@ -0,0 +1,23 @@
+#!/bin/sh
+#
+# @name Gazebo x500 gimbal
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_gimbal}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+# Gimbal settings
+param set-default MNT_MODE_IN 4
+param set-default MNT_MODE_OUT 2
+param set-default MNT_RC_IN_MODE 1
+
+param set-default MNT_MAN_ROLL 1
+param set-default MNT_MAN_PITCH 2
+param set-default MNT_MAN_YAW 3
+
+param set-default MNT_RANGE_ROLL 180
+param set-default MNT_RANGE_PITCH 180
+param set-default MNT_RANGE_YAW 720

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -91,6 +91,7 @@ px4_add_romfs_files(
 	4016_gz_x500_lidar_down
 	4017_gz_x500_lidar_front
 	4018_gz_quadtailsitter
+	4019_gz_x500_gimbal
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

msg/GimbalDeviceAttitudeStatus.msg

@@ -18,3 +18,4 @@ float32 angular_velocity_z
 uint32 failure_flags
 
 bool received_from_mavlink
+uint8 gimbal_device_id

src/modules/gimbal/input_mavlink.cpp

@@ -473,28 +473,50 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont
 
 void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data)
 {
-	// TODO: Take gimbal_device_information into account.
+	gimbal_device_information_s gimbal_device_info;
+	if (_gimbal_device_information_sub.update(&gimbal_device_info) && _parameters.mnt_mode_out == 2)
+	{
+		gimbal_manager_information_s gimbal_manager_info;
+		gimbal_manager_info.timestamp = hrt_absolute_time();
 
-	gimbal_manager_information_s gimbal_manager_info;
-	gimbal_manager_info.timestamp = hrt_absolute_time();
+		gimbal_manager_info.cap_flags = gimbal_device_info.cap_flags;
 
-	gimbal_manager_info.cap_flags =
-		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
-		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK |
-		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
-		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK |
-		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
-		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK |
-		gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
+		gimbal_manager_info.roll_max = gimbal_device_info.roll_max;
+		gimbal_manager_info.roll_min = gimbal_device_info.roll_min;
+		gimbal_manager_info.pitch_max = gimbal_device_info.pitch_max;
+		gimbal_manager_info.pitch_min = gimbal_device_info.pitch_min;
+		gimbal_manager_info.yaw_max = gimbal_device_info.yaw_max;
+		gimbal_manager_info.yaw_min = gimbal_device_info.yaw_min;
 
-	gimbal_manager_info.pitch_max = M_PI_F / 2;
-	gimbal_manager_info.pitch_min = -M_PI_F / 2;
-	gimbal_manager_info.yaw_max = M_PI_F;
-	gimbal_manager_info.yaw_min = -M_PI_F;
+		gimbal_manager_info.gimbal_device_id = control_data.device_compid;
 
-	gimbal_manager_info.gimbal_device_id = control_data.device_compid;
+		_gimbal_manager_info_pub.publish(gimbal_manager_info);
+	}
+	else if (_parameters.mnt_mode_out == 0)
+	{
+		// since we have a non-Mavlink gimbal device, the gimbal manager itself has to act as the gimbal device
+		gimbal_manager_information_s gimbal_manager_info;
+		gimbal_manager_info.timestamp = hrt_absolute_time();
+
+		gimbal_manager_info.cap_flags =
+			gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
+			gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK |
+			gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
+			gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK |
+			gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
+			gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK |
+			gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
+
+		gimbal_manager_info.pitch_max = _parameters.mnt_range_pitch;
+		gimbal_manager_info.pitch_min = -_parameters.mnt_range_pitch;
+		gimbal_manager_info.yaw_max = _parameters.mnt_range_yaw;
+		gimbal_manager_info.yaw_min = -_parameters.mnt_range_yaw;
+
+		gimbal_manager_info.gimbal_device_id = control_data.device_compid;
+
+		_gimbal_manager_info_pub.publish(gimbal_manager_info);
+	}
 
-	_gimbal_manager_info_pub.publish(gimbal_manager_info);
 }
 
 InputMavlinkGimbalV2::UpdateResult

src/modules/gimbal/input_mavlink.h

@@ -41,6 +41,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/gimbal_device_attitude_status.h>
+#include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/gimbal_manager_information.h>
 #include <uORB/topics/gimbal_manager_status.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
@@ -125,6 +126,7 @@ class InputMavlinkGimbalV2 : public InputBase
 	int _vehicle_command_sub = -1;
 
 	uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
+	uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
 	uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
 	uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
 	uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;

src/modules/gimbal/input_rc.cpp

@@ -130,9 +130,11 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData
 
 		if (_parameters.mnt_rc_in_mode == 0) {
 			// We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees.
-			matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f),
+			// We use 179.99 instead of 180 so to avoid that the conversion between quaternions and Euler representation
+			// (when new_aux_value = 1) gives the equivalent angle (e.g., -180 instead of 180).
+			matrix::Eulerf euler(new_aux_values[0] * math::radians(179.99f),
 					     new_aux_values[1] * math::radians(90.f),
-					     new_aux_values[2] * math::radians(180.f));
+					     new_aux_values[2] * math::radians(179.99f));
 
 			matrix::Quatf q(euler);
 			q.copyTo(control_data.type_data.angle.q);

src/modules/gimbal/output.cpp

@@ -258,12 +258,16 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 			_angle_outputs[i] -= euler_vehicle(i);
 		}
 
-		if (PX4_ISFINITE(_angle_outputs[i])) {
-			// bring angles into proper range [-pi, pi]
+		if (PX4_ISFINITE(_angle_outputs[i]) && _parameters.mnt_rc_in_mode == 0) {
+			// if we are in angle input mode, we bring angles into proper range [-pi, pi]
 			_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]);
 		}
 	}
 
+	// constrain angle outputs to [-range/2, range/2]
+	_angle_outputs[0] = math::constrain(_angle_outputs[0], math::radians(-_parameters.mnt_range_roll/2), math::radians(_parameters.mnt_range_roll/2));
+	_angle_outputs[1] = math::constrain(_angle_outputs[1], math::radians(-_parameters.mnt_range_pitch/2), math::radians(_parameters.mnt_range_pitch/2));
+	_angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw/2), math::radians(_parameters.mnt_range_yaw/2));
 
 	// constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed
 	if (_landed) {

src/modules/gimbal/output_rc.cpp

@@ -60,7 +60,7 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8
 
 	_calculate_angle_output(now);
 
-	_stream_device_attitude_status();
+	_stream_device_attitude_status(control_data);
 
 	// If the output is RC, then we signal this by referring to compid 1.
 	gimbal_device_id = 1;
@@ -90,7 +90,7 @@ void OutputRC::print_status() const
 	PX4_INFO("Output: AUX");
 }
 
-void OutputRC::_stream_device_attitude_status()
+void OutputRC::_stream_device_attitude_status(const ControlData &control_data)
 {
 	gimbal_device_attitude_status_s attitude_status{};
 	attitude_status.timestamp = hrt_absolute_time();
@@ -115,6 +115,7 @@ void OutputRC::_stream_device_attitude_status()
 	q.copyTo(attitude_status.q);
 
 	attitude_status.failure_flags = 0;
+	attitude_status.gimbal_device_id = control_data.device_compid;
 	_attitude_status_pub.publish(attitude_status);
 }
 

src/modules/gimbal/output_rc.h

@@ -54,7 +54,7 @@ class OutputRC : public OutputBase
 	virtual void print_status() const;
 
 private:
-	void _stream_device_attitude_status();
+	void _stream_device_attitude_status(const ControlData &control_data);
 
 	uORB::Publication <gimbal_controls_s>	_gimbal_controls_pub{ORB_ID(gimbal_controls)};
 	uORB::Publication <gimbal_device_attitude_status_s>	_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};

src/modules/mavlink/mavlink_receiver.cpp

@@ -3063,7 +3063,14 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
 	gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
 	gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
 
-	gimbal_information.gimbal_device_compid = msg->compid;
+	if (gimbal_device_info_msg.gimbal_device_id == 0)
+	{
+		gimbal_information.gimbal_device_compid = msg->compid;
+	}
+	else
+	{
+		gimbal_information.gimbal_device_compid = gimbal_device_info_msg.gimbal_device_id;
+	}
 
 	_gimbal_device_information_pub.publish(gimbal_information);
 }
@@ -3090,6 +3097,7 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 	gimbal_attitude_status.failure_flags = gimbal_device_attitude_status_msg.failure_flags;
 
 	gimbal_attitude_status.received_from_mavlink = true;
+	gimbal_attitude_status.gimbal_device_id = gimbal_device_attitude_status_msg.gimbal_device_id;
 
 	_gimbal_device_attitude_status_pub.publish(gimbal_attitude_status);
 }

src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp

@@ -74,27 +74,43 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream
 				return false;
 			}
 
-			mavlink_gimbal_device_attitude_status_t msg{};
+			if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6)
+			{
+				// A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id
+				mavlink_gimbal_device_attitude_status_t msg{};
 
-			msg.target_system = gimbal_device_attitude_status.target_system;
-			msg.target_component = gimbal_device_attitude_status.target_component;
+				msg.target_system = gimbal_device_attitude_status.target_system;
+				msg.target_component = gimbal_device_attitude_status.target_component;
 
-			msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
+				msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
 
-			msg.flags = gimbal_device_attitude_status.device_flags;
+				msg.flags = gimbal_device_attitude_status.device_flags;
 
-			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.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.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.failure_flags = gimbal_device_attitude_status.failure_flags;
+				msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
 
-			mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
+				mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);
+			}
+			else
+			{
+				// We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID
+				mavlink_message_t message;
+				mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL, _mavlink->get_channel(), &message,
+						gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component, gimbal_device_attitude_status.timestamp / 1000,
+						gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q, gimbal_device_attitude_status.angular_velocity_x,
+						gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z, gimbal_device_attitude_status.failure_flags,
+						0, 0, 0);
+				_mavlink->forward_message(&message, _mavlink);
+			}
 
 			return true;
 		}

src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp

@@ -83,6 +83,7 @@ class MavlinkStreamGimbalDeviceInformation : public MavlinkStream
 			msg.pitch_max = gimbal_device_information.pitch_max;
 			msg.yaw_min = gimbal_device_information.yaw_min;
 			msg.yaw_max = gimbal_device_information.yaw_max;
+			msg.gimbal_device_id = gimbal_device_information.gimbal_device_compid;
 
 			mavlink_msg_gimbal_device_information_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -61,6 +61,8 @@ if(gz-transport_FOUND)
 			GZMixingInterfaceServo.hpp
 			GZMixingInterfaceWheel.cpp
 			GZMixingInterfaceWheel.hpp
+			GZGimbal.cpp
+			GZGimbal.hpp
 		DEPENDS
 			mixer_module
 			px4_work_queue

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -268,6 +268,11 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	if (!_gimbal.init(_world_name, _model_name)) {
+		PX4_ERR("failed to init gimbal");
+		return PX4_ERROR;
+	}
+
 	ScheduleNow();
 	return OK;
 }
@@ -1005,6 +1010,7 @@ void GZBridge::Run()
 		_mixing_interface_esc.stop();
 		_mixing_interface_servo.stop();
 		_mixing_interface_wheel.stop();
+		_gimbal.stop();
 
 		exit_and_cleanup();
 		return;
@@ -1021,6 +1027,7 @@ void GZBridge::Run()
 		_mixing_interface_esc.updateParams();
 		_mixing_interface_servo.updateParams();
 		_mixing_interface_wheel.updateParams();
+		_gimbal.updateParams();
 	}
 
 	ScheduleDelayed(10_ms);

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -36,6 +36,7 @@
 #include "GZMixingInterfaceESC.hpp"
 #include "GZMixingInterfaceServo.hpp"
 #include "GZMixingInterfaceWheel.hpp"
+#include "GZGimbal.hpp"
 
 #include <px4_platform_common/atomic.h>
 #include <px4_platform_common/defines.h>
@@ -184,6 +185,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	GZMixingInterfaceESC   _mixing_interface_esc{_node, _node_mutex};
 	GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
 	GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex};
+	GZGimbal _gimbal{_node, _node_mutex};
 
 	px4::atomic<uint64_t> _world_time_us{0};