diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz
index 41f6561..8843795 100644
--- a/buzz_scripts/include/timesync.bzz
+++ b/buzz_scripts/include/timesync.bzz
@@ -1,5 +1,7 @@
 TIME_TO_SYNC = 100
 sync_timer = 0
+timesync_old_state = 0
+timesync_itr = 0
 timesync_state = 0
 
 # Function to sync. algo
@@ -9,11 +11,18 @@ function check_time_sync(){
   		log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC)
   		timesync_state = 1
 	}
-	else timesync_state = 0
+	else{
+		timesync_state = 0	
+	} 
+	if(timesync_old_state == 0 and timesync_state == 1){
+		timesync_itr = timesync_itr + 1
+		timesync_old_state = 0
+	}
+	timesync_old_state = timesync_state
 }
 
 # Function to set sync timer to zero and reinitiate sync. algo
 
 function reinit_time_sync(){
 	sync_timer = 0
-}
\ No newline at end of file
+}
diff --git a/include/buzz_utility.h b/include/buzz_utility.h
index 951207e..047b575 100644
--- a/include/buzz_utility.h
+++ b/include/buzz_utility.h
@@ -34,18 +34,6 @@ struct rb_struct
 };
 typedef struct rb_struct RB_struct;
 
-struct neiMsg
-{
-  uint8_t* msg;
-  uint16_t msgid;
-  double time_received;
-  double time_sent;
-  neiMsg(uint8_t* m, uint16_t mid,  double tr, double ts)
-    : msg(m), msgid(mid), time_received(tr), time_sent(ts) {};
-  neiMsg();
-};
-typedef struct pos_struct nei_msg_struct;
-
 struct neiStatus
 {
   uint gps_strenght = 0;
@@ -66,7 +54,7 @@ struct neitime
   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),age(0) {};
+      nei_rate(nr), relative_rate(mr) {};
   neitime()
   {
   }
@@ -113,4 +101,6 @@ std::vector<uint8_t*> get_inmsg_vector();
 std::string getuavstate();
 
 int get_timesync_state();
+
+int get_timesync_itr();
 }
diff --git a/include/roscontroller.h b/include/roscontroller.h
index 1ab67af..9713a9d 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -49,7 +49,7 @@ typedef enum {
 } rosbuzz_msgtype;
 
 // Time sync algo. constants
-#define COM_DELAY 33000000 // in nano seconds i.e 33 ms
+#define COM_DELAY 100000000 // in nano seconds i.e 100 ms
 #define TIME_SYNC_JUMP_THR 500000000 
 #define MOVING_AVERAGE_ALPHA 0.1
 
@@ -62,25 +62,6 @@ using namespace std;
 
 namespace rosbzz_node
 {
-  
-template <typename T>
-class circularBuffer {
-private:
-  vector<T> data;
-  unsigned int lastEntryPos;
-  int size;
-
-public:
-  circularBuffer(uint8_t s): data(s), lastEntryPos(0), size(s){};
-  ~circularBuffer(){};
-  void push(T d){ 
-    data[lastEntryPos] = d;
-    if(lastEntryPos > size-1) lastEntryPos = 0;
-    else lastEntryPos++;
-  }
-  vector<T> get_data(){ return data;};
-};
-
 class roscontroller
 {
 public:
@@ -130,7 +111,6 @@ private:
   };
   typedef struct MsgData msg_data;
 
-
   ros_pose target, home, cur_pos;
 
   uint64_t payload;
@@ -141,11 +121,10 @@ private:
   int robot_id = 0;
   ros::Time logical_clock;
   ros::Time previous_step_time;
+  std::vector<msg_data> inmsgdata; 
+  double out_msg_time; 
   double logical_time_rate;
   bool time_sync_jumped;
-  double com_delay;
-  std::vector<msg_data> inmsgdata;
-  double out_msg_time;
   std::string robot_name = "";
 
   int rc_cmd;
@@ -154,7 +133,7 @@ private:
   int barrier;
   int update;
   int statepub_active;
-  int16_t msg_id = 0;
+  int message_number = 0;
   uint8_t no_of_robots = 0;
   bool rcclient;
   bool xbeeplugged = false;
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index fdc7277..ebedb52 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -587,7 +587,7 @@ string getuavstate()
 
 int get_timesync_state()
 /*
-/ return current logical time
+/ return time sync state from bzz script
 --------------------------------------*/
 {
   int timesync_state = 0;
@@ -600,4 +600,20 @@ int get_timesync_state()
   }
   return timesync_state;
 }
