Adds multi domain support

ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp

@@ -40,7 +40,7 @@ class EcCiA402Drive : public GenericEcSlave
    *  The transition through the state machine is handled automatically. */
   bool initialized();
 
-  virtual void processData(size_t entry_idx, uint8_t * domain_address);
+  virtual void processData(unsigned int domain_index, size_t entry_idx, uint8_t * domain_address);
 
   virtual bool setupSlave(
     std::unordered_map<std::string, std::string> slave_parameters,

ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp

@@ -47,9 +47,17 @@ void EcCiA402Drive::updateState()
   initialized_ = is_operational_;
 }
 
-void EcCiA402Drive::processData(size_t entry_idx, uint8_t * domain_address)
+void EcCiA402Drive::processData(unsigned int domain_index, size_t entry_idx, uint8_t * domain_address)
 {
-  auto index = domain_map_[entry_idx];
+  (void)domain_index; // TODO add multi-domain-support
+  auto it = domains_.find(domain_index);
+  if (it == domains_.end()) {
+    throw std::runtime_error("Missing domain");
+  }
+
+  const std::vector<unsigned int>& entry_map = it->second;
+  auto index = entry_map[entry_idx];
+
   ethercat_interface::EcPdoSingleInterfaceChannelManager * channel_ptr =
     static_cast<
     ethercat_interface::EcPdoSingleInterfaceChannelManager *>(
@@ -114,7 +122,7 @@ void EcCiA402Drive::processData(size_t entry_idx, uint8_t * domain_address)
 
 
   // CHECK FOR STATE CHANGE
-  if (entry_idx == domain_map_.size() - 1) {  // if last entry in domain
+  if (entry_idx == entry_map.size() - 1) {  // if last entry in domain
     updateState();
   }
 }

ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp

@@ -191,7 +191,7 @@ TEST_F(EcCiA402DriveTest, EcReadTPDOToStateInterface)
   ASSERT_EQ(plugin_->pdo_channels_info_[8]->state_interface_index(), 1);
   uint8_t domain_address[2];
   EC_WRITE_S16(domain_address, 42);
-  plugin_->processData(8, domain_address);
+  plugin_->processData(0, 8, domain_address);
   ASSERT_EQ(plugin_->state_interface_ptr_->at(1), 42);
 }
 
@@ -209,7 +209,7 @@ TEST_F(EcCiA402DriveTest, EcWriteRPDOFromCommandInterface)
   ASSERT_EQ(channels[2]->command_interface_index(), 1);
   plugin_->mode_of_operation_display_ = 10;
   uint8_t domain_address[2];
-  plugin_->processData(2, domain_address);
+  plugin_->processData(0, 2, domain_address);
   ASSERT_EQ(channels[2]->data().last_value, 42);
   ASSERT_EQ(EC_READ_S16(domain_address), 42);
 }
@@ -221,7 +221,7 @@ TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue)
   plugin_->setup_interface_mapping();
   plugin_->mode_of_operation_display_ = 10;
   uint8_t domain_address[2];
-  plugin_->processData(2, domain_address);
+  plugin_->processData(0, 2, domain_address);
   auto channels = plugin_->pdo_channels_info_;
   ASSERT_EQ(channels[2]->data().last_value, -5);
   ASSERT_EQ(EC_READ_S16(domain_address), -5);
@@ -243,15 +243,15 @@ TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue)
 //   ASSERT_FALSE(plugin_->fault_reset_);
 //   ASSERT_EQ(plugin_->command_interface_ptr_->at(
 //     plugin_->fault_reset_command_interface_index_), 1);
-//   plugin_->processData(4, &domain_address);
+//   plugin_->processData(0, 4, &domain_address);
 //   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b10000000);
 //   plugin_->pdo_channels_info_[4].last_value = 0;
-//   plugin_->processData(4, &domain_address);
+//   plugin_->processData(0, 4, &domain_address);
 //   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b00000000);
 //   command_interface[1] = 0;
-//   plugin_->processData(4, &domain_address);
+//   plugin_->processData(0, 4, &domain_address);
 //   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b00000000);
-//   command_interface[1] = 2;  plugin_->processData(4, &domain_address);
+//   command_interface[1] = 2;  plugin_->processData(0, 4, &domain_address);
 //   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b10000000);
 // }
 
@@ -268,11 +268,11 @@ TEST_F(EcCiA402DriveTest, SwitchModeOfOperation)
   plugin_->setup_interface_mapping();
   plugin_->is_operational_ = true;
   uint8_t domain_address[2];
-  plugin_->processData(5, domain_address);
+  plugin_->processData(0, 5, domain_address);
   ASSERT_EQ(EC_READ_S8(domain_address), 8);
   command_interface[1] = 9;
-  plugin_->processData(5, domain_address);
-  plugin_->processData(10, domain_address);
+  plugin_->processData(0, 5, domain_address);
+  plugin_->processData(0, 10, domain_address);
   ASSERT_EQ(EC_READ_S8(domain_address), 9);
   ASSERT_EQ(plugin_->mode_of_operation_display_, 9);
 }
