From b2ab2ea9ec5b0094b76db0c8cbe292578921c3ef Mon Sep 17 00:00:00 2001
From: mistSpiri_Valmiki <vivekshankarv@gmail.com>
Date: Thu, 27 Sep 2018 12:18:43 -0400
Subject: [PATCH 1/7] fixes from spiri HITL

---
 buzz_scripts/include/act/states.bzz |  2 +-
 buzz_scripts/main.bzz               |  4 ++--
 include/roscontroller.h             |  2 ++
 src/roscontroller.cpp               | 31 ++++++++++++++++++++++-------
 4 files changed, 29 insertions(+), 10 deletions(-)

diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 78e9285..7b20410 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -77,7 +77,7 @@ function stop() {
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		neighbors.broadcast("cmd", 21)
 		uav_land()
-		if(pose.position.altitude <= 0.3) {
+		if(pose.position.altitude <= 0.2) {
 			BVMSTATE = "TURNEDOFF"
 			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
 			#barrier_ready(21)
diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index fe36fc3..427e61a 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -32,11 +32,11 @@ goal_list = {
 function init() {
   	init_stig()
 	init_swarm()
-
+	
 #	initGraph()
 
 	TARGET_ALTITUDE = takeoff_heights[id]
-
+	log("----------------------->alt",TARGET_ALTITUDE)
 	# start the swarm command listener
 	nei_cmd_listen()
 
diff --git a/include/roscontroller.h b/include/roscontroller.h
index e79c589..74f12ef 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -98,6 +98,7 @@ private:
   typedef struct POSE ros_pose;
 
   ros_pose target, home, cur_pos;
+  double* goto_pos;
 
   struct MsgData
   {
@@ -141,6 +142,7 @@ private:
   uint8_t old_val = 0;
   bool debug = false;
   bool setmode = false;
+  bool BClpose = false;
   std::string bzzfile_name;
   std::string bcfname, dbgfname;
   std::string stand_by;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 23fc145..fa0ac58 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -39,6 +39,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   cur_pos.longitude = 0;
   cur_pos.latitude = 0;
   cur_pos.altitude = 0;
+  goto_pos = buzzuav_closures::getgoto();
 
   // set stream rate - wait for the FC to be started
   SetStreamRate(0, 10, 1);
@@ -162,6 +163,9 @@ void roscontroller::RosControllerRun()
       prepare_msg_and_publish();
       // Call the flight controler service
       flight_controller_service_call();
+      // Broadcast local position to FCU
+      if(BClpose)
+	SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       //  Set ROBOTS variable (swarm size)
       get_number_of_robots();
       buzz_utility::set_robot_var(no_of_robots);
@@ -737,12 +741,13 @@ script
 /-------------------------------------------------------------------------------*/
 {
   //  flight controller client call if requested from Buzz
-  double* goto_pos;
   float* gimbal;
   switch (buzzuav_closures::bzz_cmd())
   {
     case NAV_TAKEOFF:
       goto_pos = buzzuav_closures::getgoto();
+      goto_pos[0] = cur_pos.x;
+      goto_pos[1] = cur_pos.y;
       cmd_srv.request.param5 = cur_pos.latitude;
       cmd_srv.request.param6 = cur_pos.longitude;
       cmd_srv.request.param7 = goto_pos[2];
@@ -756,15 +761,19 @@ script
         ros::Duration(0.5).sleep();
         // Registering HOME POINT.
         home = cur_pos;
+	BClpose = true;
       }
       if (current_mode != "GUIDED" && setmode)
         SetMode("GUIDED", 2000);  // added for compatibility with 3DR Solo
-      if (mav_client.call(cmd_srv))
+      if(setmode)
       {
-        ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+        if (mav_client.call(cmd_srv))
+        {
+          ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
+        }
+        else
+          ROS_ERROR("Failed to call service from flight controller");
       }
-      else
-        ROS_ERROR("Failed to call service from flight controller");
       break;
 
     case NAV_LAND:
@@ -778,10 +787,11 @@ script
         armstate = 0;
         Arm();
       }
-      else if(cur_pos.altitude < 0.3) //disarm only when close to ground
+      else if(cur_pos.altitude < 0.4) //disarm only when close to ground
       {
         armstate = 0;
         Arm();
+	BClpose = false;
       }
       if (mav_client.call(cmd_srv))
       {
@@ -829,7 +839,6 @@ script
 
     case NAV_SPLINE_WAYPOINT:
       goto_pos = buzzuav_closures::getgoto();
-      roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       break;
 
     case DO_MOUNT_CONTROL:
@@ -1005,6 +1014,14 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
   cur_pos.x = msg->pose.position.x;
   cur_pos.y = msg->pose.position.y;
 
+  if(!BClpose)
+  {
+  goto_pos[0]=cur_pos.x;
+  goto_pos[1]=cur_pos.y;
+  goto_pos[2]=0.0;
+  BClpose = true;
+  }
+
   buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
   //  cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
   tf::Quaternion q(

From b38b9df043b14e243fb71540420ff8efe38615f2 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Thu, 27 Sep 2018 12:30:46 -0400
Subject: [PATCH 2/7] more detailed log far WP

---
 buzz_scripts/include/act/states.bzz | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 7b20410..edce3b4 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -105,7 +105,7 @@ function goto_gps(transf) {
 	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
 	#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
  	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
-		log("Sorry this is too far.")
+		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
 	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
 		transf()
 	else {

From b7d3a0c3ac613f43310d4a01da7bf57b1914ee07 Mon Sep 17 00:00:00 2001
From: mistSpiri_Valmiki <vivekshankarv@gmail.com>
Date: Thu, 11 Feb 2016 11:40:13 -0500
Subject: [PATCH 3/7] minor fixes for the spiri

---
 buzz_scripts/include/act/states.bzz | 4 ++--
 buzz_scripts/main.bzz               | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index edce3b4..9b137d3 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -12,7 +12,7 @@ TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
 
-GOTO_MAXVEL = 0.85 # m/steps
+GOTO_MAXVEL = 1.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
@@ -35,7 +35,7 @@ function cusfun(){
 	log("cusfun: yay!!!")
 	LOCAL_TARGET = math.vec2.new(5 ,0 )
 	local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
-	if(math.vec2.length(local_set_point) > GOTODIST_TOL){
+    if(math.vec2.length(local_set_point) > GOTODIST_TOL){
 		local_set_point = LimitSpeed(local_set_point, 1.0)
 		goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
     }
diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index 427e61a..ca87ef9 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -9,6 +9,7 @@ include "vstigenv.bzz"
 include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
+
 AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
 TARGET     = 9.0
 EPSILON    = 30.0

From 0f54378b269e116bc9c04fd6ec45c505691392ff Mon Sep 17 00:00:00 2001
From: mistSpiri_Valmiki <vivekshankarv@gmail.com>
Date: Thu, 11 Feb 2016 11:32:52 -0500
Subject: [PATCH 4/7] changes made on the field

---
 buzz_scripts/main.bzz | 3 ++-
 src/roscontroller.cpp | 9 +++++----
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index ca87ef9..7791ccc 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -10,7 +10,8 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
+#AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
+AUTO_LAUNCH_STATE = "IDLE"
 TARGET     = 9.0
 EPSILON    = 30.0
 ROOT_ID = 3
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index fa0ac58..fbcc069 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -746,19 +746,20 @@ script
   {
     case NAV_TAKEOFF:
       goto_pos = buzzuav_closures::getgoto();
-      goto_pos[0] = cur_pos.x;
-      goto_pos[1] = cur_pos.y;
+      goto_pos[0] = 0.0;
+      goto_pos[1] = 0.0;
       cmd_srv.request.param5 = cur_pos.latitude;
       cmd_srv.request.param6 = cur_pos.longitude;
       cmd_srv.request.param7 = goto_pos[2];
       cmd_srv.request.command = buzzuav_closures::getcmd();
       if (!armstate)
       {
-        if(setmode)
+        if(setmode){
           SetMode("LOITER", 0);
+          ros::Duration(0.5).sleep();
+        }
         armstate = 1;
         Arm();
-        ros::Duration(0.5).sleep();
         // Registering HOME POINT.
         home = cur_pos;
 	BClpose = true;

From 1da2084419dde143323c3bfbdd1fce3c3de5e1ef Mon Sep 17 00:00:00 2001
From: Rafael Braga <faerugb@gmail.com>
Date: Thu, 27 Sep 2018 19:44:18 -0400
Subject: [PATCH 5/7] Revert "changes made on the field"

This reverts commit 5654274acf8db38896cfafe41b10a592bc602284
---
 buzz_scripts/main.bzz | 3 +--
 src/roscontroller.cpp | 9 ++++-----
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index 7791ccc..ca87ef9 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -10,8 +10,7 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-#AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
-AUTO_LAUNCH_STATE = "IDLE"
+AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
 TARGET     = 9.0
 EPSILON    = 30.0
 ROOT_ID = 3
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index fbcc069..fa0ac58 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -746,20 +746,19 @@ script
   {
     case NAV_TAKEOFF:
       goto_pos = buzzuav_closures::getgoto();
-      goto_pos[0] = 0.0;
-      goto_pos[1] = 0.0;
+      goto_pos[0] = cur_pos.x;
+      goto_pos[1] = cur_pos.y;
       cmd_srv.request.param5 = cur_pos.latitude;
       cmd_srv.request.param6 = cur_pos.longitude;
       cmd_srv.request.param7 = goto_pos[2];
       cmd_srv.request.command = buzzuav_closures::getcmd();
       if (!armstate)
       {
-        if(setmode){
+        if(setmode)
           SetMode("LOITER", 0);
-          ros::Duration(0.5).sleep();
-        }
         armstate = 1;
         Arm();
+        ros::Duration(0.5).sleep();
         // Registering HOME POINT.
         home = cur_pos;
 	BClpose = true;

From 92f7a8d1e66ab432567b333d095fee91ebd36211 Mon Sep 17 00:00:00 2001
From: mistSpiri_Vachkiri <faerugb@gmail.com>
Date: Thu, 27 Sep 2018 19:51:09 -0400
Subject: [PATCH 6/7] changes made on the field 27/09

---
 buzz_scripts/main.bzz | 3 ++-
 src/roscontroller.cpp | 9 +++++----
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index ca87ef9..7791ccc 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -10,7 +10,8 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
+#AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
+AUTO_LAUNCH_STATE = "IDLE"
 TARGET     = 9.0
 EPSILON    = 30.0
 ROOT_ID = 3
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index fa0ac58..fbcc069 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -746,19 +746,20 @@ script
   {
     case NAV_TAKEOFF:
       goto_pos = buzzuav_closures::getgoto();
-      goto_pos[0] = cur_pos.x;
-      goto_pos[1] = cur_pos.y;
+      goto_pos[0] = 0.0;
+      goto_pos[1] = 0.0;
       cmd_srv.request.param5 = cur_pos.latitude;
       cmd_srv.request.param6 = cur_pos.longitude;
       cmd_srv.request.param7 = goto_pos[2];
       cmd_srv.request.command = buzzuav_closures::getcmd();
       if (!armstate)
       {
-        if(setmode)
+        if(setmode){
           SetMode("LOITER", 0);
+          ros::Duration(0.5).sleep();
+        }
         armstate = 1;
         Arm();
-        ros::Duration(0.5).sleep();
         // Registering HOME POINT.
         home = cur_pos;
 	BClpose = true;

From e5e7500b20770df29d594a7d07d1cd8e9f6ee6f9 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Thu, 27 Sep 2018 23:59:54 -0400
Subject: [PATCH 7/7] Revert debug stuff from field in main.bzz

---
 buzz_scripts/main.bzz | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index 7791ccc..836bf81 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -10,8 +10,7 @@ include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
 
-#AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
-AUTO_LAUNCH_STATE = "IDLE"
+AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
 TARGET     = 9.0
 EPSILON    = 30.0
 ROOT_ID = 3
@@ -38,7 +37,7 @@ function init() {
 #	initGraph()
 
 	TARGET_ALTITUDE = takeoff_heights[id]
-	log("----------------------->alt",TARGET_ALTITUDE)
+	
 	# start the swarm command listener
 	nei_cmd_listen()