diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal b/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal index 80719ae3d313..9dcd642c2c31 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal +++ b/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 diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 9a36ff1fe730..3d0a9b0efcb9 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/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 diff --git a/msg/GimbalControls.msg b/msg/GimbalControls.msg index 3e1c5a9ddacf..3546a4a0af2c 100644 --- a/msg/GimbalControls.msg +++ b/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. diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 3a8d63bbf643..837a2d68ba08 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/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(node->d) * 0.5f; + float mnt_min_pitch = static_cast(-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; } diff --git a/src/modules/gimbal/common.h b/src/modules/gimbal/common.h index 2436bd68dda8..fc528a305de9 100644 --- a/src/modules/gimbal/common.h +++ b/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 */ diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 9179055b31ca..85c033888852 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/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,12 +545,10 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_get(param_handles.mnt_man_roll, ¶ms.mnt_man_roll); param_get(param_handles.mnt_man_yaw, ¶ms.mnt_man_yaw); param_get(param_handles.mnt_do_stab, ¶ms.mnt_do_stab); - param_get(param_handles.mnt_range_pitch, ¶ms.mnt_range_pitch); + param_get(param_handles.mnt_max_pitch, ¶ms.mnt_max_pitch); + param_get(param_handles.mnt_min_pitch, ¶ms.mnt_min_pitch); param_get(param_handles.mnt_range_roll, ¶ms.mnt_range_roll); param_get(param_handles.mnt_range_yaw, ¶ms.mnt_range_yaw); - param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch); - param_get(param_handles.mnt_off_roll, ¶ms.mnt_off_roll); - param_get(param_handles.mnt_off_yaw, ¶ms.mnt_off_yaw); param_get(param_handles.mav_sysid, ¶ms.mav_sysid); param_get(param_handles.mav_compid, ¶ms.mav_compid); param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); @@ -546,6 +556,7 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_get(param_handles.mnt_rc_in_mode, ¶ms.mnt_rc_in_mode); param_get(param_handles.mnt_lnd_p_min, ¶ms.mnt_lnd_p_min); param_get(param_handles.mnt_lnd_p_max, ¶ms.mnt_lnd_p_max); + param_get(param_handles.mnt_tau, ¶ms.mnt_tau); } bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) @@ -558,12 +569,10 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) 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"); @@ -571,6 +580,7 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) 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 ¶m_handles, Parameters ¶ms) 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; } diff --git a/src/modules/gimbal/gimbal_params.c b/src/modules/gimbal/gimbal_params.c index a6b1889e2f4f..7a98214ced02 100644 --- a/src/modules/gimbal/gimbal_params.c +++ b/src/modules/gimbal/gimbal_params.c @@ -146,36 +146,37 @@ 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 @@ -183,40 +184,21 @@ PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 90.0f); * @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 * diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index 5a265d02373f..e480dc41d27d 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/src/modules/gimbal/gimbal_params.h @@ -64,12 +64,10 @@ 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; @@ -77,6 +75,7 @@ struct Parameters { int32_t mnt_rc_in_mode; float mnt_lnd_p_min; float mnt_lnd_p_max; + float mnt_tau; }; struct ParameterHandles { @@ -88,12 +87,10 @@ 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; @@ -101,6 +98,7 @@ struct ParameterHandles { param_t mnt_rc_in_mode; param_t mnt_lnd_p_min; param_t mnt_lnd_p_max; + param_t mnt_tau; }; } /* namespace gimbal */ diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index aef348ca53d5..de7ea2215527 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/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; diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index 3c2926ffbc30..ab399d10cb56 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/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; diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index ea474bf33ec5..efbbcd78c6ed 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -146,6 +146,9 @@ void OutputBase::_set_angle_setpoints(const ControlData &control_data) _angle_velocity[0] = NAN; _angle_velocity[1] = NAN; _angle_velocity[2] = NAN; + _absolute_angle[0] = false; + _absolute_angle[1] = false; + _absolute_angle[2] = false; break; } } @@ -237,16 +240,19 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) float dt = math::constrain((t - _last_update) * 1.e-6f, 0.001f, 1.f); const matrix::Quatf q_setpoint(_q_setpoint); - const bool q_setpoint_valid = q_setpoint.isAllFinite(); - matrix::Eulerf euler_gimbal{}; - if (q_setpoint_valid) { - euler_gimbal = q_setpoint; + if (q_setpoint.isAllFinite()) { + _last_valid_setpoint = q_setpoint; } + matrix::Eulerf euler_gimbal(_last_valid_setpoint); + for (int i = 0; i < 3; ++i) { - if (q_setpoint_valid && PX4_ISFINITE(euler_gimbal(i))) { + if (PX4_ISFINITE(euler_gimbal(i)) && compensate[i] && PX4_ISFINITE(euler_vehicle(i))) { + _angle_outputs[i] = euler_gimbal(i) - euler_vehicle(i); + + } else if (PX4_ISFINITE(euler_gimbal(i)) && !_absolute_angle[i]) { _angle_outputs[i] = euler_gimbal(i); } @@ -254,10 +260,6 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) _angle_outputs[i] += dt * _angle_velocity[i]; } - if (compensate[i] && PX4_ISFINITE(euler_vehicle(i))) { - _angle_outputs[i] -= euler_vehicle(i); - } - 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]); @@ -267,8 +269,12 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) // 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)); + + // constrain angle outputs to [min, max] to allow for asymmetrical angular ranges + _angle_outputs[1] = math::constrain(_angle_outputs[1], + math::radians(_parameters.mnt_min_pitch), + math::radians(_parameters.mnt_max_pitch)); + // constrain angle outputs to [-range/2, range/2] _angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw / 2), math::radians(_parameters.mnt_range_yaw / 2)); @@ -280,6 +286,16 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) math::radians(_parameters.mnt_lnd_p_max)); } } + + _angle_outputs_filtered.setParameters(dt, _parameters.mnt_tau); + matrix::Vector3f filtered_outputs = _angle_outputs_filtered.update(matrix::Vector3f(_angle_outputs)); + + for (int i = 0; i < 3; i++) { + _angle_outputs[i] = filtered_outputs(i); + } + + set_last_valid_setpoint(compensate, euler_vehicle); + } void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize) @@ -289,4 +305,54 @@ void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool y _stabilize[2] = yaw_stabilize; } +void OutputBase::set_last_valid_setpoint(const bool compensate[3], const matrix::Eulerf euler_vehicle) +{ + // No updates from angular velocity, hence no modification of last valid setpoint + if (!PX4_ISFINITE(_angle_velocity[0]) && !PX4_ISFINITE(_angle_velocity[1]) && !PX4_ISFINITE(_angle_velocity[2])) { + return; + } + + // Angle outputs, from vehicle body frame to end effector (last actuator in the kinematic chain) + _last_valid_setpoint.phi() = _angle_outputs[0]; + _last_valid_setpoint.theta() = _angle_outputs[1]; + _last_valid_setpoint.psi() = _angle_outputs[2]; + + // Take into account vehicle attitude if it should + switch (_parameters.mnt_do_stab) { + + case MntDoStabilize::ALL_AXES: { + + for (int i = 0; i < 3; i++) { + if (compensate[i]) { + _last_valid_setpoint(i) += euler_vehicle(i); + } + } + + break; + } + + case MntDoStabilize::YAW_LOCK: { + if (compensate[2]) { + _last_valid_setpoint.psi() += euler_vehicle(2); + } + + break; + } + + case MntDoStabilize::PITCH_LOCK: { + + if (compensate[1]) { + _last_valid_setpoint.theta() += euler_vehicle(1); + } + + break; + } + + default: { + // Dont add anything + break; + } + } +} + } /* namespace gimbal */ diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index 0da012c35fc7..8836f7a2594b 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -38,6 +38,8 @@ #include "gimbal_params.h" #include #include +#include +#include #include #include #include @@ -92,11 +94,27 @@ class OutputBase // Pitch and role are by default aligned with the horizon. // Yaw follows the vehicle (not lock/absolute mode). bool _absolute_angle[3] = {true, true, false }; + AlphaFilter _angle_outputs_filtered; /** calculate the _angle_outputs (with speed) and stabilize if needed */ void _calculate_angle_output(const hrt_abstime &t); + /** + * The angular velocity setpoint modifies the angle setpoint. + * To correctly track and lock onto the desired orientation, the resulting change in angle + * must be incorporated into the world-frame attitude setpoint. Depending on MNT_DO_STAB and + * the received MAVLink command, the last valid setpoint is updated to account for the vehicle attitude. + * + * @param compensate Boolean per axis (roll, pitch, yaw). If true, the vehicle attitude is taken into account. + * @param euler_vehicle + */ + void set_last_valid_setpoint(const bool compensate[3], const matrix::Eulerf euler_vehicle); + float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad] + + // Quaternion float last valid setpoint. + // Depending on MNT_DO_STAB and mavlink command the setpoint can be relative to vehicle or in the world frame + matrix::Eulerf _last_valid_setpoint; hrt_abstime _last_update; private: diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index ae5222c674de..c3ceaab678c0 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -100,9 +100,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints // gimbal spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively // gimbal uses radians, MAVLink uses degrees - vehicle_command.param1 = math::degrees(_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)); - vehicle_command.param2 = math::degrees(_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)); - vehicle_command.param3 = math::degrees(_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)); + vehicle_command.param1 = math::degrees(_angle_outputs[1]); + vehicle_command.param2 = math::degrees(_angle_outputs[0]); + vehicle_command.param3 = math::degrees(_angle_outputs[2]); vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING; _gimbal_v1_command_pub.publish(vehicle_command); diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index dba7b02b1af9..3bf9ebfe5d03 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -67,24 +67,58 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 // _angle_outputs are in radians, gimbal_controls are in [-1, 1] gimbal_controls_s gimbal_controls{}; - gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( - (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) * - (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))), - -1.f, 1.f); - gimbal_controls.control[gimbal_controls_s::INDEX_PITCH] = constrain( - (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) * - (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))), - -1.f, 1.f); - gimbal_controls.control[gimbal_controls_s::INDEX_YAW] = constrain( - (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) * - (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))), - -1.f, 1.f); + gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = anglesMappedToOutput(gimbal_controls_s::INDEX_ROLL); + gimbal_controls.control[gimbal_controls_s::INDEX_PITCH] = anglesMappedToOutput(gimbal_controls_s::INDEX_PITCH); + gimbal_controls.control[gimbal_controls_s::INDEX_YAW] = anglesMappedToOutput(gimbal_controls_s::INDEX_YAW); gimbal_controls.timestamp = hrt_absolute_time(); _gimbal_controls_pub.publish(gimbal_controls); _last_update = now; } +float OutputRC::anglesMappedToOutput(const uint8_t index) +{ + + float value = 0.f; + float min_value = 0.f; + float max_value = 0.f; + + switch (index) { + case gimbal_controls_s::INDEX_ROLL: { + value = _angle_outputs[0]; + max_value = math::radians(_parameters.mnt_range_roll) * 0.5f; + min_value = -math::radians(_parameters.mnt_range_roll) * 0.5f; + break; + } + + case gimbal_controls_s::INDEX_PITCH: { + value = _angle_outputs[1]; + max_value = math::radians(_parameters.mnt_max_pitch); + min_value = math::radians(_parameters.mnt_min_pitch); + break; + } + + case gimbal_controls_s::INDEX_YAW: { + value = _angle_outputs[2]; + max_value = math::radians(_parameters.mnt_range_yaw) * 0.5f; + min_value = -math::radians(_parameters.mnt_range_yaw) * 0.5f; + break; + } + + default: { + PX4_WARN("INDEX does not exist"); + break; + } + } + + if (value >= FLT_EPSILON) { + return math::interpolate(value, 0.f, max_value, 0.f, 1.f); + + } else { + return math::interpolate(value, min_value, 0.f, -1.f, 0.f); + } +} + void OutputRC::print_status() const { PX4_INFO("Output: AUX"); @@ -98,20 +132,46 @@ void OutputRC::_stream_device_attitude_status() attitude_status.target_component = 0; attitude_status.device_flags = 0; - if (_absolute_angle[0]) { - attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK; + matrix::Quatf q; + + switch (_parameters.mnt_do_stab) { + case MntDoStabilize::PITCH_LOCK: { + // Report device attitude in relative frame as external apps are dependent + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; + matrix::AxisAnglef angle_axis(matrix::Vector3f(0.f, 1.f, 0.f), _angle_outputs[1]); + q = matrix::Quaternionf(angle_axis); + break; + } + + case MntDoStabilize::YAW_LOCK: + case MntDoStabilize::ALL_AXES: + default: { + if (_absolute_angle[0]) { + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK; + } + + if (_absolute_angle[1]) { + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK; + } + + if (_absolute_angle[2]) { + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; + // absolute frame + q = matrix::Quaternionf(_last_valid_setpoint); + + } else { + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME; + // yaw vehicle frame + q = matrix::Quaternionf(_last_valid_setpoint); + } + + + break; + } } - if (_absolute_angle[1]) { - attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK; - } - if (_absolute_angle[2]) { - attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_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; diff --git a/src/modules/gimbal/output_rc.h b/src/modules/gimbal/output_rc.h index de497c194fb4..616bfe86f69b 100644 --- a/src/modules/gimbal/output_rc.h +++ b/src/modules/gimbal/output_rc.h @@ -55,6 +55,7 @@ class OutputRC : public OutputBase private: void _stream_device_attitude_status(); + float anglesMappedToOutput(const uint8_t index); uORB::Publication _gimbal_controls_pub{ORB_ID(gimbal_controls)}; uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index 36134bb3864e..f5bc4881b758 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -38,12 +38,14 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name // Mount parameters _mnt_range_roll_handle = param_find("MNT_RANGE_ROLL"); - _mnt_range_pitch_handle = param_find("MNT_RANGE_PITCH"); + _mnt_max_pitch_handle = param_find("MNT_MAX_PITCH"); + _mnt_min_pitch_handle = param_find("MNT_MIN_PITCH"); _mnt_range_yaw_handle = param_find("MNT_RANGE_YAW"); _mnt_mode_out_handle = param_find("MNT_MODE_OUT"); if (_mnt_range_roll_handle == PARAM_INVALID || - _mnt_range_pitch_handle == PARAM_INVALID || + _mnt_max_pitch_handle == PARAM_INVALID || + _mnt_min_pitch_handle == PARAM_INVALID || _mnt_range_yaw_handle == PARAM_INVALID || _mnt_mode_out_handle == PARAM_INVALID) { return false; @@ -76,7 +78,8 @@ void GZGimbal::Run() if (pollSetpoint()) { //TODO handle device flags publishJointCommand(_gimbal_roll_cmd_publisher, _roll_stp, _roll_rate_stp, _last_roll_stp, _roll_min, _roll_max, dt); - publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _pitch_min, _pitch_max, + publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _mnt_min_pitch, + _mnt_max_pitch, dt); publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt); } @@ -123,8 +126,10 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) void GZGimbal::updateParameters() { + param_get(_mnt_range_roll_handle, &_mnt_range_roll); - param_get(_mnt_range_pitch_handle, &_mnt_range_pitch); + param_get(_mnt_max_pitch_handle, &_mnt_max_pitch); + param_get(_mnt_min_pitch_handle, &_mnt_min_pitch); param_get(_mnt_range_yaw_handle, &_mnt_range_yaw); param_get(_mnt_mode_out_handle, &_mnt_mode_out); } @@ -151,10 +156,12 @@ bool GZGimbal::pollSetpoint() gimbal_controls_s msg; if (_gimbal_controls_sub.copy(&msg)) { - // map control inputs from [-1;1] to [min_angle; max_angle] using the range parameters + // map control inputs from [-1;1] to [min_angle; max_angle] _roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * _mnt_range_roll / 2), _roll_min, _roll_max); - _pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * _mnt_range_pitch / 2), _pitch_min, - _pitch_max); + _pitch_stp = math::radians(math::constrain(_mnt_min_pitch + 0.5f * (msg.control[msg.INDEX_PITCH] + 1.f) * + (_mnt_max_pitch - _mnt_min_pitch), + _mnt_min_pitch, + _mnt_max_pitch)); _yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw / 2), _yaw_min, _yaw_max); return true; @@ -196,8 +203,8 @@ void GZGimbal::publishDeviceInfo() device_info.custom_cap_flags = _custom_cap_flags; device_info.roll_min = _roll_min; device_info.roll_max = _roll_max; - device_info.pitch_min = _pitch_min; - device_info.pitch_max = _pitch_max; + device_info.pitch_min = _mnt_min_pitch; + device_info.pitch_max = _mnt_max_pitch; device_info.yaw_min = _yaw_min; device_info.yaw_max = _yaw_max; device_info.gimbal_device_id = _gimbal_device_id; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 897c43150f5a..a2a7965094a9 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -66,11 +66,13 @@ class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams hrt_abstime _last_time_update; // Mount parameters - param_t _mnt_range_pitch_handle = PARAM_INVALID; + param_t _mnt_max_pitch_handle = PARAM_INVALID; + param_t _mnt_min_pitch_handle = PARAM_INVALID; param_t _mnt_range_roll_handle = PARAM_INVALID; param_t _mnt_range_yaw_handle = PARAM_INVALID; param_t _mnt_mode_out_handle = PARAM_INVALID; - float _mnt_range_pitch = 0.0f; + float _mnt_max_pitch = 0.0f; + float _mnt_min_pitch = 0.0f; float _mnt_range_roll = 0.0f; float _mnt_range_yaw = 0.0f; int32_t _mnt_mode_out = 0; @@ -109,8 +111,6 @@ class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams // its mechanical limits. So the values below have to match the characteristics of the simulated gimbal const float _roll_min = -0.785398f; const float _roll_max = 0.785398f; - const float _pitch_min = -2.35619f; - const float _pitch_max = 0.785398f; const float _yaw_min = NAN; // infinite yaw const float _yaw_max = NAN; // infinite yaw