From f7269c9c220cbd009c2031354a6da2a3ea3bb80f Mon Sep 17 00:00:00 2001 From: Victor Nan Fernandez-Ayala <34630264+ViktorNfa@users.noreply.github.com> Date: Tue, 18 Nov 2025 00:34:35 +0100 Subject: [PATCH] uuv_att_control: added a new surge, sway, heave, yaw control mode, and a stick selector param to switch between modes (#25891) * feat: surge, sway, heave, yaw control method added Signed-off-by: ViktorNfa * fix: ran make format * fix: clean naming and default conditions * fix: switched param selector --------- Signed-off-by: ViktorNfa Co-authored-by: Pedro Roque --- .../uuv_att_control/uuv_att_control.cpp | 90 +++++++++++++++---- .../uuv_att_control/uuv_att_control.hpp | 5 +- .../uuv_att_control/uuv_att_control_params.c | 9 ++ 3 files changed, 87 insertions(+), 17 deletions(-) diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index c7186fc487..1a20eb13dc 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -237,15 +237,22 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude void UUVAttitudeControl::generate_attitude_setpoint(float dt) { + const bool js_heave_sway_mode = joystick_heave_sway_mode(); + // Avoid accumulating absolute yaw error with arming stick gesture float roll = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).phi(); float pitch = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).theta(); float yaw = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).psi(); - // Integrate manual control inputs + float roll_setpoint = 0.0f; + float pitch_setpoint = 0.0f; float yaw_setpoint = yaw + _manual_control_setpoint.yaw * dt * _param_sgm_yaw.get(); - float roll_setpoint = roll + _manual_control_setpoint.roll * dt * _param_sgm_roll.get(); - float pitch_setpoint = pitch + -_manual_control_setpoint.pitch * dt * _param_sgm_pitch.get(); + + if (!js_heave_sway_mode) { + // Integrate roll/pitch from sticks + roll_setpoint = roll + _manual_control_setpoint.roll * dt * _param_sgm_roll.get(); + pitch_setpoint = pitch + -_manual_control_setpoint.pitch * dt * _param_sgm_pitch.get(); + } // Generate target quaternion Eulerf euler_sp(roll_setpoint, pitch_setpoint, yaw_setpoint); @@ -256,21 +263,52 @@ void UUVAttitudeControl::generate_attitude_setpoint(float dt) q_sp.copyTo(_attitude_setpoint.q_d); - _attitude_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_sgm_thrtl.get(); + // Thrust mapping + const float throttle_manual_attitude_gain = _param_sgm_thrtl.get(); + + if (js_heave_sway_mode) { + // XYZ thrust + _attitude_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * throttle_manual_attitude_gain; // surge +x + _attitude_setpoint.thrust_body[1] = _manual_control_setpoint.roll * throttle_manual_attitude_gain; // sway +y + _attitude_setpoint.thrust_body[2] = -_manual_control_setpoint.pitch * throttle_manual_attitude_gain; // heave +z down + + } else { + // Throttle only on +x (surge) + _attitude_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * throttle_manual_attitude_gain; + _attitude_setpoint.thrust_body[1] = 0.f; + _attitude_setpoint.thrust_body[2] = 0.f; + } _attitude_setpoint.timestamp = hrt_absolute_time(); } void UUVAttitudeControl::generate_rates_setpoint(float dt) { - // Integrate manual control inputs - _rates_setpoint.roll = _manual_control_setpoint.roll * dt * _param_rgm_roll.get(); - _rates_setpoint.pitch = -_manual_control_setpoint.pitch * dt * _param_rgm_pitch.get(); - _rates_setpoint.yaw = _manual_control_setpoint.yaw * dt * _param_rgm_yaw.get(); + const bool js_heave_sway_mode = joystick_heave_sway_mode(); + const float throttle_manual_rate_gain = _param_rgm_thrtl.get(); + + if (js_heave_sway_mode) { + // Hold pitch/roll level. Only yaw is a rate command. XYZ thrust + _rates_setpoint.roll = 0.0f; + _rates_setpoint.pitch = 0.0f; + _rates_setpoint.yaw = _manual_control_setpoint.yaw * dt * _param_rgm_yaw.get(); + + _rates_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * throttle_manual_rate_gain; // surge +x + _rates_setpoint.thrust_body[1] = _manual_control_setpoint.roll * throttle_manual_rate_gain; // sway +y + _rates_setpoint.thrust_body[2] = -_manual_control_setpoint.pitch * throttle_manual_rate_gain; // heave +z down + + } else { + // Roll/pitch/yaw are rate commands; thrust only surge + _rates_setpoint.roll = _manual_control_setpoint.roll * dt * _param_rgm_roll.get(); + _rates_setpoint.pitch = -_manual_control_setpoint.pitch * dt * _param_rgm_pitch.get(); + _rates_setpoint.yaw = _manual_control_setpoint.yaw * dt * _param_rgm_yaw.get(); + + _rates_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * throttle_manual_rate_gain; + _rates_setpoint.thrust_body[1] = 0.f; + _rates_setpoint.thrust_body[2] = 0.f; + } - _rates_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_rgm_thrtl.get(); _rates_setpoint.timestamp = hrt_absolute_time(); - } void UUVAttitudeControl::check_setpoint_validity(vehicle_attitude_s &v_att) @@ -352,13 +390,33 @@ void UUVAttitudeControl::Run() } else if (!_vcontrol_mode.flag_control_attitude_enabled && !_vcontrol_mode.flag_control_rates_enabled) { + /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ - constrain_actuator_commands(_manual_control_setpoint.roll * _param_mgm_roll.get(), - -_manual_control_setpoint.pitch * _param_mgm_pitch.get(), - _manual_control_setpoint.yaw * _param_mgm_yaw.get(), - _manual_control_setpoint.throttle * _param_mgm_thrtl.get(), - 0.f, - 0.f); + const bool js_heave_sway_mode = joystick_heave_sway_mode(); + + if (js_heave_sway_mode) { + // Keep roll/pitch level, yaw torque, full XYZ thrust + const float throttle_manual_gain = _param_mgm_thrtl.get(); + + const float roll_u = 0.0f; + const float pitch_u = 0.0f; + const float yaw_u = _manual_control_setpoint.yaw * _param_mgm_yaw.get(); + + const float thrust_x = _manual_control_setpoint.throttle * throttle_manual_gain; // surge + const float thrust_y = _manual_control_setpoint.roll * throttle_manual_gain; // sway + const float thrust_z = -_manual_control_setpoint.pitch * throttle_manual_gain; // heave + + constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z); + + } else { + // Pitch/roll/yaw torques from sticks, thrust only surge + constrain_actuator_commands(_manual_control_setpoint.roll * _param_mgm_roll.get(), + -_manual_control_setpoint.pitch * _param_mgm_pitch.get(), + _manual_control_setpoint.yaw * _param_mgm_yaw.get(), + _manual_control_setpoint.throttle * _param_mgm_thrtl.get(), + 0.f, + 0.f); + } } } else { diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp index 76588848a3..390d64dbb8 100644 --- a/src/modules/uuv_att_control/uuv_att_control.hpp +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -149,7 +149,8 @@ private: (ParamFloat) _param_mgm_thrtl, (ParamFloat) _param_torque_sat, (ParamFloat) _param_thrust_sat, - (ParamFloat) _param_setpoint_max_age + (ParamFloat) _param_setpoint_max_age, + (ParamInt) _param_stick_mode ) @@ -171,4 +172,6 @@ private: void generate_rates_setpoint(float dt); void reset_attitude_setpoint(vehicle_attitude_s &v_att); void check_setpoint_validity(vehicle_attitude_s &v_att); + + inline bool joystick_heave_sway_mode() const { return _param_stick_mode.get() == 0; } }; diff --git a/src/modules/uuv_att_control/uuv_att_control_params.c b/src/modules/uuv_att_control/uuv_att_control_params.c index 08dea0ef41..2a62edbcdc 100644 --- a/src/modules/uuv_att_control/uuv_att_control_params.c +++ b/src/modules/uuv_att_control/uuv_att_control_params.c @@ -239,3 +239,12 @@ PARAM_DEFINE_FLOAT(UUV_THRUST_SAT, 0.1f); * @group UUV Attitude Control */ PARAM_DEFINE_FLOAT(UUV_SP_MAX_AGE, 2.0f); + +/** + * Stick mode selector (0=Heave/sway control, roll/pitch leveled; 1=Pitch/roll control) + * + * @group UUV Attitude Control + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(UUV_STICK_MODE, 0);