diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp index 302b174be608d574afa225d978da3fe5f0a1e78b..1a857d7e674e9db4e849068ab19316a3f495fba4 100644 --- a/ethercat_driver/src/ethercat_driver.cpp +++ b/ethercat_driver/src/ethercat_driver.cpp @@ -237,7 +237,7 @@ bool EthercatDriver::deactivate_all_modules(const rclcpp::Duration & timeout) running = false; break; } - + master_->update(0,true); // calculate next shot. const rclcpp::Time time_iter_end = time_iter_start + rclcpp::Duration( 0, @@ -288,7 +288,8 @@ CallbackReturn EthercatDriver::on_configure( RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Activated EtherCAT Master!"); // Wait during one cycle period - const rclcpp::Duration cycle_period = rclcpp::Duration(0, master_->getInterval()); +// const rclcpp::Duration cycle_period = rclcpp::Duration(0, (master_->getInterval()<1000000 ? master_->getInterval() : 1000000) ); + const rclcpp::Duration cycle_period = rclcpp::Duration(0, master_->getInterval() ); rclcpp::Duration sleep_duration = cycle_period; //const rclcpp::Time time_iter_start = monotonic_clock_.now(); //rclcpp::Time time_iter_end = time_iter_start + cycle_period; @@ -434,14 +435,14 @@ CallbackReturn EthercatDriver::setupMaster() CallbackReturn EthercatDriver::configClockReference() { - uint16_t clock_reference_id = 666; + int16_t clock_reference_id = 666; // Get clock reference id if (info_.hardware_parameters.find("clock_reference_id") == info_.hardware_parameters.end()) { - // clock reference id was not provided, default to 0 - clock_reference_id = 0; + // clock reference id was not provided, default to -1 + clock_reference_id = -1; } else { try { - clock_reference_id = std::stoul(info_.hardware_parameters["clock_reference_id"]); + clock_reference_id = std::stol(info_.hardware_parameters["clock_reference_id"]); } catch (std::exception & e) { RCLCPP_FATAL( rclcpp::get_logger("EthercatDriver"), "Invalid clock reference id (%s)!", e.what()); @@ -449,7 +450,7 @@ CallbackReturn EthercatDriver::configClockReference() { } } RCLCPP_INFO( - rclcpp::get_logger("EthercatDriver"), "reference clock id (%u)!", clock_reference_id); + rclcpp::get_logger("EthercatDriver"), "reference clock id (%d)!", clock_reference_id); master_->setClockReferenceId(clock_reference_id); return CallbackReturn::SUCCESS; } @@ -552,6 +553,8 @@ CallbackReturn EthercatDriver::on_activate( return CallbackReturn::FAILURE; } + master_->update(0,true); + // calculate next shot. const rclcpp::Time time_iter_end = time_iter_start + rclcpp::Duration(0, master_->getInterval()); @@ -671,6 +674,7 @@ hardware_interface::return_type EthercatDriver::read( hardware_interface::return_type err = hardware_interface::return_type::OK; const rclcpp::Time time_begin_read = system_clock_.now(); + /*RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "read call");*/ if (in_past) { past_time_ +=rclcpp::Duration(0, master_->getInterval()); diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp index f9c031ae5984b9954284e7467514a9e0323b1e98..9550b45f048506d0d4dbdd3cfd6b72b5c87bc834 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp @@ -33,6 +33,11 @@ bool EcCiA402Drive::checkOperationEnabled() { initialized_ = (state_ == STATE_OPERATION_ENABLED) && (last_state_ == STATE_OPERATION_ENABLED); + +/* RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "initialized_ :%d", (int) initialized_); +*/ return initialized_; } @@ -240,6 +245,10 @@ DeviceState EcCiA402Drive::deviceState(uint16_t status_word) /** returns the control word that will take device from state to next desired state */ uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word) { + /*RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "transition call");*/ + switch (state) { case STATE_START: // -> STATE_NOT_READY_TO_SWITCH_ON (automatic) return control_word; diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp index bf0603a32eff22ec31bd0be3aafea1485a055e0b..06fd4e7b780c7918d468aa31bac8c9843d9777cc 100644 --- a/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -74,7 +74,7 @@ public: bool activate(); /** perform one EtherCAT cycle, passing the domain to the slaves */ - virtual bool update(uint32_t domain = 0); + virtual bool update(uint32_t domain = 0, bool data = false); /** stop the control loop. use within callback, or from a separate thread. */ virtual void stop(); @@ -91,10 +91,14 @@ public: interval_ = 1000000000.0 / frequency; } - void setClockReferenceId(uint16_t clock_reference_id) + void setClockReferenceId(int16_t clock_reference_id) { - reference_clock_postition_ = clock_reference_id; - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "reference clock id : %u, %u", reference_clock_postition_, clock_reference_id ); + if (clock_reference_id >= 0) + { + reference_clock_postition_ = (uint16_t) clock_reference_id; + reference_clock_update_ = true; + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "reference clock id : %u, %d", reference_clock_postition_, clock_reference_id ); + } } /** @@ -208,6 +212,7 @@ protected: rclcpp::Time time_begin ; bool ecat_communication_timeout_start_ = false; uint16_t reference_clock_postition_; + bool reference_clock_update_ = false; struct timespec last_time_; diff --git a/ethercat_interface/include/ethercat_interface/ec_safety.hpp b/ethercat_interface/include/ethercat_interface/ec_safety.hpp index ac17a9a87c1bf5922bbfa0d1e9b4369952799d10..6cce7bf204b310c493351da513fd66cc1a0f7e5b 100644 --- a/ethercat_interface/include/ethercat_interface/ec_safety.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_safety.hpp @@ -105,7 +105,7 @@ public: void transferAll(); /** perform one EtherCAT cycle, passing the domain to the slaves */ - bool update(uint32_t domain = 0); + bool update(uint32_t domain = 0, bool data = false); bool readData(uint32_t domain = 0); // TODO(yguel) investigate readData and writeData specifications diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_interface/src/ec_master.cpp index c0b1f4435085583d004957f83a263cddd9af769f..897719a1d23b49b2b5e69e153ce0adb63d8e5978 100644 --- a/ethercat_interface/src/ec_master.cpp +++ b/ethercat_interface/src/ec_master.cpp @@ -162,21 +162,22 @@ bool EcMaster::addSlave(EcSlave * slave) if (slave->assign_activate_dc_sync()) { struct timespec t; clock_gettime(CLOCK_MONOTONIC, &t); - //ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t)); ecrt_slave_config_dc( slave_info.config, slave->assign_activate_dc_sync(), +// (interval_ < 1000000 ? interval_ : 1000000), interval_, - //interval_ - (t.tv_nsec % (interval_)), interval_ /2.0, 0, 0); - if (slave->is_reference_clock(reference_clock_postition_)) { - ecrt_master_select_reference_clock(master_, slave_info.config); - RCLCPP_INFO( - rclcpp::get_logger("EthercatDriver"), - "Slave (0x%x, 0x%x) pos:%u selected as reference clock", slave->vendor_id_,slave->product_id_, reference_clock_postition_ - ); + if (reference_clock_update_) { + if (slave->is_reference_clock(reference_clock_postition_)) { + ecrt_master_select_reference_clock(master_, slave_info.config); + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "Slave (0x%x, 0x%x) pos:%u selected as reference clock", slave->vendor_id_,slave->product_id_, reference_clock_postition_ + ); + } } } @@ -340,7 +341,7 @@ bool EcMaster::activate() return true; } -bool EcMaster::update(uint32_t domain) +bool EcMaster::update(uint32_t domain, bool data) { bool ret; struct timespec time_start_; @@ -363,6 +364,13 @@ bool EcMaster::update(uint32_t domain) checkMasterState(); checkSlaveStates(); } + if (data) { + for (DomainInfo::Entry & entry : domain_info->entries) { + for (int i = 0; i < entry.num_pdos; ++i) { + (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]); + } + } + } // send process data diff --git a/ethercat_interface/src/ec_safety.cpp b/ethercat_interface/src/ec_safety.cpp index 37ee8bb5fd3062a32ba8355454a0da2749620f99..9147c2636aa4d5d1667cf8998b1f99fa0bda2357 100644 --- a/ethercat_interface/src/ec_safety.cpp +++ b/ethercat_interface/src/ec_safety.cpp @@ -169,7 +169,7 @@ void EcSafety::transferAll() } } -bool EcSafety::update(uint32_t domain) +bool EcSafety::update(uint32_t domain, bool data) { // receive process data ecrt_master_receive(master_);