From c503b7bd68cc8e1dae2b37c1a5ceefb3ac41739e Mon Sep 17 00:00:00 2001
From: mcbed <mcbed.robotics@gmail.com>
Date: Tue, 27 Jun 2023 12:11:19 +0200
Subject: [PATCH] fixing gap management issue in pdo mapping

---
 .../ethercat_generic_plugins/generic_ec_slave.hpp   |  1 +
 .../ethercat_generic_slave/src/generic_ec_slave.cpp | 13 +++++++++----
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp b/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp
index 85f801c..9d733ee 100644
--- a/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp
+++ b/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp
@@ -56,6 +56,7 @@ class GenericEcSlave : public ethercat_interface::EcSlave
   std::vector<ethercat_interface::EcPdoChannelManager> pdo_channels_info_;
   std::vector<ethercat_interface::SMConfig> sm_configs_;
   std::vector<ec_sync_info_t> syncs_;
+  std::vector<unsigned int> domain_map_;
   YAML::Node slave_config_;
   uint32_t assign_activate_ = 0;
 
diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp
index cd8dcbc..7514cf0 100644
--- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp
+++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp
@@ -42,7 +42,7 @@ int GenericEcSlave::assign_activate_dc_sync() {return assign_activate_;}
 
 void GenericEcSlave::processData(size_t index, uint8_t * domain_address)
 {
-  pdo_channels_info_[index].ec_update(domain_address);
+  pdo_channels_info_[domain_map_[index]].ec_update(domain_address);
 }
 
 const ec_sync_info_t * GenericEcSlave::syncs()
@@ -59,9 +59,7 @@ const ec_pdo_entry_info_t * GenericEcSlave::channels()
 }
 void GenericEcSlave::domains(DomainMap & domains) const
 {
-  std::vector<unsigned int> v(all_channels_.size());
-  std::iota(std::begin(v), std::end(v), 0);
-  domains = {{0, v}};
+  domains = {{0, domain_map_}};
 }
 
 void GenericEcSlave::setup_syncs()
@@ -205,6 +203,13 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config)
       }
     }
 
+    // Remove gaps from domain mapping
+    for (auto i = 0ul; i < all_channels_.size(); i++) {
+      if (all_channels_[i].index != 0x0000) {
+        domain_map_.push_back(i);
+      }
+    }
+
     return true;
   } else {
     std::cerr << "GenericEcSlave: failed to load slave configuration: empty configuration" <<