From aba3b47caa033a83331f679d25fa55e7ccf16d3f Mon Sep 17 00:00:00 2001
From: Philippe <zanne.philippe@unistra.fr>
Date: Thu, 27 Mar 2025 10:29:44 +0000
Subject: [PATCH] add update ethercat data in on_active and on_desactive
fonvtion
---
ethercat_driver/src/ethercat_driver.cpp | 18 ++++++++-----
.../src/generic_ec_cia402_drive.cpp | 9 +++++++
.../include/ethercat_interface/ec_master.hpp | 13 +++++++---
.../include/ethercat_interface/ec_safety.hpp | 2 +-
ethercat_interface/src/ec_master.cpp | 26 ++++++++++++-------
ethercat_interface/src/ec_safety.cpp | 2 +-
6 files changed, 48 insertions(+), 22 deletions(-)
diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp
index 302b174..1a857d7 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 f9c031a..9550b45 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 bf0603a..06fd4e7 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 ac17a9a..6cce7bf 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 c0b1f44..897719a 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 37ee8bb..9147c26 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_);
--
GitLab