diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
index 3c1b507596..f63f53243d 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
@@ -106,6 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
 
 void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
 {
+	float pos_sp_smooth;
+
+	_smoothing.updateTraj(_deltatime);
+
+	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
+	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
+	_vel_sp_smooth = _smoothing.getCurrentVelocity();
+	pos_sp_smooth = _smoothing.getCurrentPosition();
+
 	/* Get yaw setpont, un-smoothed position setpoints.*/
 	FlightTaskManualAltitude::_updateSetpoints();
 
@@ -142,10 +151,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
 		_position_lock_z_active = false;
 	}
 
-	// During position lock, lower jerk to help the optimizer
-	// to converge to 0 acceleration and velocity
-	jerk = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get();
-
 	_smoothing.setMaxJerk(jerk);
 	_smoothing.updateDurations(_velocity_setpoint(2));
 
@@ -153,16 +158,6 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
 		_smoothing.setCurrentPosition(_position(2));
 	}
 
-	float pos_sp_smooth;
-
-	// TODO: move before updateDurations
-	_smoothing.updateTraj(_deltatime);
-
-	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
-	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
-	_vel_sp_smooth = _smoothing.getCurrentVelocity();
-	pos_sp_smooth = _smoothing.getCurrentPosition();
-
 	_velocity_setpoint(2) = _vel_sp_smooth; // Feedforward
 
 	if (fabsf(_vel_sp_smooth) < 0.1f &&