From 1c821cf83dcd688a185f00205700212af191e8b5 Mon Sep 17 00:00:00 2001 From: Balduin Date: Thu, 23 Oct 2025 17:36:46 +0200 Subject: [PATCH] FlightTaskAuto: disable MPC_LAND_RADIUS by default with value -1 (#25275) * FlightTaskAuto: disable MPC_LAND_RADIUS by default * format & comment * Remove position valididy check again * clean up according to review --- .../tasks/Auto/FlightTaskAuto.cpp | 29 ++++++++++++------- .../multicopter_nudging_params.c | 12 +++++--- 2 files changed, 26 insertions(+), 15 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 47807bffb6..3a77075f11 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -266,21 +266,28 @@ void FlightTaskAuto::_prepareLandSetpoints() Vector2f sticks_ne = sticks_xy; Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading); - const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(), - _param_mpc_land_radius.get(), sticks_ne); - float max_speed; + float max_speed = INFINITY; - if (PX4_ISFINITE(distance_to_circle)) { - max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(), - _stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f); + if (_param_mpc_land_radius.get() > FLT_EPSILON) { - if (max_speed < 0.5f) { + // = NaN if we are outside of the allowed circle and nudging does not point back towards it + const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(), + _param_mpc_land_radius.get(), sticks_ne); + + if (PX4_ISFINITE(distance_to_circle)) { + + // We are inside of the allowed circle. Limit speed so we can always brake in time to not leave the circle. + max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(), + _stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f); + + if (max_speed < 0.5f) { + sticks_xy.setZero(); + } + + } else { + max_speed = 0.f; sticks_xy.setZero(); } - - } else { - max_speed = 0.f; - sticks_xy.setZero(); } _stick_acceleration_xy.setVelocityConstraint(max_speed); diff --git a/src/modules/mc_pos_control/multicopter_nudging_params.c b/src/modules/mc_pos_control/multicopter_nudging_params.c index c431e88fd0..89f8f9714f 100644 --- a/src/modules/mc_pos_control/multicopter_nudging_params.c +++ b/src/modules/mc_pos_control/multicopter_nudging_params.c @@ -53,13 +53,17 @@ PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); /** * User assisted landing radius * - * When nudging is enabled (see MPC_LAND_RC_HELP), this controls - * the maximum allowed horizontal displacement from the original landing point. + * When nudging is enabled (see MPC_LAND_RC_HELP), this defines the maximum + * allowed horizontal displacement from the original landing point. + * - If inside of the radius, only allow nudging inputs that do not move the vehicle outside of it. + * - If outside of the radius, only allow nudging inputs that move the vehicle back towards it. + * + * Set it to -1 for infinite radius. * * @unit m - * @min 0 + * @min -1 * @decimal 0 * @increment 1 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f); +PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, -1.0f);