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 292323c078..b3d5dc14c9 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -654,9 +654,11 @@ MulticopterPositionControl::task_main()
 			// Update states, setpoints and constraints.
 			_control.updateConstraints(constraints);
 			_control.updateState(_states);
-			_control.updateSetpoint(setpoint);
 
-			if (use_obstacle_avoidance()) {
+			if (!use_obstacle_avoidance()) {
+				_control.updateSetpoint(setpoint);
+
+			} else {
 				execute_avoidance_waypoint();
 			}