diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index 0b826fd55e..40fe4704e1 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -49,14 +49,17 @@ FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent,
 	FlightTaskManual(parent, name),
 	_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false),
 	_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false),
-	_yaw_rate_scaling(parent, "MPC_MAN_Y_MAX", false)
+	_yaw_rate_scaling(parent, "MPC_MAN_Y_MAX", false),
+	_acc_max_up(parent, "MPC_ACC_UP_MAX", false),
+	_acc_max_down(parent, "MPC_ACC_DOWN_MAX", false)
 {}
 
 bool FlightTaskManualAltitude::activate()
 {
-	_pos_sp_predicted = _pos_sp_z = _position(2);
 	_yaw_sp_predicted = _yaw_sp = _yaw;
 	_yaw_rate_sp = _vel_sp_z = NAN;
+	_lock_time = 0.0f;
+	_lock_time_max = 1.0f; // 1s time to brake as default
 
 	return FlightTaskManual::activate();
 }
@@ -64,7 +67,7 @@ bool FlightTaskManualAltitude::activate()
 void FlightTaskManualAltitude::scaleSticks()
 {
 
-	/* map stick to velocity */
+	/* Map stick to velocity */
 	const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
 	_vel_sp_z = vel_max_z * _sticks_expo(2);
 	_yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get());
@@ -74,12 +77,12 @@ void FlightTaskManualAltitude::scaleSticks()
 void FlightTaskManualAltitude::updateHeadingSetpoints()
 {
 	if (fabsf(_sticks(3)) < FLT_EPSILON) {
-		/* want to hold yaw */
+		/* Want to hold yaw */
 		_yaw_rate_sp = NAN;
 		_yaw_sp = _yaw_sp_predicted;
 
 	} else {
-		/* want to change yaw based on yaw-rate */
+		/* Want to change yaw based on yaw-rate */
 		_yaw_sp = NAN;
 		_yaw_sp_predicted = _wrap_pi(_yaw_sp_predicted + _yaw_rate_sp * _deltatime);
 	}
@@ -88,14 +91,37 @@ void FlightTaskManualAltitude::updateHeadingSetpoints()
 void FlightTaskManualAltitude::updateZsetpoints()
 {
 	if (fabsf(_sticks(2)) < FLT_EPSILON) {
-		/* want to hold altitude */
-		_vel_sp_z = NAN;
-		_pos_sp_z = _pos_sp_predicted;
+
+		/* Want to hold altitude */
+
+		if (_lock_time <= _lock_time_max) {
+			/* Don't lock: time has not been reached */
+			_vel_sp_z = 0.0f;
+			_pos_sp_z = NAN;
+			_lock_time += _deltatime;
+
+		} else {
+			_vel_sp_z = NAN;
+			_pos_sp_z = _position(2);
+		}
 
 	} else {
-		/* want to change altitude through velocity */
+		/* Want to change altitude through velocity */
 		_pos_sp_z = NAN;
-		_pos_sp_predicted = _position(2) + _vel_sp_z * _deltatime;
+
+		/* Maximum brake acceleration depends
+		 * on direction.
+		 */
+		float brake_acc = (_velocity(2) > 0.0f) ? _acc_max_up.get() : _acc_max_down.get();
+
+		if (PX4_ISFINITE(brake_acc)) {
+			_lock_time_max = fabsf(_velocity(2)) / brake_acc;
+
+		} else {
+			_lock_time_max = 1.0f; // 1 second time to brake if no acceleration is set
+		}
+
+		_lock_time = 0.0f;
 	}
 }
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index 50edac664c..3715e66c9f 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -54,15 +54,17 @@ public:
 	bool update() override;
 
 protected:
-
-	float _vel_sp_z{0.0f}; /**< scaled velocity directly from stick */
-	float _yaw_rate_sp{0.0f}; /** scaled yaw rate directly from stick */
-	float _pos_sp_z{0.0f};
-	float _yaw_sp{0.0f};
+	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. */
 
 	control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
 	control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */
 	control::BlockParamFloat _yaw_rate_scaling; /**< scaling factor from stick to yaw rate */
+	control::BlockParamFloat _acc_max_up; /**< maximum acceleration upward */
+	control::BlockParamFloat _acc_max_down; /**< maximum acceleration downward */
 
 	virtual void updateSetpoints(); /**< updates all setpoints */
 	virtual void scaleSticks(); /**< scales sticks to velocity */
@@ -71,7 +73,6 @@ private:
 	void updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	void updateZsetpoints(); /**< sets position or velocity setpoint */
 
-	float _pos_sp_predicted{0.0f}; /**< position setpoint computed in set_z_setpoints */
-	float _yaw_sp_predicted{0.0f}; /**< yaw setpoint computed in set_heading_setpoints */
-
+	float _lock_time_max{0.0f}; /**< defines time when altitude lock occurs */
+	float _lock_time{0.0f}; /**< time after stick are at zero position */
 };