added msg size + nei id + nei pos to log in sim

buzz_scripts/include/act/barrier.bzz

@@ -67,7 +67,7 @@ function barrier_wait(threshold, transf, resumef, bc) {
     barrier = nil
     timeW = 0
     BVMSTATE = resumef
-  } else if(timeW % 50 == 0 and bc > 0)
+  } else if(timeW % 100 == 0 and bc > 0)
     neighbors.broadcast("cmd", bc)
 
   timeW = timeW+1

buzz_scripts/include/act/states.bzz

@@ -269,7 +269,7 @@ function nei_cmd_listen() {
 						if(logical_time != nil) reinit_time_sync()
 						barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777)
 						barrier_ready(777)
-						neighbors.broadcast("cmd", 777)
+						#neighbors.broadcast("cmd", 777)
 					}else if(value==900){	# Shapes
 						barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
 						#barrier_ready(900)

buzz_scripts/include/timesync.bzz

@@ -6,7 +6,7 @@ logical_time = 0
 TIME_JUMP_THR = 5 
 TIME_TO_FORGET = 20
 TIME_TO_SYNC = 200
-COM_DELAY = 3
+COM_DELAY = 2 
 # table to store neighbor time data
 time_nei_table = {}
 # Algo. global parameters
@@ -44,6 +44,7 @@ function init_time_sync(){
 function step_time_sync(){
 	logical_time = logical_time + 1
 	sync_timer = sync_timer + 1
+  log("Logical time now ", logical_time)
 	if(sync_timer < TIME_TO_SYNC){
 		log(" SYNC ALGO ACTIVE time:", sync_timer)
 		cnt = 0
@@ -51,8 +52,7 @@ function step_time_sync(){
 		if(size(time_nei_table) > 0){
 		  	foreach(time_nei_table, function(key, value) {
 		    	if(value.time != 0){
-		    		#log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max)
-		    		var local_offset = value.time - logical_time - value.age
+		    		var local_offset = value.time - logical_time + value.age
 		    		if(local_offset > 0){
 		    			avg_offset = avg_offset + 1 * local_offset
 		    			cnt = cnt + 1
@@ -80,10 +80,8 @@ function step_time_sync(){
 	    jumped = 0
 	    syncError=0
 	    var mstr = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) }
-	    #string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical))
 	    neighbors.broadcast("time_sync",mstr)
 	}
-	log("Logical time now ", logical_time)
 }
 
 # Function to set sync timer to zero and reinitiate sync. algo

include/buzz_utility.h

@@ -78,6 +78,8 @@ void set_robot_var(int ROBOTS);
 
 int get_inmsg_size();
 
+std::vector<uint8_t*> get_inmsg_vector();
+
 std::string getuavstate();
 
 int getlogicaltime();

src/buzz_utility.cpp

@@ -565,6 +565,10 @@ int get_inmsg_size()
   return IN_MSG.size();
 }
 
+std::vector<uint8_t*> get_inmsg_vector(){
+  return IN_MSG;
+} 
+
 string getuavstate()
 /*
 / return current BVM state

src/roscontroller.cpp

@@ -158,7 +158,26 @@ void roscontroller::RosControllerRun()
       log<<cur_pos.latitude << "," << cur_pos.longitude << ","
             << cur_pos.altitude << ",";
       log << (int)no_of_robots<<",";
-      log <<(int)buzz_utility::get_inmsg_size()<<",";
+      map<int, buzz_utility::Pos_struct>::iterator it =
+            neighbours_pos_map.begin();
+      log  << neighbours_pos_map.size()<< ",";
+      for (; it != neighbours_pos_map.end(); ++it)
+      {
+        log << (double)it->second.x << "," << (double)it->second.y
+            << "," << (double)it->second.z << ",";
+      }
+      std::vector<uint8_t*> in_msg = buzz_utility::get_inmsg_vector();
+      log <<(int)in_msg.size()<<",";
+      for (std::vector<uint8_t*>::iterator it = in_msg.begin() ; it != in_msg.end(); ++it){
+        uint8_t* first_INmsg = (uint8_t*)(*it);
+        size_t tot = 0;
+        //  Size is at first 2 bytes
+        uint16_t msg_size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
+        tot += sizeof(uint16_t);
+        //  Decode neighbor Id
+        uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
+        log<<(int)neigh_id<<","<<(int)msg_size<<",";
+      }
       log <<buzz_utility::getuavstate()<<std::endl;
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();