@@ -293,32 +293,32 @@ TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition)
   uint8_t domain_address[4];
   uint8_t domain_address_moo[2];
 
-  plugin_->processData(5, domain_address_moo);  // mode_of_operation
-  plugin_->processData(10, domain_address_moo);  // mode_of_operation_display
+  plugin_->processData(0, 5, domain_address_moo);  // mode_of_operation
+  plugin_->processData(0, 10, domain_address_moo);  // mode_of_operation_display
   ASSERT_EQ(plugin_->mode_of_operation_display_, 8);
 
   EC_WRITE_S32(domain_address, 123456);
-  plugin_->processData(6, domain_address);
+  plugin_->processData(0, 6, domain_address);
   ASSERT_EQ(plugin_->last_position_, 123456);
 
   EC_WRITE_S32(domain_address, 0);
-  plugin_->processData(0, domain_address);
+  plugin_->processData(0, 0, domain_address);
   ASSERT_EQ(EC_READ_S32(domain_address), 123456);
 
   command_interface[1] = 9;
-  plugin_->processData(5, domain_address_moo);
-  plugin_->processData(10, domain_address_moo);
+  plugin_->processData(0, 5, domain_address_moo);
+  plugin_->processData(0, 10, domain_address_moo);
   ASSERT_EQ(plugin_->mode_of_operation_display_, 9);
 
   EC_WRITE_S32(domain_address, 0);
-  plugin_->processData(0, domain_address);
+  plugin_->processData(0, 0, domain_address);
   ASSERT_EQ(EC_READ_S32(domain_address), 123456);
 
   EC_WRITE_S32(domain_address, 654321);
-  plugin_->processData(6, domain_address);
+  plugin_->processData(0, 6, domain_address);
   ASSERT_EQ(plugin_->last_position_, 654321);
 
   EC_WRITE_S32(domain_address, 0);
-  plugin_->processData(0, domain_address);
+  plugin_->processData(0, 0, domain_address);
   ASSERT_EQ(EC_READ_S32(domain_address), 654321);
 }

ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp

@@ -36,7 +36,7 @@ class GenericEcSlave : public ethercat_interface::EcSlave
   virtual ~GenericEcSlave();
   virtual int assign_activate_dc_sync();
 
-  virtual void processData(size_t entry_idx, uint8_t * domain_address);
+  virtual void processData(unsigned int domain_index, size_t entry_idx, uint8_t * domain_address);
 
   virtual const ec_sync_info_t * syncs();
   virtual size_t syncSize();
@@ -53,11 +53,12 @@ class GenericEcSlave : public ethercat_interface::EcSlave
   std::vector<ec_pdo_info_t> rpdos_;
   std::vector<ec_pdo_info_t> tpdos_;
   std::vector<bool> all_channels_skip_list_;
+  std::vector<unsigned int> all_channels_domain_list_;
   std::vector<ec_pdo_entry_info_t> all_channels_;
   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_;
+  DomainMap domains_;
   YAML::Node slave_config_;
   uint32_t assign_activate_ = 0;
 

ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp

@@ -33,9 +33,17 @@ GenericEcSlave::~GenericEcSlave()
 }
 int GenericEcSlave::assign_activate_dc_sync() {return assign_activate_;}
 
-void GenericEcSlave::processData(size_t entry_idx, uint8_t * domain_address)
+void GenericEcSlave::processData(unsigned int domain_index, size_t entry_idx, uint8_t * domain_address)
 {
-  pdo_channels_info_[domain_map_[entry_idx]]->ec_update(domain_address);
+  auto it = domains_.find(domain_index);
+  if (it == domains_.end()) {
+    throw std::runtime_error("Missing domain");
+  }
+
+  const std::vector<unsigned int>& entry_map = it->second;
+  auto index = entry_map[entry_idx];
+
+  pdo_channels_info_[index]->ec_update(domain_address);
 }
 
 const ec_sync_info_t * GenericEcSlave::syncs()
@@ -52,7 +60,7 @@ const ec_pdo_entry_info_t * GenericEcSlave::channels()
 }
 void GenericEcSlave::domains(DomainMap & domains) const
 {
-  domains = {{0, domain_map_}};
+  domains = domains_;
 }
 
 void GenericEcSlave::setup_syncs()
@@ -160,10 +168,15 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config)
 
     all_channels_.reserve(channels_nbr);
     all_channels_skip_list_.reserve(channels_nbr);
