diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index f95535e543..d3b0e66c9f 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -125,12 +125,14 @@ void FlightTaskAuto::_limitYawRate()
 		const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
 		const float dyaw_max = yawrate_max * _deltatime;
 		const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max);
-		_yaw_setpoint = _yaw_sp_prev + dyaw;
-		_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint);
-		_yaw_sp_prev = _yaw_setpoint;
+		float yaw_setpoint_sat = _yaw_sp_prev + dyaw;
+		yaw_setpoint_sat = matrix::wrap_pi(yaw_setpoint_sat);
 
-		// The yaw setpoint is aligned when its rate is not saturated
-		_yaw_sp_aligned = fabsf(dyaw_desired) < fabsf(dyaw_max);
+		// The yaw setpoint is aligned when it is within tolerance
+		_yaw_sp_aligned = fabsf(_yaw_setpoint - yaw_setpoint_sat) < math::radians(_param_mis_yaw_err.get());
+
+		_yaw_setpoint = yaw_setpoint_sat;
+		_yaw_sp_prev = _yaw_setpoint;
 	}
 
 	if (PX4_ISFINITE(_yawspeed_setpoint)) {
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index c7031da898..2e32a2d2d3 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -119,7 +119,8 @@ protected:
 					_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
 					(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
 					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
-					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max
+					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
+					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err // yaw-error threshold
 				       );
 
 private:
diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
index 0aa73c67cf..3bd5299110 100644
--- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
@@ -175,14 +175,8 @@ void FlightTaskAutoLine::_generateXYsetpoints()
 			// Vehicle is still far from destination. Accelerate or keep maximum target speed.
 			float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime;
 
-			float yaw_diff = 0.0f;
-
-			if (PX4_ISFINITE(_yaw_setpoint)) {
-				yaw_diff = wrap_pi(_yaw_setpoint - _yaw);
-			}
-
 			// If yaw offset is large, only accelerate with 0.5 m/s^2.
-			float acc_max = (fabsf(yaw_diff) > math::radians(_param_mis_yaw_err.get())) ? 0.5f : _param_mpc_acc_hor.get();
+			float acc_max = (_yaw_sp_aligned) ? _param_mpc_acc_hor.get() : 0.5f;
 
 			if (acc_track > acc_max) {
 				// accelerate towards target
diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
index 338045bdad..94d0ad0ed4 100644
--- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
+++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
@@ -51,7 +51,6 @@ public:
 protected:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
-					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
 					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index 02305c77b8..bcd6ea5cb6 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -191,10 +191,12 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
 	float speed_at_target = 0.0f;
 
 	const float distance_current_next = Vector2f(&(_target - _next_wp)(0)).length();
+	const bool waypoint_overlap = Vector2f(&(_target - _prev_wp)(0)).length() < _target_acceptance_radius;
+	const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned;
 
 	if (distance_current_next > 0.001f &&
-	    (Vector2f(&(_target - _prev_wp)(0)).length() > _target_acceptance_radius) &&
-	    _param_mpc_yaw_mode.get() != 4) {
+	    !waypoint_overlap &&
+	    yaw_align_check_pass) {
 		// Max speed between current and next
 		const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next);
 		const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() * Vector2f(&(_target - _next_wp)(