diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 96918a3295..7b129ab34f 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -918,21 +918,32 @@ MulticopterPositionControl::reset_alt_sp()
 	}
 }
 
+
 void
 MulticopterPositionControl::limit_altitude()
 {
 
 	/* in altitude control, limit setpoint */
-	if (_pos_sp(2) <= -_vehicle_land_detected.alt_max && _run_alt_control) {
+	if (_run_alt_control && _pos_sp(2) <= -_vehicle_land_detected.alt_max) {
 		_pos_sp(2) = -_vehicle_land_detected.alt_max;
 		return;
 	}
 
-	/* in velocity control in z, prevent vehicle from flying upwards if 0.5m close to altitude max*/
-	if (_pos(2) <= -_vehicle_land_detected.alt_max && !_run_alt_control && _vel_sp(2) <= 0.0f) {
-		_pos_sp(2) = -_vehicle_land_detected.alt_max;
-		_run_alt_control = true;
-		return;
+	/* in velocity control mode and want to fly upwards */
+	if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
+
+		/* time to travel to reach zero velocity */
+		float delta_t = -_vel(2) / _params.acc_down_max;
+
+		/* predicted position */
+		float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
+				   _params.acc_down_max * delta_t *delta_t;
+
+		if (pos_z_next <= -_vehicle_land_detected.alt_max) {
+			_pos_sp(2) = -_vehicle_land_detected.alt_max;
+			_run_alt_control = true;
+			return;
+		}
 	}
 }