+int get_timesync_itr()
+/*
+/ return time sync iteration from bzz script
+--------------------------------------*/
+{
+  int timesync_itr = 0;
+  if(VM){
+    buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_itr", 1));
+    buzzvm_gload(VM);
+    buzzobj_t obj = buzzvm_stack_at(VM, 1);
+    if(obj->o.type == BUZZTYPE_INT) timesync_itr = obj->i.value;
+    buzzvm_pop(VM);
+  }
+  return timesync_itr;
+}
+
 }
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 6e10840..1e42ebe 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -63,11 +63,9 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   }
   // time sync algo. parameter intialization
   previous_step_time.fromNSec(ros::Time::now().toNSec());
-  if(robot_id==10)logical_clock.fromSec(101.0);
-  else logical_clock.fromNSec(0);
-  logical_time_rate = 1;
+  logical_clock.fromSec(0);
+  logical_time_rate = 0;
   time_sync_jumped = false;
-  com_delay = 0;
   out_msg_time=0;
   // Create log dir and open log file
   std::string path =
@@ -172,12 +170,14 @@ void roscontroller::RosControllerRun()
       }
       if (debug)
         ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss);
+
       // log data
       // hardware time now
       log<<ros::Time::now().toSec()<<",";
       get_logical_time();
       // logical time now
       log<<logical_clock.toSec()<<",";
+      log<<buzz_utility::get_timesync_itr()<<",";
       log<<buzz_utility::get_timesync_state()<<",";
 
       log<<cur_pos.latitude << "," << cur_pos.longitude << ","
@@ -185,7 +185,7 @@ void roscontroller::RosControllerRun()
       log << (int)no_of_robots<<",";
       log  << neighbours_pos_map.size()<< ",";
       log<<(int)inmsgdata.size()<<",";
-      log<< msg_id<<",";
+      log<< message_number<<",";
       log<< out_msg_time<<",";
       log <<buzz_utility::getuavstate();
       if(neighbours_pos_map.size() > 0)log<<",";
@@ -203,10 +203,10 @@ void roscontroller::RosControllerRun()
       }
       inmsgdata.clear();
       log<<std::endl;
+
       time_sync_step();
       if(debug)
-        ROS_INFO("[Debug : %i ] logical Time %f , har time %f, rate: %f",robot_id, 
-                  logical_clock.toSec(), ros::Time::now().toSec(),logical_time_rate);
+        ROS_INFO("[Debug : %i ] logical Time %f , har time %f",robot_id, logical_clock.toSec(), ros::Time::now().toSec());
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();
       //  Prepare messages and publish them
@@ -586,8 +586,8 @@ void roscontroller::prepare_msg_and_publish()
 / Message format of payload (Each slot is uint64_t)
 /
 /
-/|  |     |     |
-|                |   /
+/|	|     |	    |
+|			           |   /
 /|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs
 with size.........  |   /
 /|_____|_____|_____|________________________________________________|______________________________|
@@ -604,46 +604,47 @@ with size.........  |   /
   memcpy(position, tmp, 3 * sizeof(uint64_t));
   mavros_msgs::Mavlink payload_out;
   //  Add Robot id and message number to the published message
-  if (msg_id < 0)
-    msg_id = 0;
+  if (message_number < 0)
+    message_number = 0;
   else
-    msg_id++;
-  payload_out.sysid = (uint8_t)robot_id;
-  payload_out.msgid = (uint32_t)msg_id;
-  uint8_t mtype;
-  uint16_t mid  = (uint16_t)msg_id;
-  get_logical_time();
-  uint32_t stime   = (uint32_t)logical_clock.toSec() * 100000;
-  ROS_INFO("Sent stime %u for %u",stime,mid);
-  uint8_t r8header[8];
-  uint64_t rheader = 0;
+    message_number++;
 
