diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp
index 026406c57d..08abaa7d6c 100644
--- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp
+++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp
@@ -61,12 +61,14 @@ void AckermannPosControl::updatePosControl()
 	if (target_waypoint_ned.isAllFinite()) {
 		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
 
-		if (distance_to_target > _acceptance_radius) {
-			float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
-					      0.f;
-			const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
+		if (_arrival_speed > FLT_EPSILON) {
+			distance_to_target -= _acceptance_radius; // shift target to the edge of the acceptance radius if arrival speed not zero
+		}
+
+		if (distance_to_target > _acceptance_radius || _arrival_speed > FLT_EPSILON) {
+
 			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-					       _param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
+					       _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
 			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
 
 			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
@@ -130,6 +132,7 @@ void AckermannPosControl::updateSubscriptions()
 		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
 		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
 		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
+		_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
 	}
 
 }
diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
index af2d7581b0..f742471c75 100644
--- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
+++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
@@ -106,6 +106,7 @@ private:
 	Quatf _vehicle_attitude_quaternion{};
 	Vector2f _curr_pos_ned{};
 	Vector2f _start_ned{};
+	float _arrival_speed{0.f};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	float _acceptance_radius{0.f}; // Acceptance radius for the waypoint.
diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
index 52a5a00eeb..520dd116fb 100644
--- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
+++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
@@ -54,24 +54,19 @@ void DifferentialPosControl::updatePosControl()
 
 	hrt_abstime timestamp = hrt_absolute_time();
 
-	if (_rover_position_setpoint_sub.updated()) {
-		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
-		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
-	}
-
 	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
 
 	if (target_waypoint_ned.isAllFinite()) {
 		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
 
-		if (distance_to_target > _param_nav_acc_rad.get()) {
-			float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
-					      0.f;
-			const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() :
-					       distance_to_target;
+		if (_arrival_speed > FLT_EPSILON) {
+			distance_to_target -=
+				_param_nav_acc_rad.get(); // shift target to the edge of the acceptance radius if arrival speed not zero
+		}
+
+		if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
 			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-					       _param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
+					       _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
 			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
 
 			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
@@ -119,6 +114,14 @@ void DifferentialPosControl::updateSubscriptions()
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
 	}
 
+	if (_rover_position_setpoint_sub.updated()) {
+		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
+		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
+		_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
+	}
+
+
 }
 
 bool DifferentialPosControl::runSanityChecks()
diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
index 52c5253ef7..935ba97af5 100644
--- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
+++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
@@ -103,6 +103,7 @@ private:
 	// Variables
 	Vector2f _curr_pos_ned{};
 	Vector2f _start_ned{};
+	float _arrival_speed{0.f};
 	float _vehicle_yaw{0.f};
 
 	DEFINE_PARAMETERS(
diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
index 9f966823ad..135888a12b 100644
--- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
+++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
@@ -56,26 +56,21 @@ void MecanumPosControl::updatePosControl()
 
 	hrt_abstime timestamp = hrt_absolute_time();
 
-	if (_rover_position_setpoint_sub.updated()) {
-		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
-		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
-		_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
-	}
-
 	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
 
 	if (target_waypoint_ned.isAllFinite()) {
 
 		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
 
-		if (distance_to_target > _param_nav_acc_rad.get()) {
-			float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
-					      0.f;
-			const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() :
-					       distance_to_target;
+		if (_arrival_speed > FLT_EPSILON) {
+			distance_to_target -=
+				_param_nav_acc_rad.get(); // shift target to the edge of the acceptance radius if arrival speed not zero
+		}
+
+		if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
+
 			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-					       _param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
+					       _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
 			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
 
 			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
@@ -130,6 +125,14 @@ void MecanumPosControl::updateSubscriptions()
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
 	}
 
+	if (_rover_position_setpoint_sub.updated()) {
+		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
+		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
+		_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
+		_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
+	}
+
 }
 
 bool MecanumPosControl::runSanityChecks()
diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
index c82cb2c4e9..7979b39ced 100644
--- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
+++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
@@ -105,6 +105,7 @@ private:
 	Quatf _vehicle_attitude_quaternion{};
 	Vector2f _curr_pos_ned{};
 	Vector2f _start_ned{};
+	float _arrival_speed{0.f};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
 	float _yaw_setpoint{NAN};