diff --git a/source/armarx/control/ethercat/Bus.cpp b/source/armarx/control/ethercat/Bus.cpp index 246445fbde45b35b07f8f80e32fbf78ee12033bc..d9db4acbe3e76e22d74a5501d0846aa963aef4f4 100644 --- a/source/armarx/control/ethercat/Bus.cpp +++ b/source/armarx/control/ethercat/Bus.cpp @@ -7,6 +7,8 @@ #include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h> #include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h> +#include <ArmarXCore/core/time/Clock.h> +#include <ArmarXCore/core/time/ClockType.h> #include <ArmarXCore/core/util/OnScopeExit.h> #include <RobotAPI/components/units/RobotUnit/util/RtTiming.h> @@ -18,12 +20,10 @@ #include "ErrorReporting.h" #include "bus_io/SlaveRegisterReadingScheduler.h" - namespace armarx::control::ethercat { static constexpr std::uint8_t DEFAULT_ECAT_GROUP = 0; - Bus& Bus::getBus() { @@ -45,13 +45,11 @@ namespace armarx::control::ethercat } } - Bus::Bus() : busState(EtherCATState::init), currentGroup(DEFAULT_ECAT_GROUP), errorHandler(this) { setTag("ethercat::Bus"); } - bool Bus::setPDOMappings() { @@ -124,7 +122,6 @@ namespace armarx::control::ethercat return retVal; } - bool Bus::createDevices() { @@ -434,21 +431,18 @@ namespace armarx::control::ethercat return true; } - void Bus::setSocketFileDescriptor(int socketFileDescriptor) { this->socketFileDescriptor = socketFileDescriptor; } - void Bus::setIfName(const std::string& ifname) { this->ifname = ifname; } - bool Bus::rtUpdateBus(bool doErrorHandling, size_t iteration) { @@ -483,7 +477,10 @@ namespace armarx::control::ethercat emergencyStopWasActivated = emergencyStopActive; } - auto delay = (IceUtil::Time::now(IceUtil::Time::Monotonic) - busUpdateLastUpdateTime); + const auto delay = + armarx::core::time::Clock(armarx::core::time::ClockType::Monotonic).now() - + busUpdateLastUpdateTime; + if (delay.toMilliSecondsDouble() > 40) { ARMARX_RT_LOGF_WARN("Update bus was not called for a long time: %d ms", @@ -493,7 +490,9 @@ namespace armarx::control::ethercat //send and receive process data rtUpdatePDO(); - busUpdateLastUpdateTime = IceUtil::Time::now(IceUtil::Time::Monotonic); + busUpdateLastUpdateTime = + armarx::core::time::Clock(armarx::core::time::ClockType::Monotonic).now(); + ; // Execute every slave. @@ -544,7 +543,6 @@ namespace armarx::control::ethercat return true; } - void Bus::shutdown() { @@ -595,14 +593,12 @@ namespace armarx::control::ethercat functionalState = BusFunctionalState::Shutdown; } - PDOValidity Bus::getPDOValidity() const { return pdoValidity; } - bool Bus::switchBusToInit() { @@ -1156,7 +1152,6 @@ namespace armarx::control::ethercat return true; } - void Bus::readAllErrorCounters() { @@ -1218,6 +1213,7 @@ namespace armarx::control::ethercat std::uint8_t forwardedRxError = 0; // Error counter of a predecessor. std::uint8_t linkLost = 0; }; + std::vector<ErrorCounters> counts(4); for (const auto& d : registerData) @@ -1269,7 +1265,6 @@ namespace armarx::control::ethercat } } - EtherCATState Bus::rtGetEtherCATState() const { @@ -1282,21 +1277,18 @@ namespace armarx::control::ethercat return functionalState; } - void Bus::registerSlaveFactory(const SlaveFactory& factory) { slaveFactories.push_back(factory); } - void Bus::registerDeviceFactory(const std::string& name, const DeviceFactory& factory) { deviceFactories[name] = factory; } - void Bus::setAndParseHardwareConfig(const MultiNodeRapidXMLReader& hwconfig) { @@ -1305,7 +1297,6 @@ namespace armarx::control::ethercat this->hwconfig = parser.getHardwareConfig(); } - std::vector<SlaveInterface*> Bus::getSlaves() const { @@ -1323,12 +1314,10 @@ namespace armarx::control::ethercat return devices; } - Bus::~Bus() { } - SlaveInterface* Bus::getSlaveAtIndex(std::uint16_t slaveIndex) const { diff --git a/source/armarx/control/ethercat/Bus.h b/source/armarx/control/ethercat/Bus.h index da75d67a141e1d2d411d6fd6a7e69d0648d032dd..7050324ac83ef9f6f53cf1dbe0c98ad50fe189a3 100644 --- a/source/armarx/control/ethercat/Bus.h +++ b/source/armarx/control/ethercat/Bus.h @@ -2,6 +2,7 @@ #include <ArmarXCore/core/logging/Logging.h> #include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h> +#include <ArmarXCore/core/time/DateTime.h> #include <armarx/control/ethercat/SlaveIdentifier.h> #include <armarx/control/hardware_config/ConfigParser.h> @@ -44,7 +45,6 @@ namespace armarx::control::ethercat Shutdown // bus is shutdown or it is in the process of initializing }; - /** * @class Bus * @ingroup Library-ethercat @@ -224,7 +224,7 @@ namespace armarx::control::ethercat bool emergencyStopWasActivated = false; - IceUtil::Time busUpdateLastUpdateTime; + armarx::core::time::DateTime busUpdateLastUpdateTime; hardware_config::HardwareConfig hwconfig; diff --git a/source/armarx/control/ethercat/bus_io/BusIO.cpp b/source/armarx/control/ethercat/bus_io/BusIO.cpp index 883be55d34eb6c744da673bcf8eb2a7c14ff2d25..c47733d86db7d4b90593597c6f40d01145089335 100644 --- a/source/armarx/control/ethercat/bus_io/BusIO.cpp +++ b/source/armarx/control/ethercat/bus_io/BusIO.cpp @@ -178,7 +178,7 @@ namespace armarx::control::ethercat void BusIO::handleChangeStateRequest(const std::shared_ptr<ChangeStateRequest>& request) { - static constexpr int statecheckTimeoutUS = 10000; + static constexpr int statecheckTimeoutUS = 10'000; ec_slave[request->slaveIndex].state = request->state; int ret = ec_writestate(request->slaveIndex);