+    all_channels_domain_list_.reserve(channels_nbr);
     channels_nbr = 0;
 
     if (slave_config["rpdo"]) {
       for (auto i = 0ul; i < slave_config["rpdo"].size(); i++) {
+        unsigned int domain_index = 0;
+        if (slave_config["rpdo"][i]["domain"]) {
+          domain_index = slave_config["rpdo"][i]["domain"].as<unsigned int>();
+        }
         auto rpdo_channels_size = slave_config["rpdo"][i]["channels"].size();
         for (auto c = 0ul; c < rpdo_channels_size; c++) {
           ethercat_interface::EcPdoChannelManager * channel_info = nullptr;
@@ -179,6 +192,7 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config)
           pdo_channels_info_.push_back(channel_info);
           all_channels_.push_back(channel_info->get_pdo_entry_info());
           all_channels_skip_list_.push_back(channel_info->skip);
+          all_channels_domain_list_.push_back(domain_index);
         }
         rpdos_.push_back(
           {
@@ -193,6 +207,10 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config)
 
     if (slave_config["tpdo"]) {
       for (auto i = 0ul; i < slave_config["tpdo"].size(); i++) {
+        unsigned int domain_index = 0;
+        if (slave_config["tpdo"][i]["domain"]) {
+          domain_index = slave_config["tpdo"][i]["domain"].as<unsigned int>();
+        }
         auto tpdo_channels_size = slave_config["tpdo"][i]["channels"].size();
 
         for (auto c = 0ul; c < tpdo_channels_size; c++) {
@@ -208,6 +226,7 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config)
           pdo_channels_info_.push_back(channel_info);
           all_channels_.push_back(channel_info->get_pdo_entry_info());
           all_channels_skip_list_.push_back(channel_info->skip);
+          all_channels_domain_list_.push_back(domain_index);
         }
         tpdos_.push_back(
           {
@@ -223,7 +242,7 @@ 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 && all_channels_skip_list_[i] != true) {
-        domain_map_.push_back(i);
+        domains_[all_channels_domain_list_[i]].push_back(i);
       }
     }
 

ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp

@@ -193,7 +193,7 @@ TEST_F(GenericEcSlaveTest, EcReadTPDOToStateInterface)
   ASSERT_EQ(plugin_->pdo_channels_info_[8]->state_interface_index(), 1);
   uint8_t domain_address[2];
   EC_WRITE_S16(domain_address, 42);
-  plugin_->processData(8, domain_address);
+  plugin_->processData(0, 8, domain_address);
   ASSERT_EQ(plugin_->state_interface_ptr_->at(1), 5 * 42 + 15);
 }
 
@@ -210,7 +210,7 @@ TEST_F(GenericEcSlaveTest, EcWriteRPDOFromCommandInterface)
   auto channels = plugin_->pdo_channels_info_;
   ASSERT_EQ(channels[2]->command_interface_index(), 1);
   uint8_t domain_address[2];
-  plugin_->processData(2, domain_address);
+  plugin_->processData(0, 2, domain_address);
   ASSERT_EQ(channels[2]->data().last_value, 2 * 42 + 10);
   ASSERT_EQ(EC_READ_S16(domain_address), 2 * 42 + 10);
 }
@@ -221,7 +221,7 @@ TEST_F(GenericEcSlaveTest, EcWriteRPDODefaultValue)
   plugin_->setup_from_config(YAML::Load(test_slave_config));
   plugin_->setup_interface_mapping();
   uint8_t domain_address[2];
-  plugin_->processData(2, domain_address);
+  plugin_->processData(0, 2, domain_address);
   ASSERT_EQ(plugin_->pdo_channels_info_[2]->data().last_value, -5);
   ASSERT_EQ(EC_READ_S16(domain_address), -5);
 }

ethercat_interface/include/ethercat_interface/ec_master.hpp

@@ -144,7 +144,9 @@ class EcMaster
   bool activate();
 
   /** perform one EtherCAT cycle, passing the domain to the slaves */
-  virtual void update(uint32_t domain = 0);
+  virtual void update(void);
+  virtual void readData(void);
+  virtual void writeData(void);
 
   /** run a control loop of update() and user_callback(), blocking.
    *  call activate and setThreadHighPriority/RealTime first. */
@@ -182,9 +184,6 @@ class EcMaster
 
   uint32_t getInterval() {return interval_;}
 
-  virtual void readData(uint32_t domain = 0);
-  virtual void writeData(uint32_t domain = 0);
-
   /** @brief Fill in the EcTransferInfo structures
   *
   * @param transfer_nets transfer nets
@@ -246,7 +245,7 @@ class EcMaster
     EcSlave * slave);
 
   /** check for change in the domain state */
-  void checkDomainState(uint32_t domain);
+  void checkDomainState(DomainInfo * domain_info);
 
   /** check for change in the master state */
   void checkMasterState();

ethercat_interface/include/ethercat_interface/ec_slave.hpp

@@ -44,7 +44,7 @@ class EcSlave
 
 public:
   /** read or write data to the domain from the index of the entry in the recorded pdos */
-  virtual void processData(size_t /*entry_idx*/, uint8_t * /*domain_address*/) {}
+  virtual void processData(unsigned int /*domain_index*/, size_t /*entry_idx*/, uint8_t * /*domain_address*/) {}
   /** a pointer to syncs. return &syncs[0] */
   virtual const ec_sync_info_t * syncs() {return NULL;}
   virtual bool initialized() {return true;}