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] 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;