From 197ddd3e4633828fd29313fb0eb305a69b7848da Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 13 Feb 2019 13:04:44 +0100
Subject: [PATCH] mc_pos_control - Publish trajectory setpoint directly after
 the FlightTask update

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

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 40cdc24b05..237f3d53bd 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -686,6 +686,8 @@ MulticopterPositionControl::run()
 				}
 			}
 
+			publish_trajectory_sp(setpoint);
+
 			/* desired waypoints for obstacle avoidance:
 			 * point_0 contains the current position with the desired velocity
 			 * point_1 contains _pos_sp_triplet.current if valid
@@ -758,7 +760,6 @@ MulticopterPositionControl::run()
 			// Generate desired thrust and yaw.
 			_control.generateThrustYawSetpoint(_dt);
 
-			publish_trajectory_sp(setpoint);
 
 			// Fill local position, velocity and thrust setpoint.
 			// This message contains setpoints where each type of setpoint is either the input to the PositionController
-- 
GitLab