diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
index b62dc0086d..0aa73c67cf 100644
--- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
@@ -93,7 +93,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
 {
 	_setSpeedAtTarget();
 	Vector2f pos_sp_to_dest(_target - _position_setpoint);
-	const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _target_acceptance_radius;
+	const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _param_nav_mc_alt_rad.get();
 
 	if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
 	    (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index fbe2dd1292..02305c77b8 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -243,17 +243,23 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			// Use position setpoints to generate velocity setpoints
 
 			// Get various path specific vectors. */
-			Vector2f pos_traj;
+			Vector3f pos_traj;
 			pos_traj(0) = _trajectory[0].getCurrentPosition();
 			pos_traj(1) = _trajectory[1].getCurrentPosition();
-			Vector2f pos_sp_xy(_position_setpoint);
-			Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
-			Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
-			Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
+			pos_traj(2) = _trajectory[2].getCurrentPosition();
+			Vector2f pos_traj_to_dest_xy(_position_setpoint - pos_traj);
+			Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
+			const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
 
-			float speed_sp_track = _getMaxSpeedFromDistance(pos_traj_to_dest.length());
+			float speed_sp_track = _getMaxSpeedFromDistance(pos_traj_to_dest_xy.length());
 
-			speed_sp_track = math::constrain(speed_sp_track, _getSpeedAtTarget(), _mc_cruise_speed);
+			if (has_reached_altitude) {
+				speed_sp_track = math::constrain(speed_sp_track, _getSpeedAtTarget(), _mc_cruise_speed);
+
+			} else {
+				speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
+
+			}
 
 			Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;