fixing gap management issue in pdo mapping

ICube-Robotics · Jun 27, 2023 · c503b7b · c503b7b
1 parent 6fa5f6e
commit c503b7b
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
diff --git a/...eric_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp b/...eric_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
@@ -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" <<