diff --git a/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp b/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp index 16c8ede..87ed29f 100644 --- a/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp +++ b/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp @@ -129,8 +129,8 @@ class SOEM_RSL_EXPORT EthercatBusBase : private EthercatBusBaseTemplateAdapter { * Startup the bus communication. * @param abortFlag during startup it is waited till all the slaves are ready this can take some time, the abortFlag can be set to abort * this operation. - * @param sizeCheck perform a check of the Rx and Tx Pdo sizes defined in the PdoInfo object of the slaves - * @param maxDiscoverRetries number of retries till the configured number of slaves are found on the bus. + * @param sizeCheck perform a check of the Rx and Tx Pdo sizes defined in the PdoInfo object of the slaves + * @param maxDiscoverRetries number of retries till the configured number of slaves are found on the bus. * @return True if successful. */ bool startup(bool sizeCheck, int maxDiscoverRetries = 10); diff --git a/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp b/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp index 63d4b58..fc04867 100644 --- a/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp +++ b/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp @@ -115,6 +115,12 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { << "No socket connection. Execute as root."); return false; } + // Addresses are bus positions, and devices without a configured slave (e.g. EtherCAT switch + // ports) take positions too, so the bus is complete once it reaches the highest address. + int requiredDevices = 0; + for (const auto& slave : slaves_) { + requiredDevices = std::max(requiredDevices, static_cast(slave->getAddress())); + } for (int retry = 0; retry <= maxDiscoverRetries; retry++) { if (abortFlag) { MELO_WARN_STREAM("[soem_interface_rsl::" << name_ << "] " @@ -123,48 +129,46 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { return false; // avoid that execution continues. } - // SWITCH TOPOLOGY FIX: Use >= comparison to support switch topologies - // where infrastructure devices (switches) are included in the total device count - int detected_slaves = ecx_detect_slaves(&ecatContext_); - int expected_slaves = static_cast(slaves_.size()); - MELO_ERROR_STREAM("[DEBUG] [" << name_ << "] Detected slaves: " << detected_slaves << ", Expected slaves: " << expected_slaves); - if (detected_slaves >= expected_slaves) { + int detectedDevices = ecx_detect_slaves(&ecatContext_); + if (detectedDevices >= requiredDevices) { // on some of the older (rsl) anydrives there seems to be a short race between bus is responsive and slave is fully ready... // so give them this 1 sec to be fully ready to be started... soem_interface_rsl::threadSleep(1.0); break; } if (retry == maxDiscoverRetries) { - MELO_ERROR_STREAM("[soem_interface_rsl::" << name_ << "] EtherCAT discovery failed: " << detected_slaves << " slave(s) detected (expected >= 1) "); + MELO_ERROR_STREAM("[soem_interface_rsl::" << name_ << "] EtherCAT discovery failed: found " << detectedDevices + << " device(s), but the highest configured address is " << requiredDevices << "."); ecx_close(&ecatContext_); return false; } // Sleep and retry. soem_interface_rsl::threadSleep(ecatConfigRetrySleep_); - MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Detected " << detected_slaves - << " slaves, expected >= " << expected_slaves << ". Retrying (" - << (retry + 1) << "/" << maxDiscoverRetries << ")..."); + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Found " << detectedDevices << " of " << requiredDevices + << " EtherCAT devices. Retrying (" << (retry + 1) << "/" << maxDiscoverRetries << ")..."); } // this should now work cleanly, since we're sure that all slaves are started. - int detected_config = ecx_config_init(&ecatContext_, FALSE); - int expected_slaves = static_cast(slaves_.size()); - MELO_ERROR_STREAM("[DEBUG] [" << name_ << "] Config init result: " << detected_config << ", Expected slaves: " << expected_slaves); - if (detected_config < expected_slaves) { + int configuredDevices = ecx_config_init(&ecatContext_, FALSE); + if (configuredDevices < requiredDevices) { ecx_close(&ecatContext_); - MELO_ERROR_STREAM("[soem_interface_rsl::" << name_ << "] " - << "EtherCAT configuration failed: detected " << detected_config << " device(s), expected >= " << expected_slaves << " device(s)."); + MELO_ERROR_STREAM("[soem_interface_rsl::" << name_ << "] EtherCAT configuration failed: found " << configuredDevices + << " device(s), but the highest configured address is " << requiredDevices << "."); EthercatBusBase::printAvailableBusses(); return false; } int nSlaves = *ecatContext_.slavecount; - // Print the slaves which have been detected. - MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] The following " << nSlaves << " slaves have been found and configured:"); + // Print the devices which have been detected, marking those no slave is configured for. + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Found " << nSlaves << " EtherCAT devices for " << slaves_.size() + << " configured slaves:"); for (int slave = 1; slave <= nSlaves; slave++) { + const bool configured = std::any_of(slaves_.begin(), slaves_.end(), + [slave](const auto& s) { return static_cast(s->getAddress()) == slave; }); MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Address: " << slave << " - Name: '" - << std::string(ecatContext_.slavelist[slave].name) << "'"); + << std::string(ecatContext_.slavelist[slave].name) << "'" + << (configured ? "" : " (no slave configured, e.g. a switch port)")); } // Check if the given slave addresses are valid. @@ -526,10 +530,10 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { << std::setw(8) << std::hex << ecatContext_.slavelist[slave].ALstatuscode << " " << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); } else { - MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaves_[slave - 1]->getName() << " alStatusCode: 0x" + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaveNameAt(slave) << " alStatusCode: 0x" << std::setfill('0') << std::setw(8) << std::hex - << ecatContext_.slavelist[slave-1].ALstatuscode << " " - << ec_ALstatuscode2string(ecatContext_.slavelist[slave-1].ALstatuscode)); + << ecatContext_.slavelist[slave].ALstatuscode << " " + << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); } return false; } @@ -555,10 +559,10 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { << std::setw(8) << std::hex << ecatContext_.slavelist[slave].ALstatuscode << " " << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); } else { - MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaves_[slave - 1]->getName() << " alStatusCode: 0x" + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaveNameAt(slave) << " alStatusCode: 0x" << std::setfill('0') << std::setw(8) << std::hex - << ecatContext_.slavelist[slave-1].ALstatuscode << " " - << ec_ALstatuscode2string(ecatContext_.slavelist[slave-1].ALstatuscode)); + << ecatContext_.slavelist[slave].ALstatuscode << " " + << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); } return false; } @@ -589,7 +593,7 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { << std::setw(8) << std::hex << ecatContext_.slavelist[slave].ALstatuscode << " " << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); } else { - MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaves_[slave - 1]->getName() << " alStatusCode: 0x" + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaveNameAt(slave) << " alStatusCode: 0x" << std::setfill('0') << std::setw(8) << std::hex << ecatContext_.slavelist[slave].ALstatuscode << " " << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); @@ -693,7 +697,7 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { << std::setw(8) << std::hex << ecatContext_.slavelist[slave].ALstatuscode << " " << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); } else { - MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaves_[slave - 1]->getName() << " alStatusCode: 0x" + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Slave: " << slaveNameAt(slave) << " alStatusCode: 0x" << std::setfill('0') << std::setw(8) << std::hex << ecatContext_.slavelist[slave].ALstatuscode << " " << ec_ALstatuscode2string(ecatContext_.slavelist[slave].ALstatuscode)); @@ -701,6 +705,17 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { return false; } + // Name of the slave configured at a bus address. Addresses are not indices into slaves_, since + // devices without a configured slave (e.g. switch ports) take bus positions too. + std::string slaveNameAt(const uint16_t address) const { + for (const auto& slave : slaves_) { + if (slave->getAddress() == address) { + return slave->getName(); + } + } + return ""; + } + std::string getErrorString(ec_errort error) { std::stringstream stream; stream << "Time: " << (static_cast(error.Time.sec) + (static_cast(error.Time.usec) / 1000000.0));