Time Sync algorithm and rc cmd implementation to sync time on request

This commit is contained in:
vivek-shankar 2018-07-11 18:41:50 -04:00
parent 0836571c8e
commit 7261aabc40
10 changed files with 145 additions and 10 deletions

View File

@ -87,4 +87,4 @@ function barrier_allgood(barrier, bc) {
}
)
return barriergood
}
}

View File

@ -215,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)
@ -259,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)

View File

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

View File

@ -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

View File

@ -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()

View File

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

View File

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

View File

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

View File

@ -564,6 +564,7 @@ int get_inmsg_size()
{
return IN_MSG.size();
}
string getuavstate()
/*
/ return current BVM state
@ -579,4 +580,20 @@ 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;
}
}

View File

@ -165,6 +165,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<<",";
@ -1162,9 +1164,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;
}