diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index f73fbe4023..91d939dec9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -63,24 +63,6 @@ bool FlightTaskManualAcceleration::update() setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy); bool ret = FlightTaskManualAltitudeSmoothVel::update(); - float max_hagl_ratio = 0.0f; - - if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) { - max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy; - } - - // limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps - static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl - static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl - - if (max_hagl_ratio > factor_threshold) { - const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get()); - _stick_acceleration_xy.setVelocityConstraint(interpolate(max_hagl_ratio, factor_threshold, 1.f, vxy_max, min_vel)); - - } else if (PX4_ISFINITE(vehicle_local_pos.vxy_max)) { - _stick_acceleration_xy.setVelocityConstraint(vehicle_local_pos.vxy_max); - } - _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 352875764e..689770bacf 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -79,15 +79,8 @@ void FlightTaskManualPosition::_scaleSticks() stick_xy(0) *= _param_mpc_vel_man_back.get() / _param_mpc_vel_manual.get(); } - const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max; - float velocity_scale = _param_mpc_vel_manual.get(); - if (PX4_ISFINITE(max_speed_from_estimator)) { - // Constrain with optical flow limit but leave 0.3 m/s for repositioning - velocity_scale = math::constrain(velocity_scale, 0.3f, max_speed_from_estimator); - } - // scale velocity to its maximum limits Vector2f vel_sp_xy = stick_xy * velocity_scale;