Gimbal: add pitch stabilization and asymmetrical pitch ranges

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

@@ -19,5 +19,6 @@ 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_MAX_PITCH 45
+param set-default MNT_MIN_PITCH -135
 param set-default MNT_RANGE_YAW 720

boards/px4/fmu-v5/default.px4board

@@ -77,8 +77,6 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_UUV_ATT_CONTROL=y
-CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y

msg/GimbalControls.msg

@@ -4,4 +4,4 @@ uint8 INDEX_PITCH = 1
 uint8 INDEX_YAW = 2
 
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[3] control
+float32[3] control	# Normalized output. 1 means maximum positive position. -1 maximum negative position. 0 means no deflection. NaN maps to disarmed.

src/lib/parameters/param_translation.cpp

@@ -163,5 +163,19 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2025-11-17: translate MNT_RANGE_PITCH to MNT_MAX_PITCH, MNT_MIN_PITCH
+	{
+		if (strcmp("MNT_RANGE_PITCH", node->name) == 0) {
+			if (node->d > DBL_EPSILON) {
+				float mnt_max_pitch = static_cast<float>(node->d) * 0.5f;
+				float mnt_min_pitch = static_cast<float>(-node->d) * 0.5f;
+				param_set(param_find("MNT_MAX_PITCH"), &mnt_max_pitch);
+				param_set(param_find("MNT_MIN_PITCH"), &mnt_min_pitch);
+				PX4_INFO("migrating %s -> %s, %s", "MNT_RANGE_PITCH", "MNT_MAX_PITCH", "MNT_MIN_PITCH");
+			}
+
+		}
+	}
+
 	return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
 }

src/modules/gimbal/common.h

@@ -86,5 +86,12 @@ struct ControlData {
 	uint64_t timestamp_last_update{0}; // Timestamp when there was the last setpoint set by the input used for timeout
 };
 
+// Must match paraneter MNT_DO_STAB
+enum MntDoStabilize {
+	DISABLED = 0,
+	ALL_AXES, 			// Stabilize all axis
+	YAW_LOCK, 			// Stabilize yaw for absolute/lock mode.
+	PITCH_LOCK, 			// Stabilize pitch for absolute/lock mode.
+};
 
 } /* namespace gimbal */

src/modules/gimbal/gimbal.cpp

@@ -254,14 +254,26 @@ static int gimbal_thread_main(int argc, char *argv[])
 				}
 			}
 
