Time Sync algorithm and rc cmd implementation to sync time on request
This commit is contained in:
parent
0836571c8e
commit
7261aabc40
|
@ -215,7 +215,13 @@ function rc_cmd_listen() {
|
||||||
} else if (flight.rc_cmd==666){
|
} else if (flight.rc_cmd==666){
|
||||||
flight.rc_cmd=0
|
flight.rc_cmd=0
|
||||||
stattab_send()
|
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
|
flight.rc_cmd=0
|
||||||
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
|
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
|
||||||
barrier_ready(900)
|
barrier_ready(900)
|
||||||
|
@ -259,7 +265,12 @@ function nei_cmd_listen() {
|
||||||
arm()
|
arm()
|
||||||
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
||||||
disarm()
|
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_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
|
||||||
#barrier_ready(900)
|
#barrier_ready(900)
|
||||||
neighbors.broadcast("cmd", 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
|
# RETURN: The position of the first match, or nil
|
||||||
#
|
#
|
||||||
string.indexoffirst = function(s, m) {
|
string.indexoffirst = function(s, m) {
|
||||||
var ls = string.length(s)
|
var las = string.length(s)
|
||||||
var lm = string.length(m)
|
var lm = string.length(m)
|
||||||
var i = 0
|
var i = 0
|
||||||
while(i < ls-lm+1) {
|
while(i < las-lm+1) {
|
||||||
if(string.sub(s, i, i+lm) == m) return i
|
if(string.sub(s, i, i+lm) == m) return i
|
||||||
i = i + 1
|
i = i + 1
|
||||||
}
|
}
|
||||||
|
@ -36,9 +36,9 @@ string.indexoffirst = function(s, m) {
|
||||||
# RETURN: The position of the last match, or nil
|
# RETURN: The position of the last match, or nil
|
||||||
#
|
#
|
||||||
string.indexoflast = function(s, m) {
|
string.indexoflast = function(s, m) {
|
||||||
var ls = string.length(s)
|
var las = string.length(s)
|
||||||
var lm = string.length(m)
|
var lm = string.length(m)
|
||||||
var i = ls - lm + 1
|
var i = las - lm + 1
|
||||||
while(i >= 0) {
|
while(i >= 0) {
|
||||||
if(string.sub(s, i, i+lm) == m) return i
|
if(string.sub(s, i, i+lm) == m) return i
|
||||||
i = i - 1
|
i = i - 1
|
||||||
|
@ -56,10 +56,10 @@ string.split = function(s, d) {
|
||||||
var i2 = 0 # index to move along s (token end)
|
var i2 = 0 # index to move along s (token end)
|
||||||
var c = 0 # token count
|
var c = 0 # token count
|
||||||
var t = {} # token list
|
var t = {} # token list
|
||||||
var ls = string.length(s)
|
var las = string.length(s)
|
||||||
var ld = string.length(d)
|
var ld = string.length(d)
|
||||||
# Go through string s
|
# Go through string s
|
||||||
while(i2 < ls) {
|
while(i2 < las) {
|
||||||
# Try every delimiter
|
# Try every delimiter
|
||||||
var j = 0 # index to move along d
|
var j = 0 # index to move along d
|
||||||
var f = nil # whether the delimiter was found or not
|
var f = nil # whether the delimiter was found or not
|
||||||
|
|
|
@ -5,6 +5,7 @@ include "act/states.bzz"
|
||||||
include "plan/rrtstar.bzz"
|
include "plan/rrtstar.bzz"
|
||||||
include "taskallocate/graphformGPS.bzz"
|
include "taskallocate/graphformGPS.bzz"
|
||||||
include "vstigenv.bzz"
|
include "vstigenv.bzz"
|
||||||
|
include "timesync.bzz"
|
||||||
|
|
||||||
#State launched after takeoff
|
#State launched after takeoff
|
||||||
AUTO_LAUNCH_STATE = "CUSFUN"
|
AUTO_LAUNCH_STATE = "CUSFUN"
|
||||||
|
@ -31,6 +32,8 @@ function init() {
|
||||||
init_stig()
|
init_stig()
|
||||||
init_swarm()
|
init_swarm()
|
||||||
|
|
||||||
|
init_time_sync()
|
||||||
|
|
||||||
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m
|
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m
|
||||||
loop = 1
|
loop = 1
|
||||||
|
|
||||||
|
@ -43,6 +46,10 @@ function init() {
|
||||||
|
|
||||||
# Executed at each time step.
|
# Executed at each time step.
|
||||||
function step() {
|
function step() {
|
||||||
|
|
||||||
|
# step time sync algorithm
|
||||||
|
step_time_sync()
|
||||||
|
|
||||||
# listen to Remote Controller
|
# listen to Remote Controller
|
||||||
rc_cmd_listen()
|
rc_cmd_listen()
|
||||||
|
|
||||||
|
|
|
@ -79,4 +79,6 @@ void set_robot_var(int ROBOTS);
|
||||||
int get_inmsg_size();
|
int get_inmsg_size();
|
||||||
|
|
||||||
std::string getuavstate();
|
std::string getuavstate();
|
||||||
|
|
||||||
|
int getlogicaltime();
|
||||||
}
|
}
|
||||||
|
|
|
@ -43,6 +43,7 @@
|
||||||
#define TIMEOUT 60
|
#define TIMEOUT 60
|
||||||
#define BUZZRATE 10
|
#define BUZZRATE 10
|
||||||
#define CMD_REQUEST_UPDATE 666
|
#define CMD_REQUEST_UPDATE 666
|
||||||
|
#define CMD_SYNC_CLOCK 777
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
|
|
|
@ -11,6 +11,9 @@ function arm {
|
||||||
function disarm {
|
function disarm {
|
||||||
rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0
|
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 {
|
function testWP {
|
||||||
rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
|
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();
|
return IN_MSG.size();
|
||||||
}
|
}
|
||||||
|
|
||||||
string getuavstate()
|
string getuavstate()
|
||||||
/*
|
/*
|
||||||
/ return current BVM state
|
/ return current BVM state
|
||||||
|
@ -579,4 +580,20 @@ string getuavstate()
|
||||||
}
|
}
|
||||||
return uav_state;
|
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);
|
ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss);
|
||||||
// log data
|
// log data
|
||||||
log<<ros::Time::now()<<",";
|
log<<ros::Time::now()<<",";
|
||||||
|
// Fetch logical time from BVM
|
||||||
|
log<<buzz_utility::getlogicaltime()<<",";
|
||||||
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
|
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
|
||||||
<< cur_pos.altitude << ",";
|
<< cur_pos.altitude << ",";
|
||||||
log << (int)no_of_robots<<",";
|
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);
|
buzzuav_closures::rc_call(rc_cmd);
|
||||||
res.success = true;
|
res.success = true;
|
||||||
break;
|
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:
|
default:
|
||||||
buzzuav_closures::rc_call(req.command);
|
buzzuav_closures::rc_call(req.command);
|
||||||
ROS_INFO("----> Received unregistered command: ", req.command);
|
ROS_ERROR("----> Received unregistered command: ", req.command);
|
||||||
res.success = true;
|
res.success = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue