Time Sync algorithm and rc cmd implementation to sync time on request
This commit is contained in:
parent
0836571c8e
commit
7261aabc40
|
@ -87,4 +87,4 @@ function barrier_allgood(barrier, bc) {
|
|||
}
|
||||
)
|
||||
return barriergood
|
||||
}
|
||||
}
|
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
}
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
||||
|
|
|
@ -79,4 +79,6 @@ void set_robot_var(int ROBOTS);
|
|||
int get_inmsg_size();
|
||||
|
||||
std::string getuavstate();
|
||||
|
||||
int getlogicaltime();
|
||||
}
|
||||
|
|
|
@ -43,6 +43,7 @@
|
|||
#define TIMEOUT 60
|
||||
#define BUZZRATE 10
|
||||
#define CMD_REQUEST_UPDATE 666
|
||||
#define CMD_SYNC_CLOCK 777
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue