Merge remote-tracking branch 'refs/remotes/origin/dev'

Conflicts:
	src/roscontroller.cpp

include/buzz_update.h

@@ -129,6 +129,8 @@ int get_update_status();
 
 void set_read_update_status();
 
+int compile_bzz();
+
 void updates_set_robots(int robots);
 
 #endif

include/buzz_utility.h

@@ -21,12 +21,6 @@ struct pos_struct
   pos_struct(){}
 };
 
-#define function_register(TABLE, FNAME, FPOINTER)               \
-   buzzvm_push(VM, (TABLE));                                    \
-   buzzvm_pushs(VM, buzzvm_string_register(VM, (FNAME), 1));    \
-   buzzvm_pushcc(VM, buzzvm_function_register(VM, (FPOINTER))); \
-   buzzvm_tput(VM);
-
 typedef struct pos_struct Pos_struct ;
 
 uint16_t* u64_cvt_u16(uint64_t u64);

include/roscontroller.h

@@ -61,18 +61,16 @@ private:
 	  
 	}; typedef struct num_robot_count Num_robot_count ;
 
-        // WGS84 constants
+	struct gps
-        double equatorial_radius = 6378137.0;
+	{
-        double flattening = 1.0/298.257223563;
+		double longitude=0.0;
-        double excentrity2 = 2*flattening - flattening*flattening;
+		double latitude=0.0;
-        // default reference position
+		float  altitude=0.0;
-        double DEFAULT_REFERENCE_LATITUDE  = 45.457817;
+	}; typedef struct gps GPS ;
-        double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
 	
-	double target[3];
+	GPS target, home, cur_pos;
- 	double cur_pos[3];
- 	double home[3];
  	double cur_rel_altitude;
+
 	uint64_t payload;
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
 	std::map< int,  buzz_utility::Pos_struct> raw_neighbours_pos_map;
@@ -164,8 +162,12 @@ private:
 			 double longitude,
 			 double altitude);
 	/*convert from spherical to cartesian coordinate system callback */
-	void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
+	void gps_rb(GPS nei_pos, double out[]);
-	void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
+	void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
+	void gps_ned_home(float &ned_x, float &ned_y);
+	void gps_convert_ned(float &ned_x, float &ned_y,
+			double gps_t_lon, double gps_t_lat,
+			double gps_r_lon, double gps_r_lat);
 
 	/*battery status callback*/ 
 	void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);

script/stand_by.bzz

@@ -9,6 +9,8 @@ if(r. data < l. data or (r. data == l. data )) return l
 else return r
 })
 