+  //header variables
+  uint16_t tmphead[4];
+  tmphead[1] = (uint16_t)message_number;
+  get_logical_time();
+  uint32_t stime = (uint32_t)(logical_clock.toSec() * 100000);
+  ROS_INFO("Sent stime %u for %u",stime,message_number);
+  memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t));
+  uint64_t rheader[1];
+  payload_out.sysid = (uint8_t)robot_id;
+  payload_out.msgid = (uint32_t)message_number;
+  
   if(buzz_utility::get_timesync_state()){
     // prepare rosbuzz msg header
-    mtype = (uint8_t)BUZZ_MESSAGE_TIME;
-    memcpy(r8header, &mtype, sizeof(uint8_t));   
-    memcpy(r8header+1, &mid, sizeof(uint16_t));
-    memcpy(r8header+3, &stime, sizeof(uint32_t)); 
-    memcpy(&rheader, r8header, sizeof(uint64_t));
+    tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME;
+    ROS_INFO("sent header : type  %u mid  %u time  %u",tmphead[0],tmphead[1],tmphead[2]);
+    memcpy(rheader, tmphead, sizeof(uint64_t));
     // push header into the buffer
-    payload_out.payload64.push_back(rheader);
+    payload_out.payload64.push_back(rheader[0]);
     payload_out.payload64.push_back(position[0]);
     payload_out.payload64.push_back(position[1]);
     payload_out.payload64.push_back(position[2]);
-    //payload_out.payload64.push_back((uint64_t)msg_id);
+    //payload_out.payload64.push_back((uint64_t)message_number);
     // add time sync algo data 
     payload_out.payload64.push_back(ros::Time::now().toNSec());
     payload_out.payload64.push_back(logical_clock.toNSec());
+    //uint64_t ltrate64 = 0;
+    //memcpy((void*)&ltrate64, (void*)&logical_time_rate, sizeof(uint64_t));
+    //payload_out.payload64.push_back(ltrate64);
   }
   else{
     // prepare rosbuzz msg header
-    mtype = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
-    memcpy(r8header, &mtype, sizeof(uint8_t));   
-    memcpy(r8header+1, &mid, sizeof(uint16_t));
-    memcpy(r8header+3, &stime, sizeof(uint32_t)); 
-    memcpy(&rheader, r8header, sizeof(uint64_t));
+    tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
+    ROS_INFO("sent header : type  %u mid  %u time  %u",tmphead[0],tmphead[1],tmphead[2]);
+    memcpy(rheader, tmphead, sizeof(uint64_t));
     // push header into the buffer
-    payload_out.payload64.push_back(rheader);
+    payload_out.payload64.push_back(rheader[0]);
     payload_out.payload64.push_back(position[0]);
     payload_out.payload64.push_back(position[1]);
     payload_out.payload64.push_back(position[2]);
@@ -654,13 +655,14 @@ with size.........  |   /
   {
     payload_out.payload64.push_back(payload_out_ptr[i]);
   }
+  //Store out msg time
   get_logical_time();
   out_msg_time = logical_clock.toSec();
   // publish prepared messages in respective topic
   payload_pub.publish(payload_out);
   delete[] out;
   delete[] payload_out_ptr;
-  //  Check for updater message if present send
+  ///  Check for updater message if present send
   if (update && buzz_update::is_msg_present())
   {
     uint8_t* buff_send = 0;
@@ -668,14 +670,8 @@ with size.........  |   /
     ;
     int tot = 0;
     mavros_msgs::Mavlink update_packets;
-    // Add Robot id and message number to the published message
-    if (msg_id < 0)
-      msg_id = 0;
-    else
-      msg_id++;
-    update_packets.sysid = (uint8_t)robot_id;
-    update_packets.msgid = (uint32_t)msg_id;
-    if(debug) ROS_INFO("Broadcasted Update packet Size: %u", updater_msgSize);
+    fprintf(stdout, "Appending code into message ...\n");
+    fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize);
     // allocate mem and clear it
     buff_send = (uint8_t*)malloc(sizeof(uint16_t) + updater_msgSize);
     memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize);
@@ -691,21 +687,19 @@ with size.........  |   /
     uint64_t* payload_64 = new uint64_t[total_size];
     memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
     free(buff_send);
