From 8c1642a3c74774b757a22e509930abcb7e838adc Mon Sep 17 00:00:00 2001 From: Daniel Mesham Date: Thu, 2 May 2024 15:03:15 +0200 Subject: [PATCH] flight_mode_manager: create separate parameters for vertical velocity limits in Alt/Pos mode --- .../FlightTaskManualAccelerationSlow.cpp | 4 +-- .../FlightTaskManualAccelerationSlow.hpp | 2 -- .../FlightTaskManualAltitude.cpp | 21 +++++++++---- .../FlightTaskManualAltitude.hpp | 3 ++ .../MulticopterPositionControl.cpp | 30 ++++++++++++++++--- .../MulticopterPositionControl.hpp | 2 ++ .../multicopter_altitude_mode_params.c | 28 +++++++++++++++++ 7 files changed, 76 insertions(+), 14 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index 7b1c24f1dd..df80acb8bc 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -50,8 +50,8 @@ bool FlightTaskManualAccelerationSlow::update() // Limits which can only slow down from the nominal configuration we initialize with here // This is ensured by the executing classes float velocity_horizontal = _param_mpc_vel_manual.get(); - float velocity_up = _param_mpc_z_vel_max_up.get(); - float velocity_down = _param_mpc_z_vel_max_dn.get(); + float velocity_up = _param_mpc_z_v_man_up.get(); + float velocity_down = _param_mpc_z_v_man_dn.get(); float yaw_rate = math::radians(_param_mpc_man_y_max.get()); // MAVLink commanded limits diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp index b54d9b765d..e2525561ad 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -81,8 +81,6 @@ private: (ParamFloat) _param_mc_slow_def_vvel, (ParamFloat) _param_mc_slow_def_yawr, (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_z_vel_max_up, - (ParamFloat) _param_mpc_z_vel_max_dn, (ParamFloat) _param_mpc_man_y_max ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 22eb458c81..4641d09978 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -72,6 +72,15 @@ bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoi return ret; } +void FlightTaskManualAltitude::_setDefaultConstraints() +{ + FlightTask::_setDefaultConstraints(); + + // Apply velocity limits for manual modes + _constraints.speed_up = _param_mpc_z_v_man_up.get(); + _constraints.speed_down = _param_mpc_z_v_man_dn.get(); +} + void FlightTaskManualAltitude::_updateConstraintsFromEstimator() { if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) { @@ -92,8 +101,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() void FlightTaskManualAltitude::_scaleSticks() { // Use sticks input with deadzone and exponential curve for vertical velocity - const float vel_max_up = fminf(_param_mpc_z_vel_max_up.get(), _velocity_constraint_up); - const float vel_max_down = fminf(_param_mpc_z_vel_max_dn.get(), _velocity_constraint_down); + const float vel_max_up = fminf(_param_mpc_z_v_man_up.get(), _velocity_constraint_up); + const float vel_max_down = fminf(_param_mpc_z_v_man_dn.get(), _velocity_constraint_down); const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? vel_max_down : vel_max_up; _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); } @@ -234,10 +243,10 @@ void FlightTaskManualAltitude::_respectMaxAltitude() // TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller if (PX4_ISFINITE(_max_distance_to_ground)) { _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), - -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); + -_param_mpc_z_v_man_dn.get(), _param_mpc_z_v_man_up.get()); } else { - _constraints.speed_up = _param_mpc_z_vel_max_up.get(); + _constraints.speed_up = _param_mpc_z_v_man_up.get(); } // if distance to bottom exceeded maximum distance, slowly approach maximum distance @@ -247,10 +256,10 @@ void FlightTaskManualAltitude::_respectMaxAltitude() // set position setpoint to maximum distance to ground _position_setpoint(2) = _position(2) + delta_distance_to_max; // limit speed downwards to 0.7m/s - _constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f); + _constraints.speed_down = math::min(_param_mpc_z_v_man_dn.get(), 0.7f); } else { - _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + _constraints.speed_down = _param_mpc_z_v_man_dn.get(); } } } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 527e5ad6f7..344eccb4ce 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -59,6 +59,7 @@ protected: virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ bool _checkTakeoff() override; + void _setDefaultConstraints() override; void _updateConstraintsFromEstimator(); /** @@ -83,6 +84,8 @@ protected: (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_hold_max_xy, (ParamFloat) _param_mpc_z_p, /**< position controller altitude propotional gain */ + (ParamFloat) _param_mpc_z_v_man_up, /**< maximum upwards velocity */ + (ParamFloat) _param_mpc_z_v_man_dn, /**< maximum downwards velocity */ (ParamFloat) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */ (ParamFloat) _param_mpc_land_alt2, /**< altitude below which to land with land speed */ (ParamFloat) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 9092892183..c22e4d870b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -225,26 +225,48 @@ void MulticopterPositionControl::parameters_update(bool force) "Manual sideways speed has been constrained by forward speed", _param_mpc_vel_manual.get()); } + if (_param_mpc_z_v_man_up.get() > _param_mpc_z_vel_max_up.get()) { + _param_mpc_z_v_man_up.set(_param_mpc_z_vel_max_up.get()); + _param_mpc_z_v_man_up.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual ascent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_MAN_UP is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_manual_up_vel_set"), events::Log::Warning, + "Manual ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get()); + } + + if (_param_mpc_z_v_man_dn.get() > _param_mpc_z_vel_max_dn.get()) { + _param_mpc_z_v_man_dn.set(_param_mpc_z_vel_max_dn.get()); + _param_mpc_z_v_man_dn.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual descent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_MAN_DN is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_manual_down_vel_set"), events::Log::Warning, + "Manual descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get()); + } + if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) { _param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get()); _param_mpc_z_v_auto_up.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t"); + mavlink_log_critical(&_mavlink_log_pub, "Auto ascent speed has been constrained by max speed\t"); /* EVENT * @description MPC_Z_V_AUTO_UP is set to {1:.0}. */ events::send(events::ID("mc_pos_ctrl_up_vel_set"), events::Log::Warning, - "Ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get()); + "Auto ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get()); } if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) { _param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get()); _param_mpc_z_v_auto_dn.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t"); + mavlink_log_critical(&_mavlink_log_pub, "Auto descent speed has been constrained by max speed\t"); /* EVENT * @description MPC_Z_V_AUTO_DN is set to {1:.0}. */ events::send(events::ID("mc_pos_ctrl_down_vel_set"), events::Log::Warning, - "Descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get()); + "Auto descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get()); } if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() || diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 3dac59bc1b..3c724b16fe 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -140,8 +140,10 @@ private: (ParamFloat) _param_mpc_z_vel_d_acc, (ParamFloat) _param_mpc_xy_vel_max, (ParamFloat) _param_mpc_z_v_auto_up, + (ParamFloat) _param_mpc_z_v_man_up, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_v_auto_dn, + (ParamFloat) _param_mpc_z_v_man_dn, (ParamFloat) _param_mpc_z_vel_max_dn, (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c index 3f654fade7..0d8cce4c90 100644 --- a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -55,6 +55,34 @@ PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.f); */ PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.f); +/** + * Maximum upwards velocity in Altitude/Position mode + * + * Should be less than MPC_Z_VEL_MAX_UP. + * + * @unit m/s + * @min 0.5 + * @max 8 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_MAN_UP, 3.f); + +/** + * Maximum downwards velocity in Altitude/Position mode + * + * Should be less than MPC_Z_VEL_MAX_DN. + * + * @unit m/s + * @min 0.5 + * @max 4 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_MAN_DN, 1.5f); + /** * Manual yaw rate input filter time constant *