Time sync algo and rc cmd implemeantation on sim

buzz_scripts/include/act/states.bzz

@@ -143,7 +143,8 @@ function pursuit() {
   if(neighbors.count() > 0)
     r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
 	r = math.vec2.length(r_vec)
-	gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
+	var sqr = (r-rd)*(r-rd)+pc*pc*r*r
+    gamma = vd / math.sqrt(sqr)
 	dr = -gamma * (r-rd)
 	dT = gamma * pc
   vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
@@ -214,7 +215,13 @@ function rc_cmd_listen() {
 	} else if (flight.rc_cmd==666){
 		flight.rc_cmd=0
 		stattab_send()
-	} else if (flight.rc_cmd==900){
+	} else if (flight.rc_cmd==777){
+		flight.rc_cmd=0
+		if(logical_time != nil) reinit_time_sync()
+		barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777)
+		barrier_ready(777)
+		neighbors.broadcast("cmd", 777)
+	}else if (flight.rc_cmd==900){
 		flight.rc_cmd=0
 		barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
 		barrier_ready(900)
@@ -258,7 +265,12 @@ function nei_cmd_listen() {
 						arm()
 					} else if(value==401 and BVMSTATE=="TURNEDOFF"){
 						disarm()
-					} else if(value==900){	# Shapes
+					} else if(value==777 and BVMSTATE=="TURNEDOFF"){
+						if(logical_time != nil) reinit_time_sync()
+						barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777)
+						barrier_ready(777)
+						neighbors.broadcast("cmd", 777)
+					}else if(value==900){	# Shapes
 						barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
 						#barrier_ready(900)
 						neighbors.broadcast("cmd", 900)

buzz_scripts/include/timesync.bzz

@@ -0,0 +1,86 @@
+include "utils/string.bzz"
+
+# Time to be sync
+logical_time = 0
+# sync algo. constants
+TIME_JUMP_THR = 5 
+TIME_TO_FORGET = 20
+TIME_TO_SYNC = 100
+# table to store neighbor time data
+time_nei_table = {}
+# Algo. global parameters
+diffMaxLogical = 0
+jumped = 0
+syncError = 99999 
+sync_timer = 0
+
+# Function to intialize sync algorithm
+function init_time_sync(){
+	neighbors.listen("time_sync",
+		function(vid, value, rid) {
+			log(" TIME SYNC Got (", vid, ",", value, ") #", rid)
+			var msg = string.split(value,",")
+			var msg_time = string.toint(msg[0])
+			var msg_max = string.toint(msg[1])
+			#log("msg:  1: ", msg_time, " 2: ", msg_max )
+			diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time)
+			var time_offset = msg_time - logical_time
+			if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset
+			if(time_offset > TIME_JUMP_THR){
+				logical_time = logical_time + time_offset
+				diffMaxLogical = math.max(diffMaxLogical-time_offset,0)
+				jumped = 1
+			}
+			time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max}  
+		}
+	)
+}
+
+# Function to sync. algo
+function step_time_sync(){
+	logical_time = logical_time + 1
+	sync_timer = sync_timer + 1
+	if(sync_timer < TIME_TO_SYNC){
+		log(" SYNC ALGO ACTIVE time:", sync_timer)
+		cnt = 0
+		avg_offset = 0
+	  	foreach(time_nei_table, function(key, value) {
+	    	if(value != nil){
+	    		#log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max)
+	    		var local_offset = value.time - logical_time - value.age
+	    		if(local_offset > 0){
+	    			avg_offset = avg_offset + 1 * local_offset
+	    			cnt = cnt + 1
+	    		}
+	    		else{
+	    			if(math.abs(local_offset)<TIME_JUMP_THR){
+	    				avg_offset = avg_offset + local_offset
+	    				cnt = cnt + 1
+	    			}
+	    		}
+
+		    	if(value.age > TIME_TO_FORGET)
+		    		time_nei_table[key] = nil
+		    	value.age = value.age + 1
+		    }
+	    })
+	    if(cnt > 0 and jumped != 1){
+	    	var correction = math.ceil(avg_offset / (cnt + 1) )
+	    	if(math.abs(correction) < TIME_JUMP_THR){
+	    		logical_time = logical_time + correction
+	    	}
+	    }
+	    
+	    jumped = 0
+	    syncError=0
+	    var mstr =  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
+
+function reinit_time_sync(){
+	sync_timer = 0
+}

buzz_scripts/include/utils/string.bzz

@@ -17,10 +17,10 @@ string.charat = function(s, n) {
 # RETURN: The position of the first match, or nil
 #
 string.indexoffirst = function(s, m) {
-  var ls = string.length(s)
+  var las = string.length(s)
   var lm = string.length(m)
   var i = 0
-  while(i < ls-lm+1) {
+  while(i < las-lm+1) {
     if(string.sub(s, i, i+lm) == m) return i
     i = i + 1
   }
@@ -36,9 +36,9 @@ string.indexoffirst = function(s, m) {
 # RETURN: The position of the last match, or nil
 #
 string.indexoflast = function(s, m) {
-  var ls = string.length(s)
+  var las = string.length(s)
   var lm = string.length(m)
-  var i = ls - lm + 1
+  var i = las - lm + 1
   while(i >= 0) {
     if(string.sub(s, i, i+lm) == m) return i
     i = i - 1
@@ -56,10 +56,10 @@ string.split = function(s, d) {
   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 las = string.length(s)
   var ld = string.length(d)
   # Go through string s
-  while(i2 < ls) {
+  while(i2 < las) {
     # Try every delimiter
     var j = 0   # index to move along d
     var f = nil # whether the delimiter was found or not

buzz_scripts/main.bzz

@@ -5,6 +5,7 @@ include "act/states.bzz"
 include "plan/rrtstar.bzz"
 include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
+include "timesync.bzz"
 
 #State launched after takeoff
 AUTO_LAUNCH_STATE = "CUSFUN"
@@ -31,6 +32,8 @@ function init() {
   	init_stig()
 	init_swarm()
 
+	init_time_sync()
+
 	TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m
 	loop = 1
 
@@ -43,6 +46,10 @@ function init() {
 
 # Executed at each time step.
 function step() {
+
+	# step time sync algorithm
+	step_time_sync()
+
 	# listen to Remote Controller
 	rc_cmd_listen()
 

include/buzz_utility.h

@@ -79,4 +79,6 @@ void set_robot_var(int ROBOTS);
 int get_inmsg_size();
 
 std::string getuavstate();
+
+int getlogicaltime();
 }

include/roscontroller.h

@@ -42,6 +42,7 @@
 #define TIMEOUT 60
 #define BUZZRATE 10
 #define CMD_REQUEST_UPDATE 666
+#define CMD_SYNC_CLOCK 777
 
 using namespace std;
 

misc/cmdlinectr.sh

@@ -11,6 +11,9 @@ function arm {
 function disarm {
 	rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0
 }
+function timesync {
+        rosservice call $1/buzzcmd 0 777 0 0 0 0 0 0 0 0
+}
 function testWP {
 	rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
 }

src/buzz_utility.cpp

@@ -581,4 +581,19 @@ string getuavstate()
   return uav_state;
 }
 
+int getlogicaltime()
+/*
+/ return current logical time
+--------------------------------------*/
+{
+  int logical_time = 0;
+  if(VM){
+    buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1));
+    buzzvm_gload(VM);
+    buzzobj_t obj = buzzvm_stack_at(VM, 1);
+    if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value;
+    buzzvm_pop(VM);
+  }
+  return logical_time;
+}
 }

src/roscontroller.cpp

@@ -153,6 +153,8 @@ void roscontroller::RosControllerRun()
         ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss);
       // log data
       log<<ros::Time::now()<<",";
+      // Fetch logical time from BVM
+      log<<buzz_utility::getlogicaltime()<<",";
       log<<cur_pos.latitude << "," << cur_pos.longitude << ","
             << cur_pos.altitude << ",";
       log << (int)no_of_robots<<",";
@@ -1136,9 +1138,15 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
+    case CMD_SYNC_CLOCK:
+      rc_cmd = CMD_SYNC_CLOCK;
+      buzzuav_closures::rc_call(rc_cmd);
+      ROS_INFO("<---------------- Time Sync requested ----------->");
+      res.success = true;
+      break;
     default:
       buzzuav_closures::rc_call(req.command);
-      ROS_INFO("----> Received unregistered command: ", req.command);
+      ROS_ERROR("----> Received unregistered command: ", req.command);
       res.success = true;
       break;
   }