From 37570f5c6e42583347b57f2e48861aa339283871 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 17 Mar 2026 16:37:43 -0800 Subject: [PATCH] fix(flight_mode_manager): remove optical flow velocity constraint in manual modes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When relying exclusively on optical flow, the EKF publishes a vxy_max limit proportional to altitude (0.5 × max_flow_rate × height). Both ManualPosition and ManualAcceleration flight tasks applied this as a hard cap on the pilot's stick velocity authority. At low altitudes the constraint severely limits pilot control — e.g. at 0.3 m with SENS_FLOW_MAXR=8 the cap is only 1.2 m/s. This is counterproductive because optical flow is least reliable near the ground, drift is most likely, and the pilot needs full stick authority to correct it. The constraint remains in autonomous modes via MulticopterPositionControl where it is appropriate since there is no pilot to intervene. --- .../FlightTaskManualAcceleration.cpp | 18 ------------------ .../FlightTaskManualPosition.cpp | 7 ------- 2 files changed, 25 deletions(-) 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;