diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index c379bc5..c50e140 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -217,9 +217,7 @@ function rc_cmd_listen() { stattab_send() } 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) + reinit_time_sync() neighbors.broadcast("cmd", 777) }else if (flight.rc_cmd==900){ flight.rc_cmd=0 @@ -266,9 +264,7 @@ function nei_cmd_listen() { } else if(value==401 and BVMSTATE=="TURNEDOFF"){ disarm() } else if(value==777 and BVMSTATE=="TURNEDOFF"){ - if(logical_time != nil) reinit_time_sync() - barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) - barrier_ready(777) + reinit_time_sync() #neighbors.broadcast("cmd", 777) }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz index fecf9d9..41f6561 100644 --- a/buzz_scripts/include/timesync.bzz +++ b/buzz_scripts/include/timesync.bzz @@ -1,87 +1,15 @@ -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 = 200 -COM_DELAY = 2 -# table to store neighbor time data -time_nei_table = {} -# Algo. global parameters -diffMaxLogical = 0 -jumped = 0 -syncError = 99999 +TIME_TO_SYNC = 100 sync_timer = 0 - -# Function to intialize sync algorithm -function init_time_sync(){ - neighbors.listen("time_sync", - function(vid, value, rid) { - if(value != nil){ - log(" TIME SYNC Got (", vid, ",", value.time , " , ", value.max, ") #", rid) - var msg_time = value.time - var msg_max = value.max - #log("msg: 1: ", msg_time, " 2: ", msg_max ) - if(msg_time != nil and msg_max != nil){ - 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} - } - } - } - ) -} +timesync_state = 0 # Function to sync. algo -function step_time_sync(){ - logical_time = logical_time + 1 +function check_time_sync(){ 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 - avg_offset = 0 - if(size(time_nei_table) > 0){ - foreach(time_nei_table, function(key, value) { - if(value.time != 0){ - 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_TO_FORGET) - value.time = 0 - - 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 = {.time = (logical_time + COM_DELAY) , .max = (logical_time + COM_DELAY + diffMaxLogical) } - neighbors.broadcast("time_sync",mstr) + if(sync_timer < TIME_TO_SYNC){ + log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC) + timesync_state = 1 } + else timesync_state = 0 } # Function to set sync timer to zero and reinitiate sync. algo diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e187ce4..26786d2 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -11,7 +11,7 @@ include "timesync.bzz" AUTO_LAUNCH_STATE = "FORMATION" #AUTO_LAUNCH_STATE = "CUSFUN" #Lowest robot id in the network -LOWEST_ROBOT_ID = 97 +LOWEST_ROBOT_ID = 1 TARGET = 9.0 EPSILON = 30.0 @@ -33,8 +33,6 @@ function init() { init_stig() init_swarm() - init_time_sync() - TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -48,8 +46,8 @@ function init() { # Executed at each time step. function step() { - # step time sync algorithm - step_time_sync() + # check time sync algorithm state + check_time_sync() # listen to Remote Controller rc_cmd_listen() diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 4eda0fb..292f8b7 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -43,6 +43,24 @@ struct neiStatus }; typedef struct neiStatus neighbors_status; +struct neitime +{ + uint64_t nei_hardware_time; + uint64_t nei_logical_time; + uint64_t node_hardware_time; + uint64_t node_logical_time; + double nei_rate; + double relative_rate; + int age; + neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr) + : nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt), + nei_rate(nr), relative_rate(mr) {}; + neitime() + { + } +}; +typedef struct neitime neighbor_time; + uint16_t* u64_cvt_u16(uint64_t u64); int buzz_listen(const char* type, int msg_size); @@ -82,5 +100,5 @@ std::vector get_inmsg_vector(); std::string getuavstate(); -int getlogicaltime(); +int get_timesync_state(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 566b1a0..f9c684b 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -35,11 +35,17 @@ #include #include #include +#include #include "buzzuav_closures.h" #define UPDATER_MESSAGE_CONSTANT 987654321 -#define XBEE_MESSAGE_CONSTANT 586782343 -#define XBEE_STOP_TRANSMISSION 4355356352 +#define BUZZ_MESSAGE_CONSTANT_WTO_TIME 586782343 +#define BUZZ_MESSAGE_CONSTANT_TIME 523534343 +// Time sync algo. constants +#define COM_DELAY 100000000 // in nano seconds i.e 100 ms +#define TIME_SYNC_JUMP_THR 500000000 +#define MOVING_AVERAGE_ALPHA 0.1 + #define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 @@ -90,8 +96,13 @@ private: uint64_t payload; std::map neighbours_pos_map; std::map raw_neighbours_pos_map; + std::map neighbours_time_map; int timer_step = 0; int robot_id = 0; + ros::Time logical_clock; + ros::Time previous_step_time; + double logical_time_rate; + bool time_sync_jumped; std::string robot_name = ""; int rc_cmd; @@ -268,6 +279,7 @@ private: bool GetRawPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result); void get_xbee_status(); - + void time_sync_step(); + void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr); }; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 2cfcfd6..fdc7277 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -585,19 +585,19 @@ string getuavstate() return uav_state; } -int getlogicaltime() +int get_timesync_state() /* / return current logical time --------------------------------------*/ { - int logical_time = 0; + int timesync_state = 0; if(VM){ - buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_state", 1)); buzzvm_gload(VM); buzzobj_t obj = buzzvm_stack_at(VM, 1); - if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value; + if(obj->o.type == BUZZTYPE_INT) timesync_state = obj->i.value; buzzvm_pop(VM); } - return logical_time; + return timesync_state; } } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2c8265e..e6b1c44 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -13,7 +13,8 @@ namespace rosbzz_node const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const bool debug = true; -roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) +roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv): +logical_clock(ros::Time()), previous_step_time(ros::Time()) /* / roscontroller class Constructor ---------------*/ @@ -60,6 +61,11 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { robot_id = strtol(robot_name.c_str() + 5, NULL, 10); } + // time sync algo. parameter intialization + previous_step_time.fromNSec(ros::Time::now().toNSec()); + logical_clock.fromNSec(ros::Time::now().toNSec()); + logical_time_rate = 0; + time_sync_jumped = false; // Create log dir and open log file std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; @@ -164,9 +170,10 @@ void roscontroller::RosControllerRun() if (debug) ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); // log data - log<first<<","; log << (double)it->second.x << "," << (double)it->second.y << "," << (double)it->second.z << ","; } @@ -193,6 +201,9 @@ void roscontroller::RosControllerRun() log<<(int)neigh_id<<","<<(int)msg_size<<","; } log<payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT) + else if (msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME || + msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_WTO_TIME) { uint64_t message_obt[msg->payload64.size() - 1]; // Go throught the obtained payload @@ -1106,8 +1134,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; gps_rb(nei_pos, cvt_neighbours_pos_payload); + int index = 3; + if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME) index = 5; // Extract robot id of the neighbour - uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index)); if (debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); // Pass neighbour position to local maintaner @@ -1118,8 +1148,17 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) // TODO: remove roscontroller local map array for neighbors neighbours_pos_put((int)out[1], n_pos); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - delete[] out; - buzz_utility::in_msg_append((message_obt + 3)); + if(msg->payload64[0] == (uint64_t)BUZZ_MESSAGE_CONSTANT_TIME){ + // update time struct for sync algo + double nr = 0; + push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr); + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } + else{ + delete[] out; + buzz_utility::in_msg_append((message_obt + index)); + } } } @@ -1382,4 +1421,114 @@ void roscontroller::get_xbee_status() * TriggerAPIRssi(all_ids); */ } + +void roscontroller::time_sync_step() +/* + * Steps the time syncronization algorithm + ------------------------------------------------------------------ */ +{ + // ROS_INFO("Stepping time sync : %f ", logical_clock.toSec()); + double avgRate = logical_time_rate; + double avgOffset = 0; + int offsetCount = 0; + map::iterator it; + for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); ++it) + { + avgRate += (it->second).relative_rate; + // estimate current offset + int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; + // ROS_INFO("time sync loop offset %" PRId64 ", nei clock : %" PRId64 ", my clock :%" PRId64, offset,(int64_t)(it->second).nei_logical_time, (int64_t)(it->second).node_logical_time); + if(offset > 0){ + // neighbor is ahead in time + avgOffset = avgOffset + (1 * offset); + offsetCount++; + } + else{ + if(std::abs(offset)second).age < BUZZRATE) (it->second).age++; + else neighbours_time_map.erase(it); + } + if(offsetCount>0 && !time_sync_jumped){ + int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1)); + if(std::abs(correction) < TIME_SYNC_JUMP_THR){ + // ROS_INFO("With correction before correcting time sync : %f ", logical_clock.toSec()); + // correct time with the diff within this step + differnce in nei + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + // ROS_INFO(" time sync loop : avgOffset %f, offsetCount : %d , correction %" PRId64 "time diff %" PRIu64 " old_time %" PRIu64 " rate %f", + // avgOffset, offsetCount, correction, time_diff, old_time, avgRate); + uint64_t final_time = old_time + time_diff + (uint64_t)correction;// + (time_diff * avgRate); + + // ROS_INFO(" Final time: %" PRIu64, final_time); + logical_clock.fromNSec(final_time); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + + // ROS_INFO("With correction time sync : %f ", logical_clock.toSec()); + } + else{ + // correct time with the diff within this step + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + //ROS_INFO("inside non jump without coorection time sync : %f ", logical_clock.toSec()); + } + } + else{ + // correct time with the diff within this step + rate change + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + avgRate = avgRate/(neighbours_time_map.size()+1); + logical_clock.fromNSec(old_time + time_diff); // + time_diff * avgRate); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + + //ROS_INFO("without correction time sync : %f ", logical_clock.toSec()); + } + time_sync_jumped = false; + //neighbours_time_map.clear(); + logical_time_rate = avgRate; + +} + +void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr) +/* + * pushes a time syncronization msg into its data slot + ------------------------------------------------------------------ */ +{ + map::iterator it = neighbours_time_map.find(nid); + double relativeRate =0; + if (it != neighbours_time_map.end()){ + uint64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time; + uint64_t delatNei = round((nh - (it->second).nei_hardware_time ) + + (nh - (it->second).nei_hardware_time ) * nr); + double currentRate = 0; + if(deltaLocal !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal; + relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate) + + (1- MOVING_AVERAGE_ALPHA)*currentRate; + + // ROS_INFO("deltaLocal %" PRIu64 ", delatNei %" PRIu64 " , currentrate %f , this relative rate %f, final relativeRate %f", deltaLocal, delatNei, currentRate, + // (it->second).relative_rate, relativeRate); + neighbours_time_map.erase(it); + } + int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec(); + // ROS_INFO("neigh msg push from %i "", my clock %" PRIu64 ", nei clock %" PRIu64 ", offset: %" PRId64", relative rate %f", nid, logical_clock.toNSec(), nl, offset, relativeRate); + if(offset > TIME_SYNC_JUMP_THR){ + uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec(); + uint64_t old_time = logical_clock.toNSec(); + logical_clock.fromNSec(old_time + time_diff + offset );// + time_diff * logical_time_rate ); + previous_step_time.fromNSec(ros::Time::now().toNSec()); + time_sync_jumped = true; + } + buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(), + logical_clock.toNSec(), nr, relativeRate); + neighbours_time_map.insert(make_pair(nid, nt)); + + // ROS_INFO("neigh time sync : %f , nei logical %i", logical_clock.toSec(), nl); +} }