diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index 8e08a5199018..f83049a179de 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -428,11 +428,12 @@ State FlightTaskAuto::_getCurrentState()
 		// Target is behind.
 		return_state = State::target_behind;
 
-	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
+	} else if (u_prev_to_target * prev_to_pos < 0.0f
+		   && prev_to_pos.length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) {
 		// Current position is more than cruise speed in front of previous setpoint.
 		return_state = State::previous_infront;
 
-	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
+	} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed * _param_mpc_wp_nav_acc.get()) {
 		// Vehicle is more than cruise speed off track.
 		return_state = State::offtrack;
 
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index d68c26571a09..56dd849cd3e5 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -107,6 +107,7 @@ class FlightTaskAuto : public FlightTask
 	ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_WP_NAV_ACC>) _param_mpc_wp_nav_acc,
 					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
 					(ParamFloat<px4::params::MPC_CRUISE_90>) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line
 					(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
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 746f930964fb..f06a997013b1 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -249,6 +249,19 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
  */
 PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
 
+/**
+ * Navigator waypoint acceptance threshold
+ *
+ * Threshold for the internal waypoint update in FLightTaskAuto state machine.
+ *
+ * @min 1.0
+ * @max 1000.0
+ * @increment 1
+ * @decimal 2
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_WP_NAV_ACC, 1.0f);
+
 /**
  * Proportional gain for horizontal trajectory position error
  *