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..822c391 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -84,7 +84,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/script/testflocksim.bzz b/script/testflocksim.bzz index 616de9c..79c58d8 100644 --- a/script/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -141,10 +141,13 @@ function land() { } } -function table_print(t) { - foreach(t, function(key, value) { - log(key, " -> ", value) - }) +function users_print(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) + }) + } } ######################################## @@ -157,10 +160,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 +213,18 @@ 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_print(vt.get("p")) } # 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/src/buzz_utility.cpp b/src/buzz_utility.cpp index dfe19d6..024227c 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ 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; @@ -57,6 +57,61 @@ namespace buzz_utility{ } }else ROS_INFO("[%i] No new users",Robot_id); + + //compute_users_rb(); + } + + int compute_users_rb() { + if(VM->state != BUZZVM_STATE_READY) return VM->state; + /* Get users "userG" stigmergy table */ + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); + buzzvm_pushi(VM, 1); + buzzvm_callc(VM); + buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE); + buzzobj_t nbr = buzzvm_stack_at(VM, 1); + buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1)); + buzzvm_tget(VM); + buzzvm_type_assert(VM, 1, BUZZTYPE_INT); + int gid = buzzvm_stack_at(VM, 1)->i.value; + ROS_WARN("GOT ID %i FROM V.STIG", gid); + /* Get "data" field */ + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_gload(VM); + 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, gid); + /* 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, 0); + buzzvm_tput(VM); + /* Insert bearing */ + buzzvm_push(VM, entry); + buzzvm_pushs(VM, buzzvm_string_register(VM, "b", 1)); + buzzvm_pushf(VM, 0); + buzzvm_tput(VM); + /* Save entry into data table */ + buzzvm_push(VM, entry); + buzzvm_tput(VM); + return VM->state; } int buzzusers_reset() { @@ -92,13 +147,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"); 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); @@ -314,6 +369,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,6 +408,9 @@ 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; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index afaf1a9..4f98aed 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -108,8 +108,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); @@ -119,6 +119,23 @@ namespace buzzuav_closures{ return buzzvm_ret0(vm); } + 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); + float lon = buzzvm_stack_at(vm, 1)->f.value; + float lat = buzzvm_stack_at(vm, 2)->f.value; + int uid = buzzvm_stack_at(vm, 3)->i.value; + + printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, lat, lon); + + return buzzvm_ret0(vm); + } + /*----------------------------------------/ / Buzz closure to go directly to a GPS destination from the Mission Planner /----------------------------------------*/