diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index 342ecb7989..b9e79d5f74 100644
--- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -455,7 +455,7 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
 	Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
 	float position_error = drone_to_trajectory_xy.length();
 
-	float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
+	float time_stretch = 1.f - math::constrain(position_error / _param_mpc_xy_err_max.get(), 0.f, 1.f);
 
 	// Don't stretch time if the drone is ahead of the position setpoint
 	if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
index 3e4b361e99..1a6ceb8206 100644
--- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
@@ -96,6 +96,7 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
 					(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
-					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p
+					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
+					(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max
 				       );
 };
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 6a7912957c..332490345a 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -281,6 +281,24 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
 
+/**
+ * Maximum horizontal error allowed by the trajectory generator
+ *
+ * The integration speed of the trajectory setpoint is linearly
+ * reduced with the horizontal position tracking error. When the
+ * error is above this parameter, the integration of the
+ * trajectory is stopped to wait for the drone.
+ *
+ * This value can be adjusted depending on the tracking
+ * capabilities of the vehicle.
+ *
+ * @min 0.1
+ * @max 10.0
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
+
 /**
  * Maximum horizontal velocity setpoint for manual controlled mode
  * If velocity setpoint larger than MPC_XY_VEL_MAX is set, then