+s = swarm.create(1)
+s.join()
 }
 
 function stand_by(){

script/test1.bzz

@@ -1,4 +1,3 @@
-
 # 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"

script/update_sim/include/string.bzz

@@ -0,0 +1,92 @@
+#
+# Returns the string character at the given position.
+# PARAM s: The string
+# PARAM n: The position of the wanted character
+# RETURN The character at the wanted position, or nil
+#
+string.charat = function(s, n) {
+  return string.sub(s, n, n+1)
+}
+
+#
+# Returns the index of the first occurrence of the given string m
+# within another string s. If none is found, this function returns
+# nil.
+# PARAM s: The string
+# PARAM m: The string to match
+# RETURN: The position of the first match, or nil
+#
+string.indexoffirst = function(s, m) {
+  var ls = string.length(s)
+  var lm = string.length(m)
+  var i = 0
+  while(i < ls-lm+1) {
+    if(string.sub(s, i, i+lm) == m) return i
+    i = i + 1
+  }
+  return nil
+}
+
+#
+# Returns the index of the last occurrence of the given string m
+# within another string s. If none is found, this function returns
+# nil.
+# PARAM s: The string
+# PARAM m: The string to match
+# RETURN: The position of the last match, or nil
+#
+string.indexoflast = function(s, m) {
+  var ls = string.length(s)
+  var lm = string.length(m)
+  var i = ls - lm + 1
+  while(i >= 0) {
+    if(string.sub(s, i, i+lm) == m) return i
+    i = i - 1
+  }
+  return nil
+}
+
+# Splits a string s using the delimiters in d. The string list is
+# returned in a table indexed by value (starting at 0).
+# PARAM s: The string
+# PARAM d: A string containing the delimiters
+# RETURN: A table containing the tokens
+string.split = function(s, d) {
+  var i1 = 0  # index to move along s (token start)
+  var i2 = 0  # index to move along s (token end)
+  var c = 0   # token count
+  var t = {}  # token list
+  var ls = string.length(s)
+  var ld = string.length(d)
+  # Go through string s
+  while(i2 < ls) {
+    # Try every delimiter
+    var j = 0   # index to move along d
+    var f = nil # whether the delimiter was found or not
+    while(j < ld and (not f)) {
+      if(string.charat(s, i2) == string.charat(d, j)) {
+        # Delimiter found
+        f = 1
+        # Is it worth adding a new token?
+        if(i2 > i1) {
+          t[c] = string.sub(s, i1, i2)
+          c = c + 1
+        }
+        # Start new token
+        i1 = i2 + 1
+      }
+      else {
+        # Next delimiter
+        j = j + 1
+      }
+    }
+    # Next string character
+    i2 = i2 + 1
+  }
+  # Is it worth adding a new token?
+  if(i2 > i1) {
+    t[c] = string.sub(s, i1, i2)
+  }
+  # Return token list
+  return t;
+}

script/update_sim/include/vec2.bzz

@@ -0,0 +1,107 @@
+#
+# Create a new namespace for vector2 functions
+#
+math.vec2 = {}
+
+#
+# Creates a new vector2.
+# PARAM x: The x coordinate.
+# PARAM y: The y coordinate.
+# RETURN: A new vector2.
+#
+math.vec2.new = function(x, y) {
+  return { .x = x, .y = y }
+}
+
+#
+# Creates a new vector2 from polar coordinates.
+# PARAM l: The length of the vector2.
+# PARAM a: The angle of the vector2.
+# RETURN: A new vector2.
+#
+math.vec2.newp = function(l, a) {
+  return {
+    .x = l * math.cos(a),
+    .y = l * math.sin(a)
+  }
+}
+
+#
+# Calculates the length of the given vector2.
+# PARAM v: The vector2.
+# RETURN: The length of the vector.
+#
+math.vec2.length = function(v) {
+  return math.sqrt(v.x * v.x + v.y * v.y)
+}
+
+#
+# Calculates the angle of the given vector2.
+# PARAM v: The vector2.
+# RETURN: The angle of the vector.
+#
+math.vec2.angle = function(v) {
+  return math.atan2(v.y, v.x)
+}
+
+#
+# Returns the normalized form of a vector2.
+# PARAM v: The vector2.
+# RETURN: The normalized form.
+#
+math.vec2.norm = function(v) {
+  var l = math.length(v)
+  return {
+    .x = v.x / l,
+    .y = v.y / l
+  }
+}
+
+#
+# Calculates v1 + v2.
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 + v2
+#
+math.vec2.add = function(v1, v2) {
+  return {
+    .x = v1.x + v2.x,
+    .y = v1.y + v2.y
+  }
+}
+
+#
+# Calculates v1 - v2.
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 + v2
+#
+math.vec2.sub = function(v1, v2) {
+  return {
+    .x = v1.x - v2.x,
+    .y = v1.y - v2.y
+  }
+}
+
+#
+# Scales a vector by a numeric constant.
+# PARAM v: A vector2.
+# PARAM s: A number (float or int).
+# RETURN: s * v
+#
+math.vec2.scale = function(v, s) {
+  return {
+    .x = v.x * s,
+    .y = v.y * s
+  }
+}
+
+#
+# Calculates v1 . v2 (the dot product)
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 . v2
+#
+math.vec2.dot = function(v1, v2) {
+  return v1.x * v2.x + v1.y * v2.y
+}

script/update_sim/old_testflockfev1.bzz

@@ -1,4 +1,5 @@
 # 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"
 ####################################################################################################

script/update_sim/testflockfev1.bzz

@@ -1,6 +1,6 @@
 # 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"
+include "vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater
@@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
-TARGET     = 10.0	#0.000001001
+TARGET     = 12.0	#0.000001001
-EPSILON    = 18.0	#0.001
+EPSILON    = 3.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -48,9 +48,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)
 #  	}
 }
  
@@ -142,11 +145,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.select(1)
 	s.join()
+
+	vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
+
 	statef=idle
 	CURSTATE = "IDLE"
 }
@@ -197,6 +221,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.

script/update_sim/testflockfev2.bzz

@@ -1,6 +1,6 @@
 # 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"
+include "vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater
@@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
-TARGET     = 10.0	#0.000001001
+TARGET     = 12.0	#0.000001001
-EPSILON    = 18.0	#0.001
+EPSILON    = 3.0	#0.001
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
@@ -48,9 +48,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)
 #  	}
 }
  
@@ -142,11 +145,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.select(1)
 	s.join()
+
+	vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
+
 	statef=idle
 	CURSTATE = "IDLE"
 }
@@ -197,6 +221,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.