From dd05cb3496f02a4eea579f9bc7e1d23826ef54b9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 9 Nov 2021 15:19:47 +0100 Subject: [PATCH] FlightTaskManualPosition: simplify optical flow limiting logic --- .../tasks/ManualPosition/FlightTaskManualPosition.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 27ba94165b..1cb73d6a8f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -89,10 +89,8 @@ void FlightTaskManualPosition::_scaleSticks() float velocity_scale = _param_mpc_vel_manual.get(); if (PX4_ISFINITE(max_speed_from_estimator)) { - // use the minimum of the estimator and user specified limit - velocity_scale = fminf(velocity_scale, max_speed_from_estimator); - // Allow for a minimum of 0.3 m/s for repositioning - velocity_scale = fmaxf(velocity_scale, 0.3f); + // 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