diff --git a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp
index b7399318598730f87595019adcf03b57292a8abe..da4beac177ff7d48144cfe514048f4aab19a9f5c 100644
--- a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp
+++ b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp
@@ -87,6 +87,7 @@ protected:
   virtual CallbackReturn setupMaster();
 
   CallbackReturn configClockReference();
+  CallbackReturn DCSyncActiveWaitTime();
 
   CallbackReturn configNetwork();
 
@@ -123,7 +124,7 @@ protected:
   rclcpp::Duration activate_timeout_ = rclcpp::Duration(10, 0);  // 10 seconds
   rclcpp::Duration deactivate_timeout_ = rclcpp::Duration(10, 0);  // 10 seconds
   rclcpp::Duration cleanup_timeout_ = rclcpp::Duration(10, 0);  // 10 seconds
-
+  uint64_t active_wait_time_ = 100000L;
   bool in_past = false;
   rclcpp::Time past_time_;
 
diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp
index 1a857d7e674e9db4e849068ab19316a3f495fba4..3586096ec468616f1906c3b6a0b2f43cb30f813c 100644
--- a/ethercat_driver/src/ethercat_driver.cpp
+++ b/ethercat_driver/src/ethercat_driver.cpp
@@ -278,6 +278,9 @@ CallbackReturn EthercatDriver::on_configure(
     return config_return_;
   }
 
+  DCSyncActiveWaitTime();
+
+
 
   if (!master_->activate()) {
     RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), "Activate EtherCAT Master failed");
@@ -433,6 +436,33 @@ CallbackReturn EthercatDriver::setupMaster()
   return CallbackReturn::SUCCESS;
 }
 
+CallbackReturn EthercatDriver::DCSyncActiveWaitTime()
+{
+  uint64_t active_wait_time = 100;
+  // Get active wait time parameter
+  if (info_.hardware_parameters.find("dc_time_active_wait") == info_.hardware_parameters.end()) {
+    // Active wait time was not provided, default to 0
+    active_wait_time = 0;
+  } else {
+    try {
+      active_wait_time = std::stoul(info_.hardware_parameters["dc_time_active_wait"]);
+    } catch (std::exception & e) {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("EthercatDriver"), "Invalid active_wait_time (%s)!", e.what());
+      return CallbackReturn::ERROR;
+    }
+  }
+  active_wait_time_ = active_wait_time;
+  if (active_wait_time_ > master_->getInterval()) {
+    RCLCPP_WARN(
+        rclcpp::get_logger("EthercatDriver"), "active_wait_time (%lu) invalid value!", active_wait_time_);
+    active_wait_time_ = 0;
+    //return CallbackReturn::FAILURE;
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
 CallbackReturn EthercatDriver::configClockReference() {
 
   int16_t clock_reference_id = 666;
@@ -521,6 +551,7 @@ CallbackReturn EthercatDriver::on_activate(
   RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Starting ...please wait...");
 
   rclcpp::Time time_begin = monotonic_clock_.now();
+  rclcpp::Time time_wakeup = monotonic_clock_.now();
 
   rclcpp::Duration sleep_duration(0, 0);
 
@@ -556,9 +587,11 @@ CallbackReturn EthercatDriver::on_activate(
     master_->update(0,true);
 
     // calculate next shot.
-    const rclcpp::Time time_iter_end = time_iter_start +
-      rclcpp::Duration(0, master_->getInterval());
-    sleep_duration = time_iter_end - monotonic_clock_.now();
+    //const rclcpp::Time time_iter_end = time_iter_start +
+      //rclcpp::Duration(0, master_->getInterval());
+    time_wakeup = time_wakeup +  rclcpp::Duration(0, master_->getInterval()); 
+    //sleep_duration = time_iter_end - monotonic_clock_.now() - ;
+    sleep_duration = time_wakeup - monotonic_clock_.now() - rclcpp::Duration(0, active_wait_time_) ;
     // wait until next shot
     rclcpp::sleep_for(std::chrono::nanoseconds(sleep_duration.nanoseconds()));
   }
diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp
index 06fd4e7b780c7918d468aa31bac8c9843d9777cc..996ea5a943a7302ff0c9fa82d42216b479380466 100644
--- a/ethercat_interface/include/ethercat_interface/ec_master.hpp
+++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp
@@ -147,6 +147,8 @@ protected:
     RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), "WARNING. Master. %s", message.c_str());
   }
 
+  static inline uint64_t max(uint64_t a,uint64_t b) {return a>b?a:b;}
+  
   /** EtherCAT master data */
   ec_master_t * master_ = NULL;
   ec_master_state_t master_state_ = {};
@@ -215,6 +217,10 @@ protected:
   bool reference_clock_update_ = false;
 
   struct timespec last_time_;
+  uint64_t _last_occur=0;
+  uint64_t _last_publish=0;
+  uint64_t _current_lag=0;
+  uint64_t _max_jitter=0;
 
 };
 
diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_interface/src/ec_master.cpp
index 897719a1d23b49b2b5e69e153ce0adb63d8e5978..af2c6ea671dd528ef8fb364b3f8b04abf8dde09e 100644
--- a/ethercat_interface/src/ec_master.cpp
+++ b/ethercat_interface/src/ec_master.cpp
@@ -375,11 +375,45 @@ bool EcMaster::update(uint32_t domain, bool data)
 
   // send process data
   ecrt_domain_queue(domain_info->domain);
+
+  {
+    struct timespec current_time_s;
+    clock_gettime(CLOCK_MONOTONIC, &current_time_s);
+
+    uint64_t current_time = TIMESPEC2NS(current_time_s);    
+    uint64_t maxdeadline = current_time + (interval_ / 5);
+    uint64_t deadline = _last_occur ? 
+                        _last_occur + interval_ : 
+            	           current_time + _max_jitter; 
+    if(deadline > maxdeadline) deadline = maxdeadline;
+    _current_lag = deadline - current_time;
+    if(_last_publish != 0){
+      uint64_t period = current_time - _last_publish;
+      if(period > interval_)
+          _max_jitter = max(_max_jitter, period - interval_);
+      else
+          _max_jitter = max(_max_jitter, interval_ - period);
+    }
+    _last_publish = current_time;
+    _last_occur = current_time;
+    while(current_time < deadline) {
+      _last_occur = current_time; //Drift backward by default
+      clock_gettime(CLOCK_MONOTONIC, &current_time_s);
+      current_time = TIMESPEC2NS(current_time_s);   
+    }
+    if( _max_jitter * 10 < interval_ && _current_lag < _max_jitter){
+      //Consuming security margin ?
+      _last_occur = current_time; //Drift forward
+    }
+  }
+
+
+  ecrt_master_sync_reference_clock(master_);
+  ecrt_master_sync_slave_clocks(master_);
   struct timespec t;
   clock_gettime(CLOCK_MONOTONIC, &t);
   ecrt_master_application_time(master_, TIMESPEC2NS(t));
-  ecrt_master_sync_reference_clock(master_);
-  ecrt_master_sync_slave_clocks(master_);
+
   last_time_ = time_start_;
 
   ecrt_master_send(master_);