-    // prepare rosbuzz msg header
-    mtype = (uint8_t)UPDATER_MESSAGE;
-    mid  = (uint16_t)msg_id;
-    get_logical_time();
-    stime   = (uint32_t)logical_clock.toSec() * 100000;
-    memcpy(r8header, &mtype, sizeof(uint8_t));   
-    memcpy(r8header+1, &mid, sizeof(uint16_t));
-    memcpy(r8header+3, &stime, sizeof(uint32_t)); 
-    memcpy(&rheader, r8header, sizeof(uint64_t));
-    // push header into the buffer
-    payload_out.payload64.push_back(rheader);
+    // Send a constant number to differenciate updater msgs
+    update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE);
     for (int i = 0; i < total_size; i++)
     {
       update_packets.payload64.push_back(payload_64[i]);
     }
+    // Add Robot id and message number to the published message
+    if (message_number < 0)
+      message_number = 0;
+    else
+      message_number++;
+    update_packets.sysid = (uint8_t)robot_id;
+    update_packets.msgid = (uint32_t)message_number;
     payload_pub.publish(update_packets);
     delete[] payload_64;
   }
@@ -1111,7 +1105,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
 /----------------------------------------------------------------------------------------
 / Message format of payload (Each slot is uint64_t)
 / _________________________________________________________________________________________________
-/|  |     |     |                |
+/|	|     |	    |						     |
  * |
 /|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs
  * with size.........  |
@@ -1120,19 +1114,18 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
 -----------------------------------------------------------------------------------------------------*/
 {
   // decode msg header
-  uint64_t rheader = msg->payload64[0];
-  uint8_t r8header[8];
-  uint8_t mtype;
-  uint16_t mid;
-  uint32_t temptime;
-  memcpy(r8header,&rheader, sizeof(uint64_t));
-  memcpy(&mtype, r8header, sizeof(uint8_t));   
-  memcpy(&mid, r8header+1, sizeof(uint16_t));
-  memcpy(&temptime, r8header+3, sizeof(uint32_t)); 
+  uint64_t rhead = msg->payload64[0];
+  uint16_t r16head[4];
+  memcpy(r16head,&rhead, sizeof(uint64_t));
+  uint16_t mtype = r16head[0];
+  uint16_t mid = r16head[1];
+  uint32_t temptime=0;
+  memcpy(&temptime, r16head+2, sizeof(uint32_t)); 
   float stime = (float)temptime/(float)100000;
   ROS_INFO("received stime %f for %u",stime, mid);
+  ROS_INFO("Received header : type  %u mid  %u time  %u",r16head[0],r16head[1],r16head[2]);
   // Check for Updater message, if updater message push it into updater FIFO
-  if ((int)mtype == (int)UPDATER_MESSAGE)
+  if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE)
   {
     uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
     uint64_t message_obt[obt_msg_size];
@@ -1147,7 +1140,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
     //  Copy packet into temporary buffer neglecting update constant
     memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size);
     uint16_t unMsgSize = *(uint16_t*)(pl);
-    if(debug) ROS_INFO("Update packet received, size : %u", unMsgSize);
+    fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize);
     if (unMsgSize > 0)
     {
       buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);
@@ -1165,15 +1158,6 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
     {
       message_obt[i - 1] = (uint64_t)msg->payload64[i];
     }
-    // determine buzz msg index based on msg type
-    int index = 3;
-    if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5;
-    //  Extract robot id of the neighbour
-    uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index));
-    get_logical_time();
-    // store in msg data for data log
-    msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec());
-    inmsgdata.push_back(inm);
     // Extract neighbours position from payload
     double neighbours_pos_payload[3];
     memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t));
@@ -1184,8 +1168,16 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
     nei_pos.longitude = neighbours_pos_payload[1];
     nei_pos.altitude = neighbours_pos_payload[2];
     double cvt_neighbours_pos_payload[3];
-    // Compute RB in robot body ref. frame
     gps_rb(nei_pos, cvt_neighbours_pos_payload);
+    int index = 3;
+    if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5;
+    //  Extract robot id of the neighbour
+    uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index));
+    get_logical_time();
+    // store in msg data for data log
+    msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,logical_clock.toSec());
+    inmsgdata.push_back(inm);
+
     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
@@ -1196,16 +1188,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);
-    if((int)mtype == (int)BUZZ_MESSAGE_TIME){
+     if((int)mtype == (int)BUZZ_MESSAGE_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));
     }
-    delete[] out;
   }
 }
 
@@ -1529,7 +1522,7 @@ void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, dou
   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("SYNC MSG RECEIVED deltaLocal from  %i",nid);  
 }
 
   uint64_t roscontroller::get_logical_time(){