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, ¤t_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, ¤t_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_);