diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index 36062b97ac..a5bcf692b9 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -324,6 +324,14 @@ void FlightTaskAuto::_checkAvoidanceProgress()
 		pos_control_status.acceptance_radius = Vector2f(&(_triplet_target - _position)(0)).length() + 0.5f;
 	}
 
+	Vector2f pos_to_target = Vector2f(&(_triplet_target - _position)(0));
+	const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2));
+
+	if (pos_to_target.length() < NAV_ACC_RAD.get() && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
+		// vehicle above or below the target waypoint
+		pos_control_status.altitude_acceptance_radius = pos_to_target_z + 0.5f;
+	}
+
 	// do not check for waypoints yaw acceptance in navigator
 	pos_control_status.yaw_acceptance = NAN;
 
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index a80d34a563..4b0a695ec6 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -108,6 +108,7 @@ protected:
 					(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE,
 					(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
 					(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD, // acceptance radius at which waypoints are updated move to line
+					(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD, //vertical acceptance radius at which waypoints are updated
 					(ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE // defines how heading is executed
 				       );