diff --git a/include/buzz_utility.h b/include/buzz_utility.h
index fb744e5..aa5cb2a 100644
--- a/include/buzz_utility.h
+++ b/include/buzz_utility.h
@@ -39,6 +39,7 @@ void update_users();
 int make_table(buzzobj_t* t);
 int buzzusers_add(int id, double latitude, double longitude, double altitude);
 int buzzusers_reset();
+	int compute_users_rb();
 
 void in_msg_append(uint64_t* payload);
 
diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h
index 2e62d32..e501285 100644
--- a/include/buzzuav_closures.h
+++ b/include/buzzuav_closures.h
@@ -9,6 +9,9 @@
 #include "buzz_utility.h"
 //#include "roscontroller.h"
 
+	#define EARTH_RADIUS (double) 6371000.0
+	#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
+
 namespace buzzuav_closures{
 	typedef enum {
       		COMMAND_NIL = 0,     // Dummy command
@@ -84,7 +87,7 @@ int buzzuav_update_battery(buzzvm_t vm);
  * Updates current position in Buzz
  */
 int buzzuav_update_currentpos(buzzvm_t vm);
-
+int buzzuav_adduserRB(buzzvm_t vm);
 /*
  * Updates flight status and rc command in Buzz, put it in a tabel to acess it 
  * use flight.status for flight status
diff --git a/include/roscontroller.h b/include/roscontroller.h
index b3adb2b..b9ac6ff 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -69,6 +69,7 @@ private:
         double DEFAULT_REFERENCE_LATITUDE  = 45.457817;
         double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
 
+	double target[3];
  	double cur_pos[3];
  	double target[3];
  	double home[3];
diff --git a/script/testflockfev.bzz b/script/testflockfev.bzz
index 92a4fc0..06b2e0e 100644
--- a/script/testflockfev.bzz
+++ b/script/testflockfev.bzz
@@ -17,7 +17,7 @@ CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
 TARGET     = 12.0	#0.000001001
-EPSILON    = 6.0	#0.001
+EPSILON    = 3.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -49,9 +49,12 @@ function hexagon() {
 #  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
 #  	timeW =0
 #  	statef=land
+#  	} else if(timeW>=WAIT_TIMEOUT/2) {
+#  	timeW = timeW+1
+#  	uav_moveto(0.06,0.0)
 #  	} else {
 #  	timeW = timeW+1
-#  	uav_moveto(0.0,0.0)
+#  	uav_moveto(0.0,0.06)
 #  	}
 }
  
@@ -143,10 +146,32 @@ function land() {
 	}
 }
 
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+function table_print(t) {
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
+}
+
 # Executed once at init time.
 function init() {
 	s = swarm.create(1)
 	s.join()
+
+	vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
+
 	statef=idle
 	CURSTATE = "IDLE"
 }
@@ -197,6 +222,14 @@ neighbors.listen("cmd",
 	statef()
 	log("Current state: ", CURSTATE)
 	log("Swarm size: ",ROBOTS)
+
+	# Check local users and push to v.stig
+	if(size(users.dataG)>0)
+		vt.put("p", users.dataG)
+				
+        # Save locally the users and print RG
+	users_save(vt.get("p"))
+	table_print(users.dataL)
 }
  
 # Executed once when the robot (or the simulator) is reset.
diff --git a/script/testflockfev.bzz.save b/script/testflockfev.bzz.save
new file mode 100644
index 0000000..4de66e5
--- /dev/null
+++ b/script/testflockfev.bzz.save
@@ -0,0 +1,216 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 12.0	#0.000001001
+EPSILON    = 6.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else if(timeW>=WAIT_TIMEOUT/2) {
+#  	timeW = timeW+1
+#  	uav_moveto(0.06,0.0)
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.06)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+	vt = stigmergy.cerate(5)
+	t = {}
+	vt.put("p",t)
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+	if(users.dataG)
+		vt.put("p",users.dataG)
+	table_print(users.dataL)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/script/testflocksim.bzz b/script/testflocksim.bzz
index 616de9c..48f47dd 100644
--- a/script/testflocksim.bzz
+++ b/script/testflocksim.bzz
@@ -141,10 +141,22 @@ function land() {
 	}
 }
 
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+# printing the contents of a table: a custom function
 function table_print(t) {
-  foreach(t, function(key, value) {
-      log(key, " -> ", value)
-    })
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
 }
 
 ########################################
@@ -157,10 +169,9 @@ function table_print(t) {
 function init() {
 	s = swarm.create(1)
 	s.join()
-	v = stigmergy.create(5)
+	vt = stigmergy.create(5)
 	t = {}
-	v.put("p",t)
-	v.put("u",1)
+	vt.put("p",t)
 	statef=idle
 	CURSTATE = "IDLE"
 }
@@ -211,22 +222,17 @@ neighbors.listen("cmd",
 	statef()
 	log("Current state: ", CURSTATE)
 	log("Swarm size: ",ROBOTS)
-	#log("User position: ", users.range)
 	
 	# Read a value from the structure
-        #t = v.get("p")
 				log(users)      
-				table_print(users)
-				if(size(users)>0){
-					tmp = {2 3}
-					v.put("p", tmp)
-					v.put("u",2)
-				}
+				#users_print(users.dataG)
+				if(size(users.dataG)>0)
+					vt.put("p", users.dataG)
 				
         # Get the number of keys in the structure
-        log("The vstig has ", v.size(), " elements")
-				table_print(v.get("p"))
-				log(v.get("u"))
+        log("The vstig has ", vt.size(), " elements")
+				users_save(vt.get("p"))
+				table_print(users.dataL)
 }
  
 # Executed once when the robot (or the simulator) is reset.
diff --git a/script/teststig.bzz b/script/teststig.bzz
index d3c4139..dc6c89e 100644
--- a/script/teststig.bzz
+++ b/script/teststig.bzz
@@ -23,7 +23,7 @@ function step() {
     log("The vstig has ", v.size(), " elements")
     log(v.get("u"))
     if (id==1) {
-        tmp = { }
+        tmp = { .x=3}
         v.put("p",tmp)
         v.put("u",2)
     }
diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz
new file mode 100644
index 0000000..1845eb9
--- /dev/null
+++ b/script/update_sim/testflockfev1.bzz
@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/script/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz
new file mode 100644
index 0000000..1845eb9
--- /dev/null
+++ b/script/update_sim/testflockfev2.bzz
@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/script/update_sim/testflockfev3.bzz b/script/update_sim/testflockfev3.bzz
new file mode 100644
index 0000000..1845eb9
--- /dev/null
+++ b/script/update_sim/testflockfev3.bzz
@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/script/update_sim/testflockfev4.bzz b/script/update_sim/testflockfev4.bzz
new file mode 100644
index 0000000..1845eb9
--- /dev/null
+++ b/script/update_sim/testflockfev4.bzz
@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/script/update_sim/testflockfev5.bzz b/script/update_sim/testflockfev5.bzz
new file mode 100644
index 0000000..1845eb9
--- /dev/null
+++ b/script/update_sim/testflockfev5.bzz
@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/script/update_sim/testflockfev6.bzz b/script/update_sim/testflockfev6.bzz
new file mode 100644
index 0000000..1845eb9
--- /dev/null
+++ b/script/update_sim/testflockfev6.bzz
@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/script/update_sim/testflockfev7.bzz b/script/update_sim/testflockfev7.bzz
new file mode 100644
index 0000000..1845eb9
--- /dev/null
+++ b/script/update_sim/testflockfev7.bzz
@@ -0,0 +1,208 @@
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+####################################################################################################
+# Updater related
+# This should be here for the updater to work, changing position of code will crash the updater
+####################################################################################################
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+neighbors.broadcast(updated, update_no)
+}
+
+TARGET_ALTITUDE = 3.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 10.0	#0.000001001
+EPSILON    = 18.0	#0.001
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  CURSTATE = "HEXAGON"
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  uav_moveto(accum.x,accum.y)
+
+#  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+#  	timeW =0
+#  	statef=land
+#  	} else {
+#  	timeW = timeW+1
+#  	uav_moveto(0.0,0.0)
+#  	}
+}
+ 
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+# ROBOTS = 3 # number of robots in the swarm
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	log("TakeOff: ", flight.status)
+	log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		barrier_set(ROBOTS,hexagon)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		timeW=0
+		barrier = nil
+		statef=idle
+	}
+}
+
+# Executed once at init time.
+function init() {
+	s = swarm.create(1)
+#	s.select(1)
+	s.join()
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed at each time step.
+function step() {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		uav_goto()
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+}
+ 
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+ 
+# Executed once at the end of experiment.
+function destroy() {
+}
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index dfe19d6..f1679c4 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -17,10 +17,10 @@ namespace buzz_utility{
 	static char*        BO_FNAME        = 0;
 	static uint8_t*     BO_BUF          = 0;
 	static buzzdebug_t  DBG_INFO        = 0;
-	static uint8_t      MSG_SIZE        = 50;   // Only 100 bytes of Buzz messages every step
+	static uint8_t      MSG_SIZE        = 250;   // Only 100 bytes of Buzz messages every step
 	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
-	static int 	    	Robot_id        = 0;
-
+	static int 	    Robot_id        = 0;
+	static std::vector<uint8_t*> IN_MSG;
 	std::map< int,  Pos_struct> users_map;
 	
 	/****************************************/
@@ -50,13 +50,9 @@ namespace buzz_utility{
 									(it->second).x,
 									(it->second).y,
 									(it->second).z);
-				buzzusers_add(it->first+1,
-									(it->second).x,
-									(it->second).y,
-									(it->second).z);
 			}
-		}else
-			ROS_INFO("[%i] No new users",Robot_id);
+		}/*else
+			ROS_INFO("[%i] No new users",Robot_id);*/
 	}
 
 	int buzzusers_reset() {
@@ -92,13 +88,13 @@ namespace buzz_utility{
 		buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
 		buzzobj_t nbr = buzzvm_stack_at(VM, 1);
 		/* Get "data" field */
-		buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
 		buzzvm_tget(VM);
 		if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
-			ROS_INFO("Empty data, create a new table");
+			//ROS_INFO("Empty data, create a new table");
 			buzzvm_pop(VM);
 			buzzvm_push(VM, nbr);
-			buzzvm_pushs(VM, buzzvm_string_register(VM, "data", 1));
+			buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
 			buzzvm_pusht(VM);
 			buzzobj_t data = buzzvm_stack_at(VM, 1);
 			buzzvm_tput(VM);
@@ -173,41 +169,55 @@ namespace buzz_utility{
 
 	void in_msg_append(uint64_t* payload){
 
-   		/* Go through messages and add them to the FIFO */
+   		/* Go through messages and append them to the vector */
    		uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
 		/*Size is at first 2 bytes*/
    		uint16_t size=data[0]*sizeof(uint64_t);
    		delete[] data;
    		uint8_t* pl =(uint8_t*)malloc(size);
-   		memset(pl, 0,size);
-   		/* Copy packet into temporary buffer */
-   		memcpy(pl, payload ,size);
-		/*size and robot id read*/
-   		size_t tot = sizeof(uint32_t);
-		/* Go through the messages until there's nothing else to read */
-      		uint16_t unMsgSize=0;
-		
-			/*Obtain Buzz messages only when they are present*/
-	      			do {
-		 			/* Get payload size */
-		 			unMsgSize = *(uint16_t*)(pl + tot);
-	   	 			tot += sizeof(uint16_t);
-		 			/* Append message to the Buzz input message queue */
-		 			if(unMsgSize > 0 && unMsgSize <= size - tot ) {
-		    			buzzinmsg_queue_append(VM,
-		                        buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
-		    			tot += unMsgSize;
-		 			}
-	      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
-		free(pl);
+		/* Copy packet into temporary buffer */
+	   	memcpy(pl, payload ,size);
+   		IN_MSG.push_back(pl);
    		
 	}
+	
+	void in_message_process(){
+		while(!IN_MSG.empty()){
+			uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); 
+			/* Go through messages and append them to the FIFO */
+   			uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]);
+			/*Size is at first 2 bytes*/
+   			uint16_t size=data[0]*sizeof(uint64_t);
+   			delete[] data;
+			/*size and robot id read*/
+	   		size_t tot = sizeof(uint32_t);
+			/* Go through the messages until there's nothing else to read */
+	      		uint16_t unMsgSize=0;
+				/*Obtain Buzz messages push it into queue*/
+		      			do {
+			 			/* Get payload size */
+			 			unMsgSize = *(uint16_t*)(first_INmsg + tot);
+		   	 			tot += sizeof(uint16_t);
+			 			/* Append message to the Buzz input message queue */
+			 			if(unMsgSize > 0 && unMsgSize <= size - tot ) {
+			    			buzzinmsg_queue_append(VM,
+				                buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
+			    			tot += unMsgSize;
+			 			}
+		      			}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
+			IN_MSG.erase(IN_MSG.begin());
+			free(first_INmsg);
+		}
+		/* Process messages VM call*/
+		buzzvm_process_inmsgs(VM);
+	}
 	/***************************************************/
 	/*Obtains messages from buzz out message Queue*/
 	/***************************************************/
 
    	uint64_t* obt_out_msg(){
-
+		/* Process out messages */
+		buzzvm_process_outmsgs(VM); 
    		uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
    		memset(buff_send, 0, MAX_MSG_SIZE);
 		/*Taking into consideration the sizes included at the end*/
@@ -314,6 +324,9 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
    		buzzvm_gstore(VM);
+   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_user_rb", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB));
+   		buzzvm_gstore(VM);
 
    	return VM->state;
 	}
@@ -350,12 +363,15 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
+   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_user_rb", 1));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
+   		buzzvm_gstore(VM);
 
    	return VM->state;
 	}
 
 static int create_stig_tables() {
-
+/*
    		// usersvstig = stigmergy.create(123)
         buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
         // get the stigmergy table from the global scope
@@ -400,6 +416,26 @@ static int create_stig_tables() {
 		buzzvm_call(VM, 0);
         buzzvm_gstore(VM);*/
 
+		buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_push(VM,t);
+		buzzvm_gstore(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
+		buzzvm_pusht(VM);
+		buzzobj_t data = buzzvm_stack_at(VM, 1);
+		buzzvm_tput(VM);
+		buzzvm_push(VM, data);
+		
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
+		buzzvm_pusht(VM);
+		data = buzzvm_stack_at(VM, 1);
+		buzzvm_tput(VM);
+		buzzvm_push(VM, data);
+
    	return VM->state;
 }
 	/****************************************/
@@ -454,7 +490,7 @@ static int create_stig_tables() {
       		ROS_ERROR("[%i] Error registering hooks", Robot_id);
       		return 0;
    	}
-   	/* Create vstig tables 
+   	/* Create vstig tables */
 	if(create_stig_tables() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
       		buzzdebug_destroy(&DBG_INFO);
@@ -462,12 +498,7 @@ static int create_stig_tables() {
 			//cout << "ERROR!!!!   ----------  " << VM->errormsg << endl;
 			//cout << "ERROR!!!!   ----------  " << buzzvm_strerror(VM) << endl;
       		return 0;
-   	}*/
-
-	buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
-	buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
-	buzzvm_push(VM, t);
-	buzzvm_gstore(VM);
+   	}
         
    	/* Save bytecode file name */
    	BO_FNAME = strdup(bo_filename);
@@ -638,8 +669,8 @@ static int create_stig_tables() {
 	}
 
 	void buzz_script_step() {
-		/* Process messages */
-		buzzvm_process_inmsgs(VM);
+		/*Process available messages*/
+		in_message_process();
 		/*Update sensors*/
 		update_sensors();
 		/* Call Buzz step() function */
@@ -649,8 +680,7 @@ static int create_stig_tables() {
 		      buzz_error_info());
 		buzzvm_dump(VM);
 		}
-		/* Process out messages */
-		buzzvm_process_outmsgs(VM); 
+		
 		/*Print swarm*/
 		//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
 		//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index a54aeca..c75357d 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -77,7 +77,7 @@ namespace buzzuav_closures{
 	/*----------------------------------------/
 	/ Compute GPS destination from current position and desired Range and Bearing
 	/----------------------------------------*/
-	#define EARTH_RADIUS (double) 6371000.0
+
 	void gps_from_rb(double  range, double bearing, double out[3]) {
 		double lat = cur_pos[0]*M_PI/180.0;
 		double lon = cur_pos[1]*M_PI/180.0;
@@ -88,6 +88,16 @@ namespace buzzuav_closures{
 		out[2] = height; //constant height.
 	}
 
+	void rb_from_gps(double nei[], double out[], double cur[]){   
+       	double d_lon = nei[1] - cur[1];
+       	double d_lat = nei[0] - cur[0];
+        double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
+        double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
+		out[1] = atan2(ned_y,ned_x);
+		out[2] = 0.0;
+	}
+
 	// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
 	double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
 	double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
@@ -108,8 +118,8 @@ namespace buzzuav_closures{
 	   float dx = buzzvm_stack_at(vm, 2)->f.value;
 	   double d = sqrt(dx*dx+dy*dy);	//range
 	   goto_pos[0]=dx;
-           goto_pos[1]=dy;
-           goto_pos[2]=height;
+       goto_pos[1]=dy;
+       goto_pos[2]=height;
 	   /*double b = atan2(dy,dx);		//bearing
 	   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
 	   gps_from_rb(d, b, goto_pos);
@@ -120,6 +130,69 @@ namespace buzzuav_closures{
 	   return buzzvm_ret0(vm);
 	}
 
+	int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
+		if(vm->state != BUZZVM_STATE_READY) return vm->state;
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
+		buzzvm_gload(vm);
+		buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
+		buzzobj_t nbr = buzzvm_stack_at(vm, 1);
+		/* Get "data" field */
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
+		buzzvm_tget(vm);
+		if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) {
+			//ROS_INFO("Empty data, create a new table");
+			buzzvm_pop(vm);
+			buzzvm_push(vm, nbr);
+			buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
+			buzzvm_pusht(vm);
+			buzzobj_t data = buzzvm_stack_at(vm, 1);
+			buzzvm_tput(vm);
+			buzzvm_push(vm, data);
+		}
+		/* When we get here, the "data" table is on top of the stack */
+		/* Push user id */
+		buzzvm_pushi(vm, id);
+		/* Create entry table */
+		buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
+		/* Insert range */
+		buzzvm_push(vm, entry);
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
+		buzzvm_pushf(vm, range);
+		buzzvm_tput(vm);
+		/* Insert longitude */
+		buzzvm_push(vm, entry);
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
+		buzzvm_pushf(vm, bearing);
+		buzzvm_tput(vm);
+		/* Save entry into data table */
+		buzzvm_push(vm, entry);
+		buzzvm_tput(vm);
+		//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
+		return vm->state;
+	}
+
+	int buzzuav_adduserRB(buzzvm_t vm) {
+	   buzzvm_lnum_assert(vm, 3);
+	   buzzvm_lload(vm, 1); /* longitude */
+	   buzzvm_lload(vm, 2); /* latitude */
+	   buzzvm_lload(vm, 3); /* id */
+	   buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
+	   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
+	   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
+	   double tmp[3];
+	   tmp[0] = buzzvm_stack_at(vm, 2)->f.value;
+	   tmp[1] = buzzvm_stack_at(vm, 1)->f.value;
+	   tmp[2] = 0.0;
+	   int uid = buzzvm_stack_at(vm, 3)->i.value;
+	   double rb[3];
+
+	   rb_from_gps(tmp, rb, cur_pos);
+
+	   //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
+
+	   return users_add2localtable(vm, uid, rb[0], rb[1]);
+	}
+
 	/*----------------------------------------/
 	/ Buzz closure to go directly to a GPS destination from the Mission Planner
 	/----------------------------------------*/
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index d7ae0ed..273c912 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -552,9 +552,6 @@ namespace rosbzz_node{
 	/ Compute Range and Bearing of a neighbor in a local reference frame
 	/ from GPS coordinates
 	----------------------------------------------------------- */
-	#define EARTH_RADIUS (double) 6371000.0
-	#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
-	
 	void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
             double hyp_az, hyp_el;
             double sin_el, cos_el, sin_az, cos_az;
@@ -775,8 +772,8 @@ namespace rosbzz_node{
 		moveMsg.header.frame_id = 1;
 		double local_pos[3];
 		cvt_ned_coordinates(cur_pos,local_pos,home);
-                ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
-                ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
+       //         ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
+       //         ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
                     
                 /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
         target[0]+=y;