diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index c766315..a0b1903 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -54,7 +54,7 @@ function launch() {
 function launch_switch() {
 	BVMSTATE = "LAUNCH_SWITCH"
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
-		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
+		homegps = {.lat=pose.position.latitude, .lng=pose.position.longitude}
 		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, AUTO_LAUNCH_STATE, 22)
 			barrier_ready(22)
@@ -74,11 +74,13 @@ gohomeT=100
 function goinghome() {
   BVMSTATE = "GOHOME"
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
-		if(gohomeT > 0) {	# TODO: Make a real check if home is reached
-			gohome()
-			gohomeT = gohomeT - 1
-		} else
-			BVMSTATE = AUTO_LAUNCH_STATE
+		storegoal(homegps.lat, homegps.lng, pose.position.altitude)
+		goto_gps(AUTO_LAUNCH_STATE)
+		#if(gohomeT > 0) {	# TODO: Make a real check if home is reached
+		#	gohome()
+		#	gohomeT = gohomeT - 1
+		#} else
+		#	BVMSTATE = AUTO_LAUNCH_STATE
 	}
 }
 
diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz
index 28a88d3..e060e57 100644
--- a/buzz_scripts/include/utils/conversions.bzz
+++ b/buzz_scripts/include/utils/conversions.bzz
@@ -33,7 +33,7 @@ function vec_from_gps(lat, lon, home_ref) {
   d_lon = lon - pose.position.longitude
   d_lat = lat - pose.position.latitude
   if(home_ref == 1) {
-	  d_lon = lon - homegps.long
+	  d_lon = lon - homegps.lng
 	  d_lat = lat - homegps.lat
   }
   ned_x = d_lat/180*math.pi * 6371000.0;
diff --git a/include/roscontroller.h b/include/roscontroller.h
index 599f116..0f2bc2f 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -118,7 +118,6 @@ private:
   std::map<int, buzz_utility::Pos_struct> targets_found;
   std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
   std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
-  int timer_step = 0;
   int robot_id = 0;
   ros::Time logical_clock;
   ros::Time previous_step_time;
@@ -222,7 +221,7 @@ private:
   void prepare_msg_and_publish();
 
   /*Refresh neighbours Position for every ten step*/
-  void maintain_pos(int tim_step);
+  void clear_pos();
 
   /*Puts neighbours position inside neigbours_pos_map*/
   void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index e1e086f..071336b 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -111,6 +111,7 @@ void roscontroller::RosControllerRun()
 /rosbuzz_node main loop method
 /--------------------------------------------------*/
 {
+  int timer_step = 0;
   //  Set the Buzz bytecode
   if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id))
   {
@@ -161,6 +162,12 @@ void roscontroller::RosControllerRun()
       
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();
+      //  Force a refresh on neighbors array once in a while
+      if (timer_step >= 20*BUZZRATE){
+        clear_pos();
+        timer_step = 0;
+      } else
+        timer_step++;
       //  Prepare messages and publish them
       prepare_msg_and_publish();
       // Call the flight controler service
@@ -185,9 +192,6 @@ void roscontroller::RosControllerRun()
         buzzuav_closures::rc_call(NAV_LAND);
       else
         fcu_timeout -= 1 / BUZZRATE;
-      timer_step += 1;
-      //  Force a refresh on neighbors array once in a while
-      maintain_pos(timer_step);
       //  DEBUG
       // std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
     }
@@ -941,19 +945,17 @@ script
   }
 }
 
-void roscontroller::maintain_pos(int tim_step)
+void roscontroller::clear_pos()
 /*
 /Refresh neighbours Position for every ten step
 /---------------------------------------------*/
 {
-  if (timer_step >= BUZZRATE)
-  {
+  
     neighbours_pos_map.clear();
+    raw_neighbours_pos_map.clear();
     buzzuav_closures::clear_neighbours_pos();
     // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
     // have to clear !
-    timer_step = 0;
-  }
 }
 
 void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)