FlightTaskAuto: calculate the new altitude acceptance radius if the vehicle

is inside the xy acceptance radius but not inside the z acceptance radius

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;
 

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
 				       );