merging dji_mav_mist

buzz_scripts/graphform.bzz

@@ -6,6 +6,7 @@ include "vec2.bzz"
 include "update.bzz"
 include "barrier.bzz"	# don't use a stigmergy id=11 with this header.
 include "uavstates.bzz"	# require an 'action' function to be defined here.
+include "vstigenv.bzz"
 
 include "graphs/shapes_L.bzz"
 
@@ -14,7 +15,7 @@ ROBOT_DIAMETER=2.0*ROBOT_RADIUS
 ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
 
 # max velocity in cm/step
-ROBOT_MAXVEL = 500.0
+ROBOT_MAXVEL = 200.0
 
 #
 # Global variables
@@ -808,6 +809,7 @@ function init() {
 	#
 	uav_initswarm()
     v_tag = stigmergy.create(m_lockstig)
+  	uav_initstig()
 	Reset()
 	TARGET_ALTITUDE = 2.5 + id * 1.5
 }
@@ -818,6 +820,7 @@ function init() {
 function step(){
 	uav_rccmd()
 	uav_neicmd()
+  	uav_updatestig()
 	#update the graph
 	UpdateNodeInfo()
 	#reset message package to be sent
@@ -846,8 +849,10 @@ function step(){
 	debug(m_eState,m_nLabel)
 	log("Current state: ", UAVSTATE)
 	log("Swarm size: ", ROBOTS)
+	if(id==0)
+    	stattab_print()
+	
 	#navigation
-
 	#broadcast messag
 	neighbors.broadcast("m",packmessage(m_selfMessage))
 

buzz_scripts/include/vstigenv.bzz

@@ -3,6 +3,7 @@ GROUND_VSTIG = 11
 WAIT4STEP = 10
 v_status = {}
 v_ground = {}
+b_updating = 0
 
 function uav_initstig() {
 	v_status = stigmergy.create(STATUS_VSTIG)
@@ -11,29 +12,48 @@ function uav_initstig() {
 
 counter=WAIT4STEP
 function uav_updatestig() {
+	# TODO: Push values on update only.
 	if(counter<=0) {
-		var ls={.g=0,.b=battery.capacity,.x=xbee_status.rssi,.f=flight.status}
+		b_updating=1
+		#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status}
+		ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status
 	    log("Pushing ", ls, "on vstig with id:", id)
 	    v_status.put(id, ls)
 		counter=WAIT4STEP
-	} else
+	} else {
+		b_updating=0
 		counter=counter-1
+	}
+}
+
+function unpackstatus(recv_value){
+    gps=(recv_value-recv_value%1000000)/1000000
+    recv_value=recv_value-gps*1000000
+    batt=(recv_value-recv_value%1000)/1000
+    recv_value=recv_value-batt*1000
+    xbee=(recv_value-recv_value%10)/10
+    recv_value=recv_value-xbee*10
+    fc=recv_value
+	log("- GPS ", gps)
+	log("- Battery ", batt)
+	log("- Xbee ", xbee)
+	log("- Status ", fc)
 }
 	
 function checkusers() {
-    	# Read a value from the structure
+	# Read a value from the structure
-        if(size(users)>0)
+	if(size(users)>0)
-            log("Got a user!")
+		log("Got a user!")
 
-#		log(users)      
+#	log(users)      
-#		users_print(users.dataG)
+#	users_print(users.dataG)
-#		if(size(users.dataG)>0)
+#	if(size(users.dataG)>0)
-#			vt.put("p", users.dataG)
+#		vt.put("p", users.dataG)
-				
+			
-        # Get the number of keys in the structure
+	# Get the number of keys in the structure
-        #log("The vstig has ", vt.size(), " elements")
+#	log("The vstig has ", vt.size(), " elements")
-	#			users_save(vt.get("p"))
+#	users_save(vt.get("p"))
-	#			table_print(users.dataL)
+#	table_print(users.dataL)
 }
 
 function users_save(t) {
@@ -56,17 +76,14 @@ function usertab_print(t) {
 
 function stattab_print() {
 	if(v_status.size()>0) {
-		u=0
+		if(b_updating==0) {
-		while(u<8){
+			u=0
-			tab = v_status.get(u)
+			while(u<8){
-			if(tab!=nil) {
+				tab = v_status.get(u)
-				log("id: ", u)
+				if(tab!=nil)
-				log("- GPS ", tab.g)
+					unpackstatus(tab)
-				log("- Battery ", tab.b)
+				u=u+1
-				log("- Xbee ", tab.x)
-				log("- Status ", tab.f)
 			}
-			u=u+1
 		}
 	}
 }

src/buzzuav_closures.cpp

@@ -25,9 +25,9 @@ namespace buzzuav_closures{
 	static int buzz_cmd=0;
 	static float height=0;
   static bool deque_full = false;
-  static float rssi = 0.0;
+  static int rssi = 0;
   static float raw_packet_loss = 0.0;
-  static float filtered_packet_loss = 0.0;
+  static int filtered_packet_loss = 0;
   static float api_rssi = 0.0;
 
 	std::map< int,  buzz_utility::RB_struct> targets_map;
@@ -247,7 +247,7 @@ namespace buzzuav_closures{
 	}
 
 	/*---------------------------------------/
-	/ Buss closure for basic UAV commands
+	/ Buzz closure for basic UAV commands
 	/---------------------------------------*/
 	int buzzuav_takeoff(buzzvm_t vm) {
 	   buzzvm_lnum_assert(vm, 1);
@@ -334,7 +334,7 @@ namespace buzzuav_closures{
 	   buzzvm_tput(vm);
 	   buzzvm_dup(vm);
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
-	   buzzvm_pushf(vm, batt[2]);
+	   buzzvm_pushi(vm, (int)batt[2]);
 	   buzzvm_tput(vm);
 	   buzzvm_gstore(vm);
 	   return vm->state;
@@ -347,7 +347,7 @@ namespace buzzuav_closures{
 
   void set_rssi(float value)
   {
-   rssi = value;
+   rssi = round(value);
   }
 
   void set_raw_packet_loss(float value)
@@ -357,7 +357,7 @@ namespace buzzuav_closures{
 
   void set_filtered_packet_loss(float value)
   {
-   filtered_packet_loss = value;
+   filtered_packet_loss = round(100*value);
   }
 
   void set_api_rssi(float value)
@@ -374,7 +374,7 @@ namespace buzzuav_closures{
      buzzvm_tput(vm);
 	   buzzvm_dup(vm);
      buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
-     buzzvm_pushf(vm, rssi);
+     buzzvm_pushi(vm, rssi);
      buzzvm_tput(vm);
 	   buzzvm_dup(vm);
      buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
@@ -382,7 +382,7 @@ namespace buzzuav_closures{
      buzzvm_tput(vm);
 	   buzzvm_dup(vm);
      buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
-     buzzvm_pushf(vm, filtered_packet_loss);
+     buzzvm_pushi(vm, filtered_packet_loss);
      buzzvm_tput(vm);
 	   buzzvm_dup(vm);
      buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));