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 cc8a450f4d..c29705c918 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -410,10 +410,14 @@ private:
 	/**
 	 * trajctory generation
 	 */
+	void execute_avoidance_position_waypoint();
+
 	void execute_avoidance_velocity_waypoint();
 
 	bool use_obstacle_avoidance();
 
+	bool use_pos_wp_avoidance();
+
 	bool use_vel_wp_avoidance();
 
 	/**
@@ -2396,6 +2400,10 @@ MulticopterPositionControl::control_position()
 void
 MulticopterPositionControl::calculate_velocity_setpoint()
 {
+	if (use_pos_wp_avoidance()) {
+		execute_avoidance_position_waypoint();
+	}
+
 	/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
 	if (_run_pos_control) {
 
@@ -3372,6 +3380,14 @@ MulticopterPositionControl::set_idle_state()
 	_att_sp.thrust = 0.0f;
 }
 
+void
+MulticopterPositionControl::execute_avoidance_position_waypoint()
+{
+	_pos_sp(0) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::X];
+	_pos_sp(1) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::Y];
+	_pos_sp(2) = _traj_wp_avoidance.point_0[trajectory_waypoint_s::Z];
+}
+
 void
 MulticopterPositionControl::execute_avoidance_velocity_waypoint()
 {
@@ -3392,6 +3408,14 @@ MulticopterPositionControl::use_obstacle_avoidance()
 		TRAJECTORY_STREAM_TIMEOUT_US && (_traj_wp_avoidance.point_valid[trajectory_waypoint_s::POINT_0] == true));
 }
 
+bool
+MulticopterPositionControl::use_pos_wp_avoidance()
+{
+	return use_obstacle_avoidance() && PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::X]) &&
+	       PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::Y])
+	       && PX4_ISFINITE(_traj_wp_avoidance.point_0[trajectory_waypoint_s::Z]);
+}
+
 bool
 MulticopterPositionControl::use_vel_wp_avoidance()
 {