From 2aa058af2ed0ee9a8f9db63a116f410947330bb7 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Fri, 13 Jun 2025 12:26:56 +0000
Subject: [PATCH 01/20] multiple masters compatibility added

---
 .../ethercat_driver/ethercat_driver.hpp       |  2 +-
 ethercat_driver/src/ethercat_driver.cpp       | 31 +++++++++++++------
 2 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp
index 8862c0a0..c5e633e7 100644
--- a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp
+++ b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp
@@ -83,7 +83,7 @@ class EthercatDriver : public hardware_interface::SystemInterface
     "ethercat_interface", "ethercat_interface::EcSlave"};
 
   int control_frequency_;
-  ethercat_interface::EcMaster master_;
+  std::unique_ptr<ethercat_interface::EcMaster> master_;
   std::mutex ec_mutex_;
   bool activated_;
 };
diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp
index deea06b5..b1a73675 100644
--- a/ethercat_driver/src/ethercat_driver.cpp
+++ b/ethercat_driver/src/ethercat_driver.cpp
@@ -33,6 +33,16 @@ CallbackReturn EthercatDriver::on_init(
   const std::lock_guard<std::mutex> lock(ec_mutex_);
   activated_ = false;
 
+  // Parse master_id from hardware parameters
+  int master_id = 0;  // Default to 0 if not specified
+  if (info_.hardware_parameters.find("master_id") != info_.hardware_parameters.end()) {
+    master_id = std::stoi(info_.hardware_parameters["master_id"]);
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Master ID: %d", master_id);
+
+  // Initialize master with the specified ID
+  master_ = std::make_unique<ethercat_interface::EcMaster>(master_id);
+
   hw_joint_states_.resize(info_.joints.size());
   for (uint j = 0; j < info_.joints.size(); j++) {
     hw_joint_states_[j].resize(
@@ -282,10 +292,10 @@ CallbackReturn EthercatDriver::on_activate(
 
   // start EC and wait until state operative
 
-  master_.setCtrlFrequency(control_frequency_);
+  master_->setCtrlFrequency(control_frequency_);
 
   for (auto i = 0ul; i < ec_modules_.size(); i++) {
-    master_.addSlave(
+    master_->addSlave(
       std::stod(ec_module_parameters_[i]["alias"]),
       std::stod(ec_module_parameters_[i]["position"]),
       ec_modules_[i].get());
@@ -295,7 +305,7 @@ CallbackReturn EthercatDriver::on_activate(
   for (auto i = 0ul; i < ec_modules_.size(); i++) {
     for (auto & sdo : ec_modules_[i]->sdo_config) {
       uint32_t abort_code;
-      int ret = master_.configSlaveSdo(
+      int ret = master_->configSlaveSdo(
         std::stod(ec_module_parameters_[i]["position"]),
         sdo,
         &abort_code
@@ -311,7 +321,7 @@ CallbackReturn EthercatDriver::on_activate(
     }
   }
 
-  if (!master_.activate()) {
+  if (!master_->activate()) {
     RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), "Activate EcMaster failed");
     return CallbackReturn::ERROR;
   }
@@ -328,7 +338,7 @@ CallbackReturn EthercatDriver::on_activate(
     clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
     // update EtherCAT bus
 
-    master_.update();
+    master_->update();
     RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
 
     // check if operational
@@ -340,7 +350,7 @@ CallbackReturn EthercatDriver::on_activate(
       running = false;
     }
     // calculate next shot. carry over nanoseconds into microseconds.
-    t.tv_nsec += master_.getInterval();
+    t.tv_nsec += master_->getInterval();
     while (t.tv_nsec >= 1000000000) {
       t.tv_nsec -= 1000000000;
       t.tv_sec++;
@@ -363,8 +373,11 @@ CallbackReturn EthercatDriver::on_deactivate(
 
   RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait...");
 
+  // Clean up all modules before stopping the master
+  ec_modules_.clear();
+
   // stop EC and disconnect
-  master_.stop();
+  master_->stop();
 
   RCLCPP_INFO(
     rclcpp::get_logger("EthercatDriver"), "System successfully stopped!");
@@ -379,7 +392,7 @@ hardware_interface::return_type EthercatDriver::read(
   // try to lock so we can avoid blocking the read/write loop on the lock.
   const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
   if (lock.owns_lock() && activated_) {
-    master_.readData();
+    master_->readData();
   }
   return hardware_interface::return_type::OK;
 }
@@ -391,7 +404,7 @@ hardware_interface::return_type EthercatDriver::write(
   // try to lock so we can avoid blocking the read/write loop on the lock.
   const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
   if (lock.owns_lock() && activated_) {
-    master_.writeData();
+    master_->writeData();
   }
   return hardware_interface::return_type::OK;
 }

From aa305043c12c2efac33acdcad5b7903845cac259 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Tue, 8 Jul 2025 14:07:09 +0000
Subject: [PATCH 02/20] temperature error code check for zeroerr drives

---
 .../cia402_common_defs.hpp                    |  1 +
 .../generic_ec_cia402_drive.hpp               |  2 ++
 .../src/generic_ec_cia402_drive.cpp           | 30 ++++++++++++++++++-
 .../ethercat_manager/ec_master_async_io.hpp   |  2 +-
 .../src/ethercat_sdo_srv_server.cpp           |  2 +-
 5 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp
index c4ba4aad..e2fb1b89 100644
--- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp
@@ -24,6 +24,7 @@
 #define CiA402D_TPDO_POSITION ((uint16_t) 0x6064)
 #define CiA402D_TPDO_STATUSWORD  ((uint16_t) 0x6041)
 #define CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY  ((uint16_t) 0x6061)
+#define CiA402D_TPDO_ERROR_CODE  ((uint16_t) 0x603f)
 
 #include <map>
 #include <string>
diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
index 07ec2a04..77e3bc60 100644
--- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
@@ -64,6 +64,8 @@ class EcCiA402Drive : public GenericEcSlave
   int fault_reset_command_interface_index_ = -1;
   bool last_fault_reset_command_ = false;
   double last_position_ = std::numeric_limits<double>::quiet_NaN();
+  bool temperature_fault_detected_ = false;
+  uint16_t error_code_ = 0;
 
   /** returns device state based upon the status_word */
   DeviceState deviceState(uint16_t status_word);
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 f06426db..0f86c441 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
@@ -87,6 +87,34 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address)
     status_word_ = pdo_channels_info_[index].last_value;
   }
 
+  // Special case: Error Code (0x603F)
+  if (pdo_channels_info_[index].index == CiA402D_TPDO_ERROR_CODE) {
+    uint16_t new_error_code = pdo_channels_info_[index].last_value;
+    if (new_error_code != error_code_) {
+      error_code_ = new_error_code;
+      std::cout << "Error Code: " << std::hex << error_code_ << std::dec << std::endl;
+      
+      // During startup, clear any stored temperature faults
+      if (counter_ < 100) {  // First 100 cycles = startup phase
+        if (error_code_ == 0x4110) {
+          std::cout << "Startup: Clearing stored temperature fault error code" << std::endl;
+          temperature_fault_detected_ = false;
+        }
+      }
+      // After startup, check for temperature fault using specific error code 0x4110
+      else if (error_code_ == 0x4110) {
+        temperature_fault_detected_ = true;
+        std::cout << "TEMPERATURE FAULT DETECTED (Error Code 0x4110) - Disabling automatic fault reset" << std::endl;
+      }
+      // Reset temperature fault flag when error code returns to 0 (no fault)
+      else if (error_code_ == 0x0) {
+        if (temperature_fault_detected_) {
+          temperature_fault_detected_ = false;
+          std::cout << "Temperature fault cleared - Automatic fault reset re-enabled" << std::endl;
+        }
+      }
+    }
+  }
 
   // CHECK FOR STATE CHANGE
   if (index == all_channels_.size() - 1) {  // if last entry  in domain
@@ -213,7 +241,7 @@ uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word)
     case STATE_FAULT_REACTION_ACTIVE:     // -> STATE_FAULT (automatic)
       return control_word;
     case STATE_FAULT:                     // -> STATE_SWITCH_ON_DISABLED
-      if (auto_fault_reset_ || fault_reset_) {
+      if ((auto_fault_reset_ || fault_reset_) && !temperature_fault_detected_) {
         fault_reset_ = false;
         return (control_word & 0b11111111) | 0b10000000;     // automatic reset
       } else {
diff --git a/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp b/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp
index 00294775..b3e4fb4e 100644
--- a/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp
+++ b/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp
@@ -20,7 +20,7 @@
 #define EC_IOR(nr, type)  _IOR(EC_IOCTL_TYPE, nr, type)
 #define EC_IOW(nr, type)  _IOW(EC_IOCTL_TYPE, nr, type)
 #define EC_IOWR(nr, type)  _IOWR(EC_IOCTL_TYPE, nr, type)
-#define EC_IOCTL_VERSION_MAGIC 31
+#define EC_IOCTL_VERSION_MAGIC 37 // ioctl() version magic is matching: /dev/EtherCAT0: 37, ethercat tool: 37
 #define EC_IOCTL_MODULE  EC_IOR(0x00, ec_ioctl_module_t)
 #define EC_IOCTL_SLAVE_SDO_UPLOAD  EC_IOWR(0x0e, ec_ioctl_slave_sdo_upload_t)
 #define EC_IOCTL_SLAVE_SDO_DOWNLOAD  EC_IOWR(0x0f, ec_ioctl_slave_sdo_download_t)
diff --git a/ethercat_manager/src/ethercat_sdo_srv_server.cpp b/ethercat_manager/src/ethercat_sdo_srv_server.cpp
index 24a70132..72154626 100644
--- a/ethercat_manager/src/ethercat_sdo_srv_server.cpp
+++ b/ethercat_manager/src/ethercat_sdo_srv_server.cpp
@@ -54,7 +54,7 @@ void upload(
 
   EcMasterAsync master(request->master_id);
   try {
-    master.open(EcMasterAsync::Read);
+    master.open(EcMasterAsync::ReadWrite);
   } catch (MasterException & e) {
     return_stream << e.what();
     response->success = false;

From 195f26eab043caa92f99ac71f29996e5954bb37f Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Thu, 10 Jul 2025 17:28:31 +0000
Subject: [PATCH 03/20] stop drifting when the controllers are inactive

---
 .../src/generic_ec_cia402_drive.cpp           | 434 ++++++++++--------
 1 file changed, 253 insertions(+), 181 deletions(-)

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 0f86c441..a4125bcd 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
@@ -22,238 +22,310 @@ namespace ethercat_generic_plugins
 {
 
 EcCiA402Drive::EcCiA402Drive()
-: GenericEcSlave() {}
+    : GenericEcSlave()
+{
+}
 EcCiA402Drive::~EcCiA402Drive() {}
 
-bool EcCiA402Drive::initialized() const {return initialized_;}
+bool EcCiA402Drive::initialized() const { return initialized_; }
 
-void EcCiA402Drive::processData(size_t index, uint8_t * domain_address)
+void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
 {
-  // Special case: ControlWord
-  if (pdo_channels_info_[index].index == CiA402D_RPDO_CONTROLWORD) {
-    if (is_operational_) {
-      if (fault_reset_command_interface_index_ >= 0) {
-        if (command_interface_ptr_->at(fault_reset_command_interface_index_) == 0) {
-          last_fault_reset_command_ = false;
-        }
-        if (last_fault_reset_command_ == false &&
-          command_interface_ptr_->at(fault_reset_command_interface_index_) != 0 &&
-          !std::isnan(command_interface_ptr_->at(fault_reset_command_interface_index_)))
+    // Special case: ControlWord
+    if (pdo_channels_info_[index].index == CiA402D_RPDO_CONTROLWORD)
+    {
+        if (is_operational_)
         {
-          last_fault_reset_command_ = true;
-          fault_reset_ = true;
-        }
-      }
+            if (fault_reset_command_interface_index_ >= 0)
+            {
+                if (command_interface_ptr_->at(fault_reset_command_interface_index_) == 0)
+                {
+                    last_fault_reset_command_ = false;
+                }
+                if (last_fault_reset_command_ == false &&
+                    command_interface_ptr_->at(fault_reset_command_interface_index_) != 0 &&
+                    !std::isnan(command_interface_ptr_->at(fault_reset_command_interface_index_)))
+                {
+                    last_fault_reset_command_ = true;
+                    fault_reset_ = true;
+                }
+            }
 
-      if (auto_state_transitions_) {
-        pdo_channels_info_[index].default_value = transition(
-          state_,
-          pdo_channels_info_[index].ec_read(domain_address));
-      }
+            if (auto_state_transitions_)
+            {
+                pdo_channels_info_[index].default_value =
+                    transition(state_, pdo_channels_info_[index].ec_read(domain_address));
+            }
+        }
     }
-  }
 
-  // setup current position as default position
-  if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) {
-    if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE) {
-      pdo_channels_info_[index].default_value =
-        pdo_channels_info_[index].factor * last_position_ +
-        pdo_channels_info_[index].offset;
+    // setup current position as default position
+    if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION)
+    {
+        if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE)
+        {
+            pdo_channels_info_[index].default_value =
+                pdo_channels_info_[index].factor * last_position_ + pdo_channels_info_[index].offset;
+        }
+        pdo_channels_info_[index].override_command =
+            (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION) ? true : false;
     }
-    pdo_channels_info_[index].override_command =
-      (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION) ? true : false;
-  }
 
-  // setup mode of operation
-  if (pdo_channels_info_[index].index == CiA402D_RPDO_MODE_OF_OPERATION) {
-    if (mode_of_operation_ >= 0 && mode_of_operation_ <= 10) {
-      pdo_channels_info_[index].default_value = mode_of_operation_;
+    // setup mode of operation
+    if (pdo_channels_info_[index].index == CiA402D_RPDO_MODE_OF_OPERATION)
+    {
+        if (mode_of_operation_ >= 0 && mode_of_operation_ <= 10)
+        {
+            pdo_channels_info_[index].default_value = mode_of_operation_;
+        }
     }
-  }
-
-  pdo_channels_info_[index].ec_update(domain_address);
 
-  // get mode_of_operation_display_
-  if (pdo_channels_info_[index].index == CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY) {
-    mode_of_operation_display_ = pdo_channels_info_[index].last_value;
-  }
+    pdo_channels_info_[index].ec_update(domain_address);
 
-  if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION) {
-    last_position_ = pdo_channels_info_[index].last_value;
-  }
-
-  // Special case: StatusWord
-  if (pdo_channels_info_[index].index == CiA402D_TPDO_STATUSWORD) {
-    status_word_ = pdo_channels_info_[index].last_value;
-  }
+    // get mode_of_operation_display_
+    if (pdo_channels_info_[index].index == CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY)
+    {
+        mode_of_operation_display_ = pdo_channels_info_[index].last_value;
+    }
 
-  // Special case: Error Code (0x603F)
-  if (pdo_channels_info_[index].index == CiA402D_TPDO_ERROR_CODE) {
-    uint16_t new_error_code = pdo_channels_info_[index].last_value;
-    if (new_error_code != error_code_) {
-      error_code_ = new_error_code;
-      std::cout << "Error Code: " << std::hex << error_code_ << std::dec << std::endl;
-      
-      // During startup, clear any stored temperature faults
-      if (counter_ < 100) {  // First 100 cycles = startup phase
-        if (error_code_ == 0x4110) {
-          std::cout << "Startup: Clearing stored temperature fault error code" << std::endl;
-          temperature_fault_detected_ = false;
+    if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION)
+    {
+        double current_position = pdo_channels_info_[index].last_value;
+        // Initialize last_position_ if it hasn't been set yet (first time)
+        if (std::isnan(last_position_))
+        {
+            last_position_ = current_position;
+            std::cout << "Position initialized: " << last_position_ << std::endl;
         }
-      }
-      // After startup, check for temperature fault using specific error code 0x4110
-      else if (error_code_ == 0x4110) {
-        temperature_fault_detected_ = true;
-        std::cout << "TEMPERATURE FAULT DETECTED (Error Code 0x4110) - Disabling automatic fault reset" << std::endl;
-      }
-      // Reset temperature fault flag when error code returns to 0 (no fault)
-      else if (error_code_ == 0x0) {
-        if (temperature_fault_detected_) {
-          temperature_fault_detected_ = false;
-          std::cout << "Temperature fault cleared - Automatic fault reset re-enabled" << std::endl;
+        else
+        {
+            // Only update last_position_ if the change is significant (prevents drifting)
+            double position_threshold = 0.0001; // Adjust this threshold as needed
+            double change = std::abs(current_position - last_position_);
+            if (change > position_threshold)
+            {
+                last_position_ = current_position;
+            }
         }
-      }
     }
-  }
 
-  // CHECK FOR STATE CHANGE
-  if (index == all_channels_.size() - 1) {  // if last entry  in domain
-    if (status_word_ != last_status_word_) {
-      state_ = deviceState(status_word_);
-      if (state_ != last_state_) {
-        std::cout << "STATE: " << DEVICE_STATE_STR.at(state_)
-                  << " with status word :" << status_word_ << std::endl;
-      }
+    // Special case: StatusWord
+    if (pdo_channels_info_[index].index == CiA402D_TPDO_STATUSWORD)
+    {
+        status_word_ = pdo_channels_info_[index].last_value;
     }
-    initialized_ = ((state_ == STATE_OPERATION_ENABLED) &&
-      (last_state_ == STATE_OPERATION_ENABLED)) ? true : false;
 
-    last_status_word_ = status_word_;
-    last_state_ = state_;
-    counter_++;
-  }
+    // Special case: Error Code (0x603F)
+    if (pdo_channels_info_[index].index == CiA402D_TPDO_ERROR_CODE)
+    {
+        uint16_t new_error_code = pdo_channels_info_[index].last_value;
+        if (new_error_code != error_code_)
+        {
+            error_code_ = new_error_code;
+            std::cout << "Error Code: " << std::hex << error_code_ << std::dec << std::endl;
+
+            // During startup, clear any stored temperature faults
+            if (counter_ < 100)
+            { // First 100 cycles = startup phase
+                if (error_code_ == 0x4110)
+                {
+                    std::cout << "Startup: Clearing stored temperature fault error code" << std::endl;
+                    temperature_fault_detected_ = false;
+                }
+            }
+            // After startup, check for temperature fault using specific error code 0x4110
+            else if (error_code_ == 0x4110)
+            {
+                temperature_fault_detected_ = true;
+                std::cout << "TEMPERATURE FAULT DETECTED (Error Code 0x4110) - Disabling automatic fault reset"
+                          << std::endl;
+            }
+            // Reset temperature fault flag when error code returns to 0 (no fault)
+            else if (error_code_ == 0x0)
+            {
+                if (temperature_fault_detected_)
+                {
+                    temperature_fault_detected_ = false;
+                    std::cout << "Temperature fault cleared - Automatic fault reset re-enabled" << std::endl;
+                }
+            }
+        }
+    }
+
+    // CHECK FOR STATE CHANGE
+    if (index == all_channels_.size() - 1)
+    { // if last entry  in domain
+        if (status_word_ != last_status_word_)
+        {
+            state_ = deviceState(status_word_);
+            if (state_ != last_state_)
+            {
+                std::cout << "STATE: " << DEVICE_STATE_STR.at(state_) << " with status word :" << status_word_
+                          << std::endl;
+            }
+        }
+        initialized_ = ((state_ == STATE_OPERATION_ENABLED) && (last_state_ == STATE_OPERATION_ENABLED)) ? true : false;
+
+        last_status_word_ = status_word_;
+        last_state_ = state_;
+        counter_++;
+    }
 }
 
-bool EcCiA402Drive::setupSlave(
-  std::unordered_map<std::string, std::string> slave_paramters,
-  std::vector<double> * state_interface,
-  std::vector<double> * command_interface)
+bool EcCiA402Drive::setupSlave(std::unordered_map<std::string, std::string> slave_paramters,
+                               std::vector<double> *state_interface, std::vector<double> *command_interface)
 {
-  state_interface_ptr_ = state_interface;
-  command_interface_ptr_ = command_interface;
-  paramters_ = slave_paramters;
+    state_interface_ptr_ = state_interface;
+    command_interface_ptr_ = command_interface;
+    paramters_ = slave_paramters;
 
-  if (paramters_.find("slave_config") != paramters_.end()) {
-    if (!setup_from_config_file(paramters_["slave_config"])) {
-      return false;
+    if (paramters_.find("slave_config") != paramters_.end())
+    {
+        if (!setup_from_config_file(paramters_["slave_config"]))
+        {
+            return false;
+        }
+    }
+    else
+    {
+        std::cerr << "EcCiA402Drive: failed to find 'slave_config' tag in URDF." << std::endl;
+        return false;
     }
-  } else {
-    std::cerr << "EcCiA402Drive: failed to find 'slave_config' tag in URDF." << std::endl;
-    return false;
-  }
 
-  setup_interface_mapping();
-  setup_syncs();
+    setup_interface_mapping();
+    setup_syncs();
 
-  if (paramters_.find("mode_of_operation") != paramters_.end()) {
-    mode_of_operation_ = std::stod(paramters_["mode_of_operation"]);
-  }
+    if (paramters_.find("mode_of_operation") != paramters_.end())
+    {
+        mode_of_operation_ = std::stod(paramters_["mode_of_operation"]);
+    }
 
-  if (paramters_.find("command_interface/reset_fault") != paramters_.end()) {
-    fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]);
-  }
+    if (paramters_.find("command_interface/reset_fault") != paramters_.end())
+    {
+        fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]);
+    }
 
-  return true;
+    return true;
 }
 
 bool EcCiA402Drive::setup_from_config(YAML::Node drive_config)
 {
-  if (!GenericEcSlave::setup_from_config(drive_config)) {return false;}
-  // additional configuration parameters for CiA402 Drives
-  if (drive_config["auto_fault_reset"]) {
-    auto_fault_reset_ = drive_config["auto_fault_reset"].as<bool>();
-  }
-  if (drive_config["auto_state_transitions"]) {
-    auto_state_transitions_ = drive_config["auto_state_transitions"].as<bool>();
-  }
-  return true;
+    if (!GenericEcSlave::setup_from_config(drive_config))
+    {
+        return false;
+    }
+    // additional configuration parameters for CiA402 Drives
+    if (drive_config["auto_fault_reset"])
+    {
+        auto_fault_reset_ = drive_config["auto_fault_reset"].as<bool>();
+    }
+    if (drive_config["auto_state_transitions"])
+    {
+        auto_state_transitions_ = drive_config["auto_state_transitions"].as<bool>();
+    }
+    return true;
 }
 
 bool EcCiA402Drive::setup_from_config_file(std::string config_file)
 {
-  // Read drive configuration from YAML file
-  try {
-    slave_config_ = YAML::LoadFile(config_file);
-  } catch (const YAML::ParserException & ex) {
-    std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl;
-    return false;
-  } catch (const YAML::BadFile & ex) {
-    std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl;
-    return false;
-  }
-  if (!setup_from_config(slave_config_)) {
-    return false;
-  }
-  return true;
+    // Read drive configuration from YAML file
+    try
+    {
+        slave_config_ = YAML::LoadFile(config_file);
+    }
+    catch (const YAML::ParserException &ex)
+    {
+        std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl;
+        return false;
+    }
+    catch (const YAML::BadFile &ex)
+    {
+        std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl;
+        return false;
+    }
+    if (!setup_from_config(slave_config_))
+    {
+        return false;
+    }
+    return true;
 }
 
 /** returns device state based upon the status_word */
 DeviceState EcCiA402Drive::deviceState(uint16_t status_word)
 {
-  if ((status_word & 0b01001111) == 0b00000000) {
-    return STATE_NOT_READY_TO_SWITCH_ON;
-  } else if ((status_word & 0b01001111) == 0b01000000) {
-    return STATE_SWITCH_ON_DISABLED;
-  } else if ((status_word & 0b01101111) == 0b00100001) {
-    return STATE_READY_TO_SWITCH_ON;
-  } else if ((status_word & 0b01101111) == 0b00100011) {
-    return STATE_SWITCH_ON;
-  } else if ((status_word & 0b01101111) == 0b00100111) {
-    return STATE_OPERATION_ENABLED;
-  } else if ((status_word & 0b01101111) == 0b00000111) {
-    return STATE_QUICK_STOP_ACTIVE;
-  } else if ((status_word & 0b01001111) == 0b00001111) {
-    return STATE_FAULT_REACTION_ACTIVE;
-  } else if ((status_word & 0b01001111) == 0b00001000) {
-    return STATE_FAULT;
-  }
-  return STATE_UNDEFINED;
+    if ((status_word & 0b01001111) == 0b00000000)
+    {
+        return STATE_NOT_READY_TO_SWITCH_ON;
+    }
+    else if ((status_word & 0b01001111) == 0b01000000)
+    {
+        return STATE_SWITCH_ON_DISABLED;
+    }
+    else if ((status_word & 0b01101111) == 0b00100001)
+    {
+        return STATE_READY_TO_SWITCH_ON;
+    }
+    else if ((status_word & 0b01101111) == 0b00100011)
+    {
+        return STATE_SWITCH_ON;
+    }
+    else if ((status_word & 0b01101111) == 0b00100111)
+    {
+        return STATE_OPERATION_ENABLED;
+    }
+    else if ((status_word & 0b01101111) == 0b00000111)
+    {
+        return STATE_QUICK_STOP_ACTIVE;
+    }
+    else if ((status_word & 0b01001111) == 0b00001111)
+    {
+        return STATE_FAULT_REACTION_ACTIVE;
+    }
+    else if ((status_word & 0b01001111) == 0b00001000)
+    {
+        return STATE_FAULT;
+    }
+    return STATE_UNDEFINED;
 }
 
 /** returns the control word that will take device from state to next desired state */
 uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word)
 {
-  switch (state) {
-    case STATE_START:                     // -> STATE_NOT_READY_TO_SWITCH_ON (automatic)
-      return control_word;
-    case STATE_NOT_READY_TO_SWITCH_ON:    // -> STATE_SWITCH_ON_DISABLED (automatic)
-      return control_word;
-    case STATE_SWITCH_ON_DISABLED:        // -> STATE_READY_TO_SWITCH_ON
-      return (control_word & 0b01111110) | 0b00000110;
-    case STATE_READY_TO_SWITCH_ON:        // -> STATE_SWITCH_ON
-      return (control_word & 0b01110111) | 0b00000111;
-    case STATE_SWITCH_ON:                 // -> STATE_OPERATION_ENABLED
-      return (control_word & 0b01111111) | 0b00001111;
-    case STATE_OPERATION_ENABLED:         // -> GOOD
-      return control_word;
-    case STATE_QUICK_STOP_ACTIVE:         // -> STATE_OPERATION_ENABLED
-      return (control_word & 0b01111111) | 0b00001111;
-    case STATE_FAULT_REACTION_ACTIVE:     // -> STATE_FAULT (automatic)
-      return control_word;
-    case STATE_FAULT:                     // -> STATE_SWITCH_ON_DISABLED
-      if ((auto_fault_reset_ || fault_reset_) && !temperature_fault_detected_) {
-        fault_reset_ = false;
-        return (control_word & 0b11111111) | 0b10000000;     // automatic reset
-      } else {
+    switch (state)
+    {
+    case STATE_START: // -> STATE_NOT_READY_TO_SWITCH_ON (automatic)
+        return control_word;
+    case STATE_NOT_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON_DISABLED (automatic)
+        return control_word;
+    case STATE_SWITCH_ON_DISABLED: // -> STATE_READY_TO_SWITCH_ON
+        return (control_word & 0b01111110) | 0b00000110;
+    case STATE_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON
+        return (control_word & 0b01110111) | 0b00000111;
+    case STATE_SWITCH_ON: // -> STATE_OPERATION_ENABLED
+        return (control_word & 0b01111111) | 0b00001111;
+    case STATE_OPERATION_ENABLED: // -> GOOD
         return control_word;
-      }
+    case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED
+        return (control_word & 0b01111111) | 0b00001111;
+    case STATE_FAULT_REACTION_ACTIVE: // -> STATE_FAULT (automatic)
+        return control_word;
+    case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED
+        if ((auto_fault_reset_ || fault_reset_) && !temperature_fault_detected_)
+        {
+            fault_reset_ = false;
+            return (control_word & 0b11111111) | 0b10000000; // automatic reset
+        }
+        else
+        {
+            return control_word;
+        }
     default:
-      break;
-  }
-  return control_word;
+        break;
+    }
+    return control_word;
 }
 
-}  // namespace ethercat_generic_plugins
+} // namespace ethercat_generic_plugins
 
 #include <pluginlib/class_list_macros.hpp>
 

From 083aab124d9499e6db07476729d3913a7ca7d8c5 Mon Sep 17 00:00:00 2001
From: Yuriy Strezhik <ius@skl.vc>
Date: Thu, 17 Jul 2025 17:59:01 +0300
Subject: [PATCH 04/20] add optional CONDA_PREFIX to Etherlab files path

---
 ethercat_interface/CMakeLists.txt | 7 +++++--
 ethercat_manager/CMakeLists.txt   | 7 +++++--
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/ethercat_interface/CMakeLists.txt b/ethercat_interface/CMakeLists.txt
index 3843fe6e..d77f2e41 100644
--- a/ethercat_interface/CMakeLists.txt
+++ b/ethercat_interface/CMakeLists.txt
@@ -11,9 +11,12 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(rclcpp REQUIRED)
 
 # EtherLab
-set(ETHERLAB_DIR /usr/local/etherlab)
+if(DEFINED ENV{CONDA_PREFIX})
+  set(ETHERLAB_DIR $ENV{CONDA_PREFIX}/usr/local/etherlab)
+else()
+  set(ETHERLAB_DIR /usr/local/etherlab)
+endif()
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-
 find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib)
 
 ament_export_include_directories(
diff --git a/ethercat_manager/CMakeLists.txt b/ethercat_manager/CMakeLists.txt
index eef818f6..29dc4db1 100644
--- a/ethercat_manager/CMakeLists.txt
+++ b/ethercat_manager/CMakeLists.txt
@@ -6,9 +6,12 @@ find_package(rclcpp REQUIRED)
 find_package(ethercat_msgs REQUIRED)
 
 # EtherLab
-set(ETHERLAB_DIR /usr/local/etherlab)
+if(DEFINED ENV{CONDA_PREFIX})
+  set(ETHERLAB_DIR $ENV{CONDA_PREFIX}/usr/local/etherlab)
+else()
+  set(ETHERLAB_DIR /usr/local/etherlab)
+endif()
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-
 find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib)
 
 ament_export_include_directories(

From 1eab6daf7d1df5bf604f10550f4ae48d07c652f4 Mon Sep 17 00:00:00 2001
From: Yuriy Strezhik <ius@skl.vc>
Date: Fri, 18 Jul 2025 02:39:17 +0300
Subject: [PATCH 05/20] add required header for correct build

---
 ethercat_manager/include/ethercat_manager/ec_master_async.hpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/ethercat_manager/include/ethercat_manager/ec_master_async.hpp b/ethercat_manager/include/ethercat_manager/ec_master_async.hpp
index d1a8a86a..f603322e 100644
--- a/ethercat_manager/include/ethercat_manager/ec_master_async.hpp
+++ b/ethercat_manager/include/ethercat_manager/ec_master_async.hpp
@@ -21,6 +21,7 @@
 #include <fcntl.h>
 #include <sys/ioctl.h>
 #include <stdio.h>
+#include <unistd.h>
 #include <ecrt.h>
 #include <errno.h>
 #include <sstream>

From 077bd5305fac0f934ca20bf19c19107607c6822c Mon Sep 17 00:00:00 2001
From: Muhammad Awais Rafique <mraf@skl.vc>
Date: Wed, 13 Aug 2025 00:08:27 +0000
Subject: [PATCH 06/20] Added limits functionality for command interfaces

1. Updated the ec_pdo_channel_manager.hpp file to incorporate the new limit parameters and apply them to the command interface values before sending them to the actuators.
2. Updated the Sphinx documentation to include descriptions of the new parameters in the configuration guide.
3. Modified the CST versions of the actuator YAML configuration files for the eRob70F, eRob80F, eRob90F, eRob142F and their reverses to include peak_upper_limit, peak_lower_limit, and peak_limit_multiplier parameters for the effort command interface. This change is done in the hmnd repository where this repo is a submodule.
4. Verified changes by running and testing with hardware.
---
 .../user_guide/config_generic_slave.rst       |  6 +++
 .../ec_pdo_channel_manager.hpp                | 45 +++++++++++++++----
 2 files changed, 42 insertions(+), 9 deletions(-)

diff --git a/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst b/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst
index 51a4ce0a..6347c179 100644
--- a/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst
+++ b/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst
@@ -83,6 +83,12 @@ Each PDO Channel has the following configuration flags:
     - Data conversion factor/scale (:code:`type` : :code:`double`).
   * - :code:`offset`
     - Data offset term (:code:`type` : :code:`double`).
+  * - :code:`peak_upper_limit`
+    - Peak Upper limit (:code:`type` : :code:`double`) applied to command_interface as saturation before multiplying factor and adding offset. default value is :code:`6.2831853` (2*PI).
+  * - :code:`peak_lower_limit`
+    - Peak Lower limit (:code:`type` : :code:`double`) applied to command_interface as saturation before multiplying factor and adding offset. default value is :code:`-6.2831853` (-2*PI).
+  * - :code:`peak_limit_multiplier`
+    - Peak limit multiplier (:code:`type` : :code:`double`) applied to peak limits to scale them slightly below the actual actuator limits (specified in peak upper/lower limits). This is useful to avoid hitting the limits during operation.
 
 
 .. warning:: For each channel, tags :code:`index`, :code:`sub_index` and :code:`type` are **mandatory** even if the channel is not used in order to fill the data layout expected by the module. All other tags can remain unset.
diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp
index f6981d93..4b302e0a 100644
--- a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp
+++ b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp
@@ -1,3 +1,6 @@
+// Modified by: Muhammad Awais Rafique (mraf@skl.vc), Peyman Karimi Eskandari (pkes@skl.vc)
+// Modification: Added peak limits to specify the torque limits in the EtherCAT PDO channel manager before sending them to actuator drivers. The limits do not apply to CSP unless explicitly specified in the ethercat_config .yaml files because the default upper and lower limit is 2*pi and -2*pi which is the maximum position range.
+
 // Copyright 2023 ICUBE Laboratory, University of Strasbourg
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
@@ -21,6 +24,7 @@
 #include <string>
 #include <vector>
 #include <limits>
+#include <algorithm>
 
 #include "yaml-cpp/yaml.h"
 
@@ -109,19 +113,24 @@ class EcPdoChannelManager
   void ec_update(uint8_t * domain_address)
   {
     // update state interface
-    if (pdo_type == TPDO) {
+    if (pdo_type == TPDO)
+    {
       ec_read(domain_address);
-      if (interface_index >= 0) {
+      if (interface_index >= 0)
+      {
         state_interface_ptr_->at(interface_index) = last_value;
       }
-    } else if (pdo_type == RPDO && allow_ec_write) {
-      if (interface_index >= 0 &&
-        !std::isnan(command_interface_ptr_->at(interface_index)) &&
-        !override_command)
+    } 
+    else if (pdo_type == RPDO && allow_ec_write)
+    {
+      if (interface_index >= 0 && !std::isnan(command_interface_ptr_->at(interface_index)) && !override_command)
+      {
+        ec_write(domain_address, factor * std::clamp(command_interface_ptr_->at(interface_index), lower_limit, upper_limit) + offset);
+      } 
+      else 
       {
-        ec_write(domain_address, factor * command_interface_ptr_->at(interface_index) + offset);
-      } else {
-        if (!std::isnan(default_value)) {
+        if (!std::isnan(default_value))
+        {
           ec_write(domain_address, default_value);
         }
       }
@@ -179,6 +188,21 @@ class EcPdoChannelManager
       data_mask = channel_config["mask"].as<uint8_t>();
     }
 
+    if (channel_config["peak_limit_multiplier"]) {
+      limit_multiplier = std::abs(channel_config["peak_limit_multiplier"].as<double>());
+    }
+
+    // lower limit
+    if (channel_config["peak_lower_limit"]) {
+      lower_limit = limit_multiplier * channel_config["peak_lower_limit"].as<double>();
+    }
+    // upper limit
+    if (channel_config["peak_upper_limit"]) {
+      upper_limit = limit_multiplier * channel_config["peak_upper_limit"].as<double>();
+    }
+
+
+
     return true;
   }
 
@@ -217,6 +241,9 @@ class EcPdoChannelManager
   double factor = 1;
   double offset = 0;
 
+  double upper_limit = 6.2831853; // Set to 2*PI by default, all torque limits are lower than this value
+  double lower_limit = -6.2831853; // Set to -2*PI by default, all torque limits are higher than this value
+  double limit_multiplier = 0.95; // Used to scale the peak limits, only applies if peak limits are set in the yaml file, otherwise the default limits are used.
 private:
   std::vector<double> * command_interface_ptr_;
   std::vector<double> * state_interface_ptr_;

From 003a811698f54f8036c570f802ea12be76b56740 Mon Sep 17 00:00:00 2001
From: Andy Park <andypark@thehumanoid.ai>
Date: Tue, 19 Aug 2025 22:22:26 +0000
Subject: [PATCH 07/20] The simple-but-effective bug fix

-- check `is_operational` in the condition when setting current to the target position
---
 .../src/generic_ec_cia402_drive.cpp                | 14 ++------------
 1 file changed, 2 insertions(+), 12 deletions(-)

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 a4125bcd..9a0514d0 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
@@ -91,22 +91,12 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
     if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION)
     {
         double current_position = pdo_channels_info_[index].last_value;
-        // Initialize last_position_ if it hasn't been set yet (first time)
-        if (std::isnan(last_position_))
+        // Initialize last_position_ if it hasn't been set yet (first time) and the drive is operational
+        if (std::isnan(last_position_) && is_operational_)
         {
             last_position_ = current_position;
             std::cout << "Position initialized: " << last_position_ << std::endl;
         }
-        else
-        {
-            // Only update last_position_ if the change is significant (prevents drifting)
-            double position_threshold = 0.0001; // Adjust this threshold as needed
-            double change = std::abs(current_position - last_position_);
-            if (change > position_threshold)
-            {
-                last_position_ = current_position;
-            }
-        }
     }
 
     // Special case: StatusWord

From 8cf89d35a38f8409d71a0e4f870cff1472c434c6 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Fri, 26 Sep 2025 14:14:24 +0100
Subject: [PATCH 08/20] add is operational for driver

---
 ethercat_interface/include/ethercat_interface/ec_slave.hpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/ethercat_interface/include/ethercat_interface/ec_slave.hpp b/ethercat_interface/include/ethercat_interface/ec_slave.hpp
index da88a826..99104454 100644
--- a/ethercat_interface/include/ethercat_interface/ec_slave.hpp
+++ b/ethercat_interface/include/ethercat_interface/ec_slave.hpp
@@ -41,6 +41,7 @@ class EcSlave
   virtual const ec_sync_info_t * syncs() {return NULL;}
   virtual bool initialized() {return true;}
   virtual void set_state_is_operational(bool value) {is_operational_ = value;}
+  bool is_operational() const { return is_operational_; }
   /** Assign activate DC synchronization. return activate word*/
   virtual int assign_activate_dc_sync() {return 0x00;}
   /** number of elements in the syncs array. */

From 9c725091fb9610b4217cc3e02e8d95366a021e14 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Wed, 1 Oct 2025 14:06:13 +0100
Subject: [PATCH 09/20] trigger quick stop to all if any one in error

---
 .../generic_ec_cia402_drive.hpp               |  4 ++++
 .../src/generic_ec_cia402_drive.cpp           | 20 +++++++++++++++++--
 2 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
index 77e3bc60..eb822c2a 100644
--- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
@@ -21,6 +21,7 @@
 #include <string>
 #include <unordered_map>
 #include <limits>
+#include <atomic>
 
 #include "yaml-cpp/yaml.h"
 #include "ethercat_interface/ec_slave.hpp"
@@ -51,6 +52,9 @@ class EcCiA402Drive : public GenericEcSlave
   int8_t mode_of_operation_ = -1;
 
 protected:
+  // Global aggregated count of faulted instances (process-wide)
+  static std::atomic<int> num_faulted_;
+
   uint32_t counter_ = 0;
   uint16_t last_status_word_ = -1;
   uint16_t status_word_ = 0;
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 9a0514d0..c59eea60 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
@@ -21,6 +21,8 @@
 namespace ethercat_generic_plugins
 {
 
+std::atomic<int> EcCiA402Drive::num_faulted_{0};
+
 EcCiA402Drive::EcCiA402Drive()
     : GenericEcSlave()
 {
@@ -53,8 +55,12 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
 
             if (auto_state_transitions_)
             {
-                pdo_channels_info_[index].default_value =
-                    transition(state_, pdo_channels_info_[index].ec_read(domain_address));
+                uint16_t cw = transition(state_, pdo_channels_info_[index].ec_read(domain_address));
+                // If any instance is faulted, force quick-stop controlword
+                if (num_faulted_.load(std::memory_order_relaxed) > 0) {
+                    cw = (cw & 0b01111110) | 0b00000110; // safe quick-stop path
+                }
+                pdo_channels_info_[index].default_value = cw;
             }
         }
     }
@@ -111,9 +117,19 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
         uint16_t new_error_code = pdo_channels_info_[index].last_value;
         if (new_error_code != error_code_)
         {
+            uint16_t prev = error_code_;
             error_code_ = new_error_code;
             std::cout << "Error Code: " << std::hex << error_code_ << std::dec << std::endl;
 
+            // Update registry counters
+            bool was_faulted = (prev != 0);
+            bool now_faulted = (error_code_ != 0);
+            if (!was_faulted && now_faulted) {
+                num_faulted_.fetch_add(1, std::memory_order_relaxed);
+            } else if (was_faulted && !now_faulted) {
+                num_faulted_.fetch_sub(1, std::memory_order_relaxed);
+            }
+
             // During startup, clear any stored temperature faults
             if (counter_ < 100)
             { // First 100 cycles = startup phase

From dcb9d4aacd4b30b1e865bb63ac0c64208fdee0a8 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Wed, 1 Oct 2025 20:23:45 +0100
Subject: [PATCH 10/20] allow resetting may be

---
 .../src/generic_ec_cia402_drive.cpp                        | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

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 c59eea60..85c55339 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
@@ -56,9 +56,12 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
             if (auto_state_transitions_)
             {
                 uint16_t cw = transition(state_, pdo_channels_info_[index].ec_read(domain_address));
-                // If any instance is faulted, force quick-stop controlword
+                // If any instance is faulted, force quick-stop only on non-faulted drives
                 if (num_faulted_.load(std::memory_order_relaxed) > 0) {
-                    cw = (cw & 0b01111110) | 0b00000110; // safe quick-stop path
+                    if (error_code_ == 0) {
+                        cw = (cw & 0b01111110) | 0b00000110; // safe quick-stop path for healthy drives
+                    }
+                    // If this drive is faulted (error_code_ != 0), do not override so reset can proceed
                 }
                 pdo_channels_info_[index].default_value = cw;
             }

From ef6574a842081f774f46e17a86af4257bb38636c Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Mon, 6 Oct 2025 19:41:37 +0100
Subject: [PATCH 11/20] check status and error code both to prevent boot up
 chaos

---
 .../src/generic_ec_cia402_drive.cpp                         | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

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 85c55339..d2407560 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
@@ -58,8 +58,10 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
                 uint16_t cw = transition(state_, pdo_channels_info_[index].ec_read(domain_address));
                 // If any instance is faulted, force quick-stop only on non-faulted drives
                 if (num_faulted_.load(std::memory_order_relaxed) > 0) {
-                    if (error_code_ == 0) {
-                        cw = (cw & 0b01111110) | 0b00000110; // safe quick-stop path for healthy drives
+                    if ((error_code_ == 0) && ((status_word_ & 0x07) == 0x07)) { // & status 7 = 7 (first 3 bits always 1)
+                        // Clear bit7 and bit0, set bit2 and bit1 (CiA-402 Shutdown)
+                        cw = (cw & 0x7E) | 0x06;   // same as (cw & 0b01111110) | 0b00000110
+                        // safe quick-stop path for healthy drives, this is writing 6 to the actuators. @babak 
                     }
                     // If this drive is faulted (error_code_ != 0), do not override so reset can proceed
                 }

From 49c1ef6315eb41a00f3fdfd6cec3ff6a4490f145 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Mon, 6 Oct 2025 19:37:17 +0000
Subject: [PATCH 12/20] rever status word check | causes weird sound to the
 actuators

---
 .../src/generic_ec_cia402_drive.cpp                             | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 d2407560..5f41201d 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
@@ -58,7 +58,7 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
                 uint16_t cw = transition(state_, pdo_channels_info_[index].ec_read(domain_address));
                 // If any instance is faulted, force quick-stop only on non-faulted drives
                 if (num_faulted_.load(std::memory_order_relaxed) > 0) {
-                    if ((error_code_ == 0) && ((status_word_ & 0x07) == 0x07)) { // & status 7 = 7 (first 3 bits always 1)
+                    if (error_code_ == 0) { // && ((status_word_ & 0x07) == 0x07)) { // & status 7 = 7 (first 3 bits always 1) this induces weird sound while resetting error!!! 
                         // Clear bit7 and bit0, set bit2 and bit1 (CiA-402 Shutdown)
                         cw = (cw & 0x7E) | 0x06;   // same as (cw & 0b01111110) | 0b00000110
                         // safe quick-stop path for healthy drives, this is writing 6 to the actuators. @babak 

From ed381167d13b664b2ce494200ad7667e610664c1 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Tue, 7 Oct 2025 09:57:59 +0000
Subject: [PATCH 13/20] only uses error code, status word is weird

---
 .../src/generic_ec_cia402_drive.cpp                             | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 5f41201d..683cfe33 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
@@ -58,7 +58,7 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
                 uint16_t cw = transition(state_, pdo_channels_info_[index].ec_read(domain_address));
                 // If any instance is faulted, force quick-stop only on non-faulted drives
                 if (num_faulted_.load(std::memory_order_relaxed) > 0) {
-                    if (error_code_ == 0) { // && ((status_word_ & 0x07) == 0x07)) { // & status 7 = 7 (first 3 bits always 1) this induces weird sound while resetting error!!! 
+                    if (error_code_ == 0) {  
                         // Clear bit7 and bit0, set bit2 and bit1 (CiA-402 Shutdown)
                         cw = (cw & 0x7E) | 0x06;   // same as (cw & 0b01111110) | 0b00000110
                         // safe quick-stop path for healthy drives, this is writing 6 to the actuators. @babak 

From 51374d3d8291813468db926328696cec7ad640c4 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Tue, 7 Oct 2025 10:45:09 +0000
Subject: [PATCH 14/20] check is operational for the whole chain

---
 ethercat_driver/src/ethercat_driver.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp
index b1a73675..a964f123 100644
--- a/ethercat_driver/src/ethercat_driver.cpp
+++ b/ethercat_driver/src/ethercat_driver.cpp
@@ -339,12 +339,12 @@ CallbackReturn EthercatDriver::on_activate(
     // update EtherCAT bus
 
     master_->update();
-    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
+    // RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
 
     // check if operational
     bool isAllInit = true;
     for (auto & module : ec_modules_) {
-      isAllInit = isAllInit && module->initialized();
+      isAllInit = isAllInit && module->initialized() && module->is_operational();
     }
     if (isAllInit) {
       running = false;

From 76fdfd15d7e5a8040efbb51d46fdd4567dacc72a Mon Sep 17 00:00:00 2001
From: Andy Park <andypark@thehumanoid.ai>
Date: Thu, 16 Oct 2025 20:28:00 +0000
Subject: [PATCH 15/20] undo some change based on hardware testing

---
 ethercat_driver/src/ethercat_driver.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp
index a964f123..b1a73675 100644
--- a/ethercat_driver/src/ethercat_driver.cpp
+++ b/ethercat_driver/src/ethercat_driver.cpp
@@ -339,12 +339,12 @@ CallbackReturn EthercatDriver::on_activate(
     // update EtherCAT bus
 
     master_->update();
-    // RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
+    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
 
     // check if operational
     bool isAllInit = true;
     for (auto & module : ec_modules_) {
-      isAllInit = isAllInit && module->initialized() && module->is_operational();
+      isAllInit = isAllInit && module->initialized();
     }
     if (isAllInit) {
       running = false;

From e0f55dd362ca30b78e472102d5cebb3df7f37ebe Mon Sep 17 00:00:00 2001
From: Andy Park <andy.park@thehumanoid.ai>
Date: Fri, 24 Oct 2025 09:10:07 +0000
Subject: [PATCH 16/20] Fixed CiA402 initialized function

---
 .../ethercat_generic_plugins/generic_ec_cia402_drive.hpp      | 2 +-
 .../src/generic_ec_cia402_drive.cpp                           | 4 +++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
index 77e3bc60..772f71b0 100644
--- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
@@ -38,7 +38,7 @@ class EcCiA402Drive : public GenericEcSlave
   virtual ~EcCiA402Drive();
   /** Returns true if drive has reached "operation enabled" state.
    *  The transition through the state machine is handled automatically. */
-  bool initialized() const;
+  bool initialized() override;
 
   virtual void processData(size_t index, uint8_t * domain_address);
 
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 9a0514d0..03eccd1a 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
@@ -27,7 +27,9 @@ EcCiA402Drive::EcCiA402Drive()
 }
 EcCiA402Drive::~EcCiA402Drive() {}
 
-bool EcCiA402Drive::initialized() const { return initialized_; }
+bool EcCiA402Drive::initialized() { 
+    return initialized_; 
+}
 
 void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
 {

From 0f2beea2a839ef958205ec8e10a307f3866ff598 Mon Sep 17 00:00:00 2001
From: Tobias Jacob <tobi.jacob@gmx.net>
Date: Thu, 30 Oct 2025 13:20:34 +0000
Subject: [PATCH 17/20] Revert "Fixed CiA402 initialized function"

This reverts commit e0f55dd362ca30b78e472102d5cebb3df7f37ebe.
---
 .../ethercat_generic_plugins/generic_ec_cia402_drive.hpp      | 2 +-
 .../src/generic_ec_cia402_drive.cpp                           | 4 +---
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
index 772f71b0..77e3bc60 100644
--- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
@@ -38,7 +38,7 @@ class EcCiA402Drive : public GenericEcSlave
   virtual ~EcCiA402Drive();
   /** Returns true if drive has reached "operation enabled" state.
    *  The transition through the state machine is handled automatically. */
-  bool initialized() override;
+  bool initialized() const;
 
   virtual void processData(size_t index, uint8_t * domain_address);
 
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 03eccd1a..9a0514d0 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
@@ -27,9 +27,7 @@ EcCiA402Drive::EcCiA402Drive()
 }
 EcCiA402Drive::~EcCiA402Drive() {}
 
-bool EcCiA402Drive::initialized() { 
-    return initialized_; 
-}
+bool EcCiA402Drive::initialized() const { return initialized_; }
 
 void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
 {

From 23232f049ae7e5c4ab96b5c9a72880c43e56f943 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Thu, 6 Nov 2025 10:55:49 +0000
Subject: [PATCH 18/20] quick stop toggle config parameter added

---
 .../generic_ec_cia402_drive.hpp                    |  3 +++
 .../src/generic_ec_cia402_drive.cpp                | 14 ++++++++++----
 2 files changed, 13 insertions(+), 4 deletions(-)

diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
index eb822c2a..f8543144 100644
--- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
@@ -71,6 +71,9 @@ class EcCiA402Drive : public GenericEcSlave
   bool temperature_fault_detected_ = false;
   uint16_t error_code_ = 0;
 
+  // Quick stop behavior controls
+  bool exclude_from_global_quick_stop_ = false; // Do not propagate this drive's faults globally if true
+
   /** returns device state based upon the status_word */
   DeviceState deviceState(uint16_t status_word);
   /** returns the control word that will take device from state to next desired state */
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 683cfe33..2acfcf3c 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
@@ -129,10 +129,12 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
             // Update registry counters
             bool was_faulted = (prev != 0);
             bool now_faulted = (error_code_ != 0);
-            if (!was_faulted && now_faulted) {
-                num_faulted_.fetch_add(1, std::memory_order_relaxed);
-            } else if (was_faulted && !now_faulted) {
-                num_faulted_.fetch_sub(1, std::memory_order_relaxed);
+            if (!exclude_from_global_quick_stop_) {
+                if (!was_faulted && now_faulted) {
+                    num_faulted_.fetch_add(1, std::memory_order_relaxed);
+                } else if (was_faulted && !now_faulted) {
+                    num_faulted_.fetch_sub(1, std::memory_order_relaxed);
+                }
             }
 
             // During startup, clear any stored temperature faults
@@ -234,6 +236,10 @@ bool EcCiA402Drive::setup_from_config(YAML::Node drive_config)
     {
         auto_state_transitions_ = drive_config["auto_state_transitions"].as<bool>();
     }
+    if (drive_config["exclude_from_global_quick_stop"])
+    {
+        exclude_from_global_quick_stop_ = drive_config["exclude_from_global_quick_stop"].as<bool>();
+    }
     return true;
 }
 

From 5454a95742d0fe62f76e009544e41014dbb24933 Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Mon, 15 Dec 2025 12:03:30 +0000
Subject: [PATCH 19/20] adding auto state transition command handler

---
 .../generic_ec_cia402_drive.hpp               |  3 +++
 .../src/generic_ec_cia402_drive.cpp           | 26 +++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
index f8543144..654396d8 100644
--- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp
@@ -64,12 +64,15 @@ class EcCiA402Drive : public GenericEcSlave
   bool initialized_ = false;
   bool auto_fault_reset_ = false;
   bool auto_state_transitions_ = true;
+  bool auto_state_transitions_cmd_ = false;
   bool fault_reset_ = false;
   int fault_reset_command_interface_index_ = -1;
   bool last_fault_reset_command_ = false;
   double last_position_ = std::numeric_limits<double>::quiet_NaN();
   bool temperature_fault_detected_ = false;
   uint16_t error_code_ = 0;
+  int auto_state_cmd_index_ = -1;
+  bool last_auto_state_cmd_ = false;
 
   // Quick stop behavior controls
   bool exclude_from_global_quick_stop_ = false; // Do not propagate this drive's faults globally if true
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 2acfcf3c..d5f88a2a 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
@@ -67,6 +67,20 @@ void EcCiA402Drive::processData(size_t index, uint8_t *domain_address)
                 }
                 pdo_channels_info_[index].default_value = cw;
             }
+            if (auto_state_cmd_index_ >= 0)
+            {
+                if (command_interface_ptr_->at(auto_state_cmd_index_) == 0)
+                {
+                    last_auto_state_cmd_ = false;
+                }
+                if (last_auto_state_cmd_ == false &&
+                    command_interface_ptr_->at(auto_state_cmd_index_) != 0 &&
+                    !std::isnan(command_interface_ptr_->at(auto_state_cmd_index_)))
+                {
+                    last_auto_state_cmd_ = true;
+                    auto_state_transitions_cmd_ = true;
+                }
+            }
         }
     }
 
@@ -218,6 +232,11 @@ bool EcCiA402Drive::setupSlave(std::unordered_map<std::string, std::string> slav
         fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]);
     }
 
+    if (paramters_.find("command_interface/auto_state_transitions") != paramters_.end())
+    {
+        auto_state_cmd_index_ = std::stoi(paramters_["command_interface/auto_state_transitions"]);
+    }
+
     return true;
 }
 
@@ -319,7 +338,14 @@ uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word)
     case STATE_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON
         return (control_word & 0b01110111) | 0b00000111;
     case STATE_SWITCH_ON: // -> STATE_OPERATION_ENABLED
+        if (auto_state_transitions_cmd_)
+        {
         return (control_word & 0b01111111) | 0b00001111;
+        }
+        else
+        {
+            return (control_word & 0b01110111) | 0b00000111;
+        }
     case STATE_OPERATION_ENABLED: // -> GOOD
         return control_word;
     case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED

From 11f761875fb3ad3b103ec266cf24398289f0250c Mon Sep 17 00:00:00 2001
From: anubhav-dogra <anubhavdograa@gmail.com>
Date: Thu, 5 Feb 2026 16:08:52 +0000
Subject: [PATCH 20/20] valid check and release in destructor

---
 ethercat_interface/include/ethercat_interface/ec_master.hpp | 2 ++
 ethercat_interface/src/ec_master.cpp                        | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp
index 26ae64f0..a3b1107e 100644
--- a/ethercat_interface/include/ethercat_interface/ec_master.hpp
+++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp
@@ -90,6 +90,8 @@ class EcMaster
   void readData(uint32_t domain = 0);
   void writeData(uint32_t domain = 0);
 
+  bool isValid() const { return master_ != NULL; }
+
 private:
   /** true if running */
   volatile bool running_ = false;
diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_interface/src/ec_master.cpp
index db31e680..390d99ca 100644
--- a/ethercat_interface/src/ec_master.cpp
+++ b/ethercat_interface/src/ec_master.cpp
@@ -74,6 +74,10 @@ EcMaster::~EcMaster()
       delete domain.second;
     }
   }
+  if (master_ != NULL) {
+    ecrt_release_master(master_);
+    master_ = NULL;
+  }
 }
 
 void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave)