From 84220407ea832fe032f409911fa3702b9c9a689b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 14 Aug 2023 16:40:42 +0200 Subject: [PATCH] FlightTaskManualAltitude: vertical velocity limit interface --- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 5 +++-- .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp | 3 +++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 3173a0c7d2..22eb458c81 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -92,8 +92,9 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() void FlightTaskManualAltitude::_scaleSticks() { // Use sticks input with deadzone and exponential curve for vertical velocity - const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() : - _param_mpc_z_vel_max_up.get(); + 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_z = (_sticks.getPosition()(2) > 0.0f) ? vel_max_down : vel_max_up; _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 05e7b74b74..527e5ad6f7 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -75,6 +75,9 @@ protected: bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ + float _velocity_constraint_up{INFINITY}; + float _velocity_constraint_down{INFINITY}; + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_hold_max_z, (ParamInt) _param_mpc_alt_mode,