From 0694abf916c967ddcf694fa49d5611fae4dfef18 Mon Sep 17 00:00:00 2001
From: Martina <martina@rivizzigno.it>
Date: Mon, 16 Jul 2018 15:26:17 +0200
Subject: [PATCH] mc_pos_control: do not execute updateSetpoint twice if
 obstacle avoidance active

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

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