first working implementation of users vstig

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);
 

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

script/testflocksim.bzz

@@ -141,10 +141,13 @@ function land() {
 	}
 }
 
-function table_print(t) {
+function users_print(t) {
-  foreach(t, function(key, value) {
+	if(size(t)>0) {
-      log(key, " -> ", value)
+		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)
+	vt.put("p",t)
-	v.put("u",1)
 	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}
+					#users_print(users.dataG)
-					v.put("p", tmp)
+					if(size(users.dataG)>0)
-					v.put("u",2)
+						vt.put("p", users.dataG)
 				}
 				
         # Get the number of keys in the structure
-        log("The vstig has ", v.size(), " elements")
+        log("The vstig has ", vt.size(), " elements")
-				table_print(v.get("p"))
+				users_print(vt.get("p"))
-				log(v.get("u"))
 }
  
 # Executed once when the robot (or the simulator) is reset.

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)
     }

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;
 	}

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[1]=dy;
-           goto_pos[2]=height;
+       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
 	/----------------------------------------*/