diff --git a/include/buzz_update.h b/include/buzz_update.h index c8bbaf0..9c51601 100644 --- a/include/buzz_update.h +++ b/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 diff --git a/include/buzz_utility.h b/include/buzz_utility.h index aa5cb2a..381b417 100644 --- a/include/buzz_utility.h +++ b/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); diff --git a/include/roscontroller.h b/include/roscontroller.h index 5cc6e63..6dc1859 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -52,7 +52,7 @@ public: void RosControllerRun(); private: - struct num_robot_count + struct num_robot_count { uint8_t history[10]; uint8_t index=0; @@ -61,18 +61,16 @@ private: }; typedef struct num_robot_count Num_robot_count ; - // WGS84 constants - double equatorial_radius = 6378137.0; - double flattening = 1.0/298.257223563; - double excentrity2 = 2*flattening - flattening*flattening; - // default reference position - double DEFAULT_REFERENCE_LATITUDE = 45.457817; - double DEFAULT_REFERENCE_LONGITUDE = -73.636075; - - double target[3]; - double cur_pos[3]; - double home[3]; + struct gps + { + double longitude=0.0; + double latitude=0.0; + float altitude=0.0; + }; typedef struct gps GPS ; + + GPS target, home, cur_pos; 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; @@ -159,13 +157,17 @@ private: /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); - /*Set the current position of the robot callback*/ + /*Set the current position of the robot callback*/ void set_cur_pos(double latitude, 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 cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]); + void gps_rb(GPS nei_pos, double out[]); + 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); @@ -178,7 +180,7 @@ private: /*current position callback*/ void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); - void users_pos(const rosbuzz::neigh_pos msg); + void users_pos(const rosbuzz::neigh_pos msg); /*current relative altitude callback*/ void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); diff --git a/script/stand_by.bzz b/script/stand_by.bzz index bf2ac78..d51e99e 100644 --- a/script/stand_by.bzz +++ b/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(){ diff --git a/script/test1.bzz b/script/test1.bzz index 3e9d1fb..7debdf4 100644 --- a/script/test1.bzz +++ b/script/test1.bzz @@ -1,4 +1,3 @@ - # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." include "/home/ubuntu/buzz/src/include/vec2.bzz" diff --git a/script/update_sim/include/string.bzz b/script/update_sim/include/string.bzz new file mode 100644 index 0000000..4140a1b --- /dev/null +++ b/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; +} diff --git a/script/update_sim/include/vec2.bzz b/script/update_sim/include/vec2.bzz new file mode 100644 index 0000000..e2fb9b0 --- /dev/null +++ b/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 +} diff --git a/script/update_sim/testflockfev4.bzz b/script/update_sim/old_testflockfev1.bzz similarity index 99% rename from script/update_sim/testflockfev4.bzz rename to script/update_sim/old_testflockfev1.bzz index 1845eb9..02abd4c 100644 --- a/script/update_sim/testflockfev4.bzz +++ b/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 ..." include "/home/ubuntu/buzz/src/include/vec2.bzz" #################################################################################################### diff --git a/script/update_sim/testflockfev3.bzz b/script/update_sim/old_testflockfev2.bzz similarity index 100% rename from script/update_sim/testflockfev3.bzz rename to script/update_sim/old_testflockfev2.bzz diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz index 1845eb9..06b3e32 100644 --- a/script/update_sim/testflockfev1.bzz +++ b/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 ..." -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 -EPSILON = 18.0 #0.001 +TARGET = 12.0 #0.000001001 +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. diff --git a/script/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz index 1845eb9..06b3e32 100644 --- a/script/update_sim/testflockfev2.bzz +++ b/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 ..." -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 -EPSILON = 18.0 #0.001 +TARGET = 12.0 #0.000001001 +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.