diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index df77a5d20c..650a748965 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -60,6 +60,7 @@ bool FlightTaskManualAltitude::activate()
 {
 	_yaw_sp_predicted = _yaw_sp = _yaw;
 	_yaw_rate_sp = _vel_sp_z = NAN;
+	_pos_sp_z = _pos_sp_z_lock = _position(2);
 	_lock_time = 0.0f;
 	_lock_time_max = 1.0f; // 1s time to brake as default
 
@@ -100,11 +101,12 @@ void FlightTaskManualAltitude::updateZsetpoints()
 			/* Don't lock: time has not been reached */
 			_vel_sp_z = 0.0f;
 			_pos_sp_z = NAN;
+			_pos_sp_z_lock = _position(2);
 			_lock_time += _deltatime;
 
 		} else {
 			_vel_sp_z = NAN;
-			_pos_sp_z = _position(2);
+			_pos_sp_z = _pos_sp_z_lock;
 		}
 
 	} else {
@@ -112,9 +114,11 @@ void FlightTaskManualAltitude::updateZsetpoints()
 		_pos_sp_z = NAN;
 
 		/* Maximum brake acceleration depends
-		 * on direction.
+		 * on direction. We take half of max acceleration
+		 * because it is better to lock late than early to prevent
+		 * up and down movement.
 		 */
-		float brake_acc = (_velocity(2) > 0.0f) ? _acc_max_up.get() : _acc_max_down.get();
+		float brake_acc = (_velocity(2) > 0.0f) ? (0.5f * _acc_max_up.get()) : (0.5f * _acc_max_down.get());;
 
 		if (PX4_ISFINITE(brake_acc)) {
 			_lock_time_max = fabsf(_velocity(2)) / brake_acc;
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index 0749effeca..298e69f3ff 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -54,11 +54,12 @@ public:
 	bool update() override;
 
 protected:
-	float _yaw_rate_sp{0.0f}; /** scaled yaw rate directly from stick. NAN if yaw is locked */
-	float _yaw_sp{0.0f}; /** yaw setpoint once locked. otherwise NAN */
-	float _yaw_sp_predicted{0.0f}; /** _yaw_sp during lock; predicted yaw through _yaw_rate_sp demand */
-	float _vel_sp_z{0.0f}; /**< scaled velocity directly from stick. During altitude lock is equal to NAN */
-	float _pos_sp_z{0.0f}; /**< position setpoint in z during lock. Otherwise NAN. */
+	float _yaw_rate_sp{}; /** scaled yaw rate directly from stick. NAN if yaw is locked */
+	float _yaw_sp{}; /** yaw setpoint once locked. otherwise NAN */
+	float _yaw_sp_predicted{}; /** _yaw_sp during lock; predicted yaw through _yaw_rate_sp demand */
+	float _vel_sp_z{}; /**< scaled velocity directly from stick. During altitude lock is equal to NAN */
+	float _pos_sp_z{}; /**< altitude setpoint in z during lock. Otherwise NAN. */
+	float _pos_sp_z_lock{}; /**< altitude which used when lock is engaged */
 
 	control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
 	control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */