GZ: add gimbal simulation (#23382)

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

src/modules/gimbal/gimbal.cpp

@@ -113,7 +113,7 @@ static int gimbal_thread_main(int argc, char *argv[])
 	thread_data.input_objs[thread_data.input_objs_len++] = thread_data.test_input;
 
 	switch (params.mnt_mode_in) {
-	case 0:
+	case MNT_MODE_IN_AUTO:
 		// Automatic
 		// MAVLINK_V2 as well as RC input are supported together.
 		// Whichever signal is updated last, gets control, for RC there is a deadzone
@@ -123,19 +123,19 @@ static int gimbal_thread_main(int argc, char *argv[])
 		thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
 		break;
 
-	case 1: // RC only
+	case MNT_MODE_IN_RC: // RC only
 		thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
 		break;
 
-	case 2: // MAVLINK_ROI commands only (to be deprecated)
+	case MNT_MODE_IN_MAVLINK_ROI: // MAVLINK_ROI commands only (to be deprecated)
 		thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkROI(params);
 		break;
 
-	case 3: // MAVLINK_DO_MOUNT commands only (to be deprecated)
+	case MNT_MODE_IN_MAVLINK_DO_MOUNT: // MAVLINK_DO_MOUNT commands only (to be deprecated)
 		thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkCmdMount(params);
 		break;
 
-	case 4: //MAVLINK_V2
+	case MNT_MODE_IN_MAVLINK_V2: //MAVLINK_V2
 		thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
 		break;
 
@@ -165,21 +165,21 @@ static int gimbal_thread_main(int argc, char *argv[])
 	}
 
 	switch (params.mnt_mode_out) {
-	case 0: //AUX
+	case MNT_MODE_OUT_AUX: //AUX
 		thread_data.output_obj = new OutputRC(params);
 
 		if (!thread_data.output_obj) { alloc_failed = true; }
 
 		break;
 
-	case 1: //MAVLink gimbal v1 protocol
+	case MNT_MODE_OUT_MAVLINK_V1: //MAVLink gimbal v1 protocol
 		thread_data.output_obj = new OutputMavlinkV1(params);
 
 		if (!thread_data.output_obj) { alloc_failed = true; }
 
 		break;
 
-	case 2: //MAVLink gimbal v2 protocol
+	case MNT_MODE_OUT_MAVLINK_V2: //MAVLink gimbal v2 protocol
 		thread_data.output_obj = new OutputMavlinkV2(params);
 
 		if (!thread_data.output_obj) { alloc_failed = true; }
@@ -276,7 +276,7 @@ static int gimbal_thread_main(int argc, char *argv[])
 
 			// 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
+			if (params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V1 && params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V2) {
 				thread_data.output_obj->publish();
 			}
 

src/modules/gimbal/gimbal_params.h

@@ -40,6 +40,21 @@
 namespace gimbal
 {
 
+enum MntModeIn {
+	MNT_MODE_IN_DISABLED = -1,
+	MNT_MODE_IN_AUTO,				// RC and MAVLink gimbal protocol v2
+	MNT_MODE_IN_RC,
+	MNT_MODE_IN_MAVLINK_ROI,			// MAVLink gimbal protocol v1 (to be deprecated)
+	MNT_MODE_IN_MAVLINK_DO_MOUNT,		// MAVLink gimbal protocol v1 (to be deprecated)
+	MNT_MODE_IN_MAVLINK_V2			// MAVLink gimbal protocol v2
+};
+
+enum MntModeOut {
+	MNT_MODE_OUT_AUX = 0,
+	MNT_MODE_OUT_MAVLINK_V1, 			// MAVLink gimbal protocol v1 (to be deprecated)
+	MNT_MODE_OUT_MAVLINK_V2 			// MAVLink gimbal protocol v2
+};
+
 struct Parameters {
 	int32_t mnt_mode_in;
 	int32_t mnt_mode_out;

src/modules/gimbal/input_mavlink.cpp

@@ -473,28 +473,49 @@ 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;
 
-	gimbal_manager_information_s gimbal_manager_info;
-	gimbal_manager_info.timestamp = hrt_absolute_time();
+	if (_gimbal_device_information_sub.update(&gimbal_device_info) && _parameters.mnt_mode_out == MNT_MODE_OUT_MAVLINK_V2) {
+		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.cap_flags = gimbal_device_info.cap_flags;
 
-	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.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.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 == MNT_MODE_OUT_AUX) {
+		// 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,19 @@ 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/mavlink/mavlink_receiver.cpp

@@ -3063,7 +3063,12 @@ 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_id = msg->compid;
+	if (gimbal_device_info_msg.gimbal_device_id == 0) {
+		gimbal_information.gimbal_device_id = msg->compid;
+
+	} else {
+		gimbal_information.gimbal_device_id = gimbal_device_info_msg.gimbal_device_id;
+	}
 
 	_gimbal_device_information_pub.publish(gimbal_information);
 }
@@ -3090,6 +3095,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,32 +74,48 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream
 				return false;
 			}
 
-			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.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
-
-			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.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.delta_yaw = gimbal_device_attitude_status.delta_yaw;
-			msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;
-
-			msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
-
-			mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &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.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000;
+
+				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.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.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;
+
+				msg.delta_yaw = gimbal_device_attitude_status.delta_yaw;
+				msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;
+
+				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_id;
 
 			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);