From 0d2304116092196feca6320e38527089a987be88 Mon Sep 17 00:00:00 2001
From: vivek-shankar <vivek-shankar.varadharajan@polymtl.ca>
Date: Thu, 4 Oct 2018 16:06:45 -0400
Subject: [PATCH] Local setpoint fix for the solos.

---
 src/roscontroller.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index e401299..c27bd31 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -164,7 +164,7 @@ void roscontroller::RosControllerRun()
       // Call the flight controler service
       flight_controller_service_call();
       // Broadcast local position to FCU
-      if(BClpose)
+      if(BClpose && !setmode)
 	      SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       //  Set ROBOTS variable (swarm size)
       get_number_of_robots();
@@ -838,6 +838,8 @@ script
 
     case NAV_SPLINE_WAYPOINT:
       goto_pos = buzzuav_closures::getgoto();
+      if(setmode) 
+        SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       break;
 
     case DO_MOUNT_CONTROL: