diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 9454f8cedd..5ef59be4bb 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -102,8 +102,6 @@ void FlightTaskManualPosition::_scaleSticks() _velocity_scale = _constraints.speed_xy; } - _velocity_scale = fminf(_computeVelXYGroundDist(), _velocity_scale); - // scale velocity to its maximum limits Vector2f vel_sp_xy = stick_xy * _velocity_scale; @@ -118,20 +116,6 @@ void FlightTaskManualPosition::_scaleSticks() _velocity_setpoint.xy() = vel_sp_xy; } -float FlightTaskManualPosition::_computeVelXYGroundDist() -{ - float max_vel_xy = _constraints.speed_xy; - - // limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2 - if (PX4_ISFINITE(_dist_to_ground)) { - max_vel_xy = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_vel_xy.get(), _constraints.speed_xy); - } - - return max_vel_xy; -} - void FlightTaskManualPosition::_updateXYlock() { /* If position lock is not active, position setpoint is set to NAN.*/ diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index 0d0594aed4..b0db7c3387 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -65,12 +65,10 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude, (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_land_vel_xy, (ParamFloat) _param_mpc_acc_hor_max, (ParamFloat) _param_mpc_hold_max_xy ) private: - float _computeVelXYGroundDist(); float _velocity_scale{0.0f}; //scales the stick input to velocity uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 453c37b437..674d44b48b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -121,7 +121,6 @@ int MulticopterPositionControl::parameters_update(bool force) num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel); num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel); num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel); - num_changed += _param_mpc_land_vel_xy.commit_no_notification(xy_vel * 0.75f); } if (_param_mpc_z_vel_all.get() >= 0.f) { diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 3b0f13228a..1e6b591a42 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -172,8 +172,7 @@ private: (ParamFloat) _param_mpc_man_y_tau, (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all, - (ParamFloat) _param_mpc_land_vel_xy + (ParamFloat) _param_mpc_z_vel_all ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 2ed13f323d..46c5fb3946 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -363,18 +363,6 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); -/** - * Maximum horizontal position mode velocity when close to ground/home altitude - * - * Set the value higher than the otherwise expected maximum to disable any slowdown. - * - * @unit m/s - * @min 0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_VEL_XY, 10.0f); - /** * Enable user assisted descent speed for autonomous land routine. * @@ -659,12 +647,8 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f); /** * Altitude for 1. step of slow landing (descend) * - * Below this altitude: - * - descending velocity gets limited to a value + * Below this altitude descending velocity gets limited to a value * between "MPC_Z_VEL_MAX_DN" and "MPC_LAND_SPEED" - * - horizontal velocity gets limited to a value - * between "MPC_VEL_MANUAL" and "MPC_LAND_VEL_XY" - * for a smooth descent and landing experience. * Value needs to be higher than "MPC_LAND_ALT2" * * @unit m @@ -678,8 +662,8 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); /** * Altitude for 2. step of slow landing (landing) * - * Below this altitude descending and horizontal velocities get - * limited to "MPC_LAND_SPEED" and "MPC_LAND_VEL_XY", respectively. + * Below this altitude descending velocity gets + * limited to "MPC_LAND_SPEED". * Value needs to be lower than "MPC_LAND_ALT1" * * @unit m