-			if (params.mnt_do_stab == 1) {
-				thread_data.output_obj->set_stabilize(true, true, true);
+			switch (params.mnt_do_stab) {
+			case MntDoStabilize::ALL_AXES: {
+					thread_data.output_obj->set_stabilize(true, true, true);
+					break;
+				}
 
-			} else if (params.mnt_do_stab == 2) {
-				thread_data.output_obj->set_stabilize(false, false, true);
+			case MntDoStabilize::YAW_LOCK: {
+					thread_data.output_obj->set_stabilize(false, false, true);
+					break;
+				}
 
-			} else {
-				thread_data.output_obj->set_stabilize(false, false, false);
+			case MntDoStabilize::PITCH_LOCK: {
+					thread_data.output_obj->set_stabilize(false, true, false);
+					break;
+				}
+
+			default: {
+					thread_data.output_obj->set_stabilize(false, false, false);
+					break;
+				}
 			}
 
 			if (thread_data.output_obj->check_and_handle_setpoint_timeout(thread_data.control_data, hrt_absolute_time())) {
@@ -533,19 +545,18 @@ void update_params(ParameterHandles &param_handles, Parameters &params)
 	param_get(param_handles.mnt_man_roll, &params.mnt_man_roll);
 	param_get(param_handles.mnt_man_yaw, &params.mnt_man_yaw);
 	param_get(param_handles.mnt_do_stab, &params.mnt_do_stab);
-	param_get(param_handles.mnt_range_pitch, &params.mnt_range_pitch);
+	param_get(param_handles.mnt_max_pitch, &params.mnt_max_pitch);
+	param_get(param_handles.mnt_min_pitch, &params.mnt_min_pitch);
 	param_get(param_handles.mnt_range_roll, &params.mnt_range_roll);
 	param_get(param_handles.mnt_range_yaw, &params.mnt_range_yaw);
-	param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
-	param_get(param_handles.mnt_off_roll, &params.mnt_off_roll);
-	param_get(param_handles.mnt_off_yaw, &params.mnt_off_yaw);
 	param_get(param_handles.mav_sysid, &params.mav_sysid);
 	param_get(param_handles.mav_compid, &params.mav_compid);
 	param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
 	param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
 	param_get(param_handles.mnt_rc_in_mode, &params.mnt_rc_in_mode);
 	param_get(param_handles.mnt_lnd_p_min, &params.mnt_lnd_p_min);
 	param_get(param_handles.mnt_lnd_p_max, &params.mnt_lnd_p_max);
+	param_get(param_handles.mnt_tau, &params.mnt_tau);
 }
 
 bool initialize_params(ParameterHandles &param_handles, Parameters &params)
@@ -558,19 +569,18 @@ bool initialize_params(ParameterHandles &param_handles, Parameters &params)
 	param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL");
 	param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW");
 	param_handles.mnt_do_stab = param_find("MNT_DO_STAB");
-	param_handles.mnt_range_pitch = param_find("MNT_RANGE_PITCH");
+	param_handles.mnt_max_pitch = param_find("MNT_MAX_PITCH");
+	param_handles.mnt_min_pitch = param_find("MNT_MIN_PITCH");
 	param_handles.mnt_range_roll = param_find("MNT_RANGE_ROLL");
 	param_handles.mnt_range_yaw = param_find("MNT_RANGE_YAW");
-	param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH");
-	param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL");
-	param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW");
 	param_handles.mav_sysid = param_find("MAV_SYS_ID");
 	param_handles.mav_compid = param_find("MAV_COMP_ID");
 	param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH");
 	param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW");
 	param_handles.mnt_rc_in_mode = param_find("MNT_RC_IN_MODE");
 	param_handles.mnt_lnd_p_min = param_find("MNT_LND_P_MIN");
 	param_handles.mnt_lnd_p_max = param_find("MNT_LND_P_MAX");
+	param_handles.mnt_tau = param_find("MNT_TAU");
 
 	if (param_handles.mnt_mode_in == PARAM_INVALID ||
 	    param_handles.mnt_mode_out == PARAM_INVALID ||
@@ -580,19 +590,18 @@ bool initialize_params(ParameterHandles &param_handles, Parameters &params)
 	    param_handles.mnt_man_roll == PARAM_INVALID ||
 	    param_handles.mnt_man_yaw == PARAM_INVALID ||
 	    param_handles.mnt_do_stab == PARAM_INVALID ||
-	    param_handles.mnt_range_pitch == PARAM_INVALID ||
+	    param_handles.mnt_max_pitch == PARAM_INVALID ||
+	    param_handles.mnt_min_pitch == PARAM_INVALID ||
 	    param_handles.mnt_range_roll == PARAM_INVALID ||
 	    param_handles.mnt_range_yaw == PARAM_INVALID ||
-	    param_handles.mnt_off_pitch == PARAM_INVALID ||
-	    param_handles.mnt_off_roll == PARAM_INVALID ||
-	    param_handles.mnt_off_yaw == PARAM_INVALID ||
 	    param_handles.mav_sysid == PARAM_INVALID ||
 	    param_handles.mav_compid == PARAM_INVALID ||
 	    param_handles.mnt_rate_pitch == PARAM_INVALID ||
 	    param_handles.mnt_rate_yaw == PARAM_INVALID ||
 	    param_handles.mnt_rc_in_mode == PARAM_INVALID ||
 	    param_handles.mnt_lnd_p_min == PARAM_INVALID ||
-	    param_handles.mnt_lnd_p_max == PARAM_INVALID
+	    param_handles.mnt_lnd_p_max == PARAM_INVALID ||
+	    param_handles.mnt_tau == PARAM_INVALID
 	   ) {
 		return false;
 	}

src/modules/gimbal/gimbal_params.c

@@ -146,77 +146,59 @@ PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);
 * @value 0 Disable
 * @value 1 Stabilize all axis
 * @value 2 Stabilize yaw for absolute/lock mode.
-* @min 0
-* @max 2
+* @value 3 Stabilize pitch for absolute/lock mode.
 * @group Mount
 */
 PARAM_DEFINE_INT32(MNT_DO_STAB, 0);
 
 /**
-* Range of pitch channel output in degrees (only in AUX output mode).
+* Max positive angle of pitch setpoint (only in MNT_MODE_OUT=AUX).
+*
+* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX).
 *
-* @min 1.0
-* @max 720.0
 * @unit deg
 * @decimal 1
 * @group Mount
 */
-PARAM_DEFINE_FLOAT(MNT_RANGE_PITCH, 90.0f);
+PARAM_DEFINE_FLOAT(MNT_MAX_PITCH, 45.0f);
 
 /**
-* Range of roll channel output in degrees (only in AUX output mode).
+* Min negative angle of pitch setpoint (only in MNT_MODE_OUT=AUX).
+*
+* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX).
 *
-* @min 1.0
-* @max 720.0
 * @unit deg
 * @decimal 1
 * @group Mount
 */
-PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 90.0f);
+PARAM_DEFINE_FLOAT(MNT_MIN_PITCH, -45.0f);
 
 /**
-* Range of yaw channel output in degrees (only in AUX output mode).
+* Range of roll channel output (only in MNT_MODE_OUT=AUX).
+*
+* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX). Note that only symmetric angular ranges are supported.
 *
 * @min 1.0
 * @max 720.0
 * @unit deg
 * @decimal 1
 * @group Mount
 */
-PARAM_DEFINE_FLOAT(MNT_RANGE_YAW, 360.0f);
+PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 90.0f);
 
 /**
-* Offset for pitch channel output in degrees.
+* Range of yaw channel output (only in MNT_MODE_OUT=AUX).
 *
-* @min -360.0
-* @max 360.0
-* @unit deg
-* @decimal 1
-* @group Mount
-*/
-PARAM_DEFINE_FLOAT(MNT_OFF_PITCH, 0.0f);
-
-/**
-* Offset for roll channel output in degrees.
+* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX). Note that only symmetric angular ranges are supported.
 *
-* @min -360.0
-* @max 360.0
+* @min 1.0
+* @max 720.0
 * @unit deg
 * @decimal 1
 * @group Mount
 */
-PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f);
+PARAM_DEFINE_FLOAT(MNT_RANGE_YAW, 360.0f);
 
-/**
-* Offset for yaw channel output in degrees.
-*
-* @min -360.0
-* @max 360.0
-* @unit deg
-* @decimal 1
-* @group Mount
-*/
-PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f);
 
 /**
  * Angular pitch rate for manual input in degrees/second.
@@ -242,6 +224,19 @@ PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f);
  */
 PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);
 
+/**
+ * Alpha filter time constant defining the convergence rate (in seconds) for open-loop AUX mount control.
+ *
+ * Use when no angular position feedback is available.
+ * With MNT_MODE_OUT set to AUX, the mount operates in open-loop and directly commands the servo output.
+ * Parameters must be tuned for the specific servo to approximate its speed and response.
+ *
+ * @min 0.0
+ * @group Mount
+ */
+PARAM_DEFINE_FLOAT(MNT_TAU, 0.3f);
+
+
 /**
  * Input mode for RC gimbal input
  *

src/modules/gimbal/gimbal_params.h

@@ -64,19 +64,18 @@ struct Parameters {
 	int32_t mnt_man_roll;
 	int32_t mnt_man_yaw;
 	int32_t mnt_do_stab;
-	float mnt_range_pitch;
+	float mnt_max_pitch;
+	float mnt_min_pitch;
 	float mnt_range_roll;
 	float mnt_range_yaw;
-	float mnt_off_pitch;
-	float mnt_off_roll;
-	float mnt_off_yaw;
 	int32_t mav_sysid;
 	int32_t mav_compid;
 	float mnt_rate_pitch;
 	float mnt_rate_yaw;
 	int32_t mnt_rc_in_mode;
 	float mnt_lnd_p_min;
 	float mnt_lnd_p_max;
+	float mnt_tau;
 };
 
 struct ParameterHandles {
@@ -88,19 +87,18 @@ struct ParameterHandles {
 	param_t mnt_man_roll;
 	param_t mnt_man_yaw;
 	param_t mnt_do_stab;
-	param_t mnt_range_pitch;
+	param_t mnt_max_pitch;
+	param_t mnt_min_pitch;
 	param_t mnt_range_roll;
 	param_t mnt_range_yaw;
-	param_t mnt_off_pitch;
-	param_t mnt_off_roll;
-	param_t mnt_off_yaw;
 	param_t mav_sysid;
 	param_t mav_compid;
 	param_t mnt_rate_pitch;
 	param_t mnt_rate_yaw;
 	param_t mnt_rc_in_mode;
 	param_t mnt_lnd_p_min;
 	param_t mnt_lnd_p_max;
+	param_t mnt_tau;
 };
 
 } /* namespace gimbal */

src/modules/gimbal/input_mavlink.cpp

@@ -508,10 +508,12 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData
 			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.pitch_max = math::radians(_parameters.mnt_max_pitch);
+		gimbal_manager_info.pitch_min = math::radians(_parameters.mnt_min_pitch);
+		gimbal_manager_info.yaw_max = math::radians(_parameters.mnt_range_yaw * 0.5f);
+		gimbal_manager_info.yaw_min = math::radians(-_parameters.mnt_range_yaw * 0.5f);
+		gimbal_manager_info.roll_max = math::radians(_parameters.mnt_range_roll * 0.5f);
+		gimbal_manager_info.roll_min = math::radians(-_parameters.mnt_range_roll * 0.5f);
 
 		gimbal_manager_info.gimbal_device_id = control_data.device_compid;
 

src/modules/gimbal/input_rc.cpp

@@ -141,7 +141,9 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData
 
 			control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
 			control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
-			control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+			control_data.type_data.angle.frames[2] = (_parameters.mnt_do_stab == MntDoStabilize::ALL_AXES
+					|| _parameters.mnt_do_stab == MntDoStabilize::YAW_LOCK) ?
+					ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 
 			control_data.type_data.angle.angular_velocity[0] = NAN;
 			control_data.type_data.angle.angular_velocity[1] = NAN;