BusIO.cpp

Go to the documentation of this file.

#include "BusIO.h"
 
#include <ethercat.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
 
#include <armarx/control/ethercat/ErrorReporting.h>
#include <armarx/control/ethercat/RTUtility.h>
#include <armarx/control/ethercat/Timing.h>
 
#include "EtherCATFrame.h"
#include "requests/ChangeStateRequest.h"
#include "requests/ReadStatesRequest.h"
#include "requests/RegisterReadRequest.h"
#include "requests/RegisterResetRequest.h"
#include "requests/SDOUpdateRequest.h"
 
namespace armarx::control::ethercat
{
    static constexpr int TIMEOUT_UPDATE_PDO_US = 600;
 
    BusIO::BusIO()
    {
        //writing zeros to the IOMap
        ioMap.fill(0);
 
        requestHandlerThread = std::thread(&BusIO::requestHandlerLoop, this);
    }
 
    bool
    BusIO::generalSDOWrite(std::uint16_t slaveIndex,
                           std::uint16_t index,
                           std::uint8_t subindex,
                           std::uint16_t buflen,
                           const void* buf,
                           bool completeAccess)
    {
        return requestQueues.postSDOWriteRequest(
            {slaveIndex, index, subindex},
            buflen,
            static_cast<unsigned char*>(const_cast<void*>(buf)),
            completeAccess);
    }
 
    bool
    BusIO::generalSDORead(std::uint16_t slaveIndex,
                          std::uint16_t index,
                          std::uint8_t subindex,
                          int* buflen,
                          void* buf,
                          bool completeAccess)
    {
        return requestQueues.postSDOReadRequest({slaveIndex, index, subindex},
                                                buflen,
                                                static_cast<unsigned char*>(buf),
                                                completeAccess);
    }
 
    bool
    BusIO::isSDOAccessAvailable() const
    {
        if (!sdoAccessAvailable)
        {
            GENERAL_WARNING("SDO-access is not yet available. Probably the bus was not started yet");
            return false;
        }
        return true;
    }
 
    std::pair<int, int>
    BusIO::sendAndReceiveEtherCATFrame(const EtherCATFrame* frame,
                                       size_t frameLength,
                                       uint16_t slaveConfiguredAddress,
                                       const std::vector<size_t>& slaveAddressOffsets)
    {
        int bufferIndex = ecx_getindex(ecx_context.port);
        uint8_t* buffer =
            reinterpret_cast<uint8_t*>(&(ecx_context.port->txbuf[bufferIndex])); // NOLINT
 
        // The Ethernet header will already be placed in the buffer, so skip it
        EtherCATFrame* framePlacement =
            reinterpret_cast<EtherCATFrame*>(buffer + ETH_HEADERSIZE); // NOLINT
        std::memcpy(framePlacement, frame, frameLength);
 
        framePlacement->pduArea[1] = static_cast<std::uint8_t>(bufferIndex);
        for (size_t offset : slaveAddressOffsets)
        {
            *reinterpret_cast<uint16_t*>(framePlacement->pduArea + offset) // NOLINT
                = slaveConfiguredAddress;
        }
 
        ecx_context.port->txbuflength[bufferIndex] =
            static_cast<int>(ETH_HEADERSIZE + frameLength); // NOLINT
 
        // This is blocking, which seems good enough for now.
        int const workingCounter =
            ecx_srconfirm(ecx_context.port, bufferIndex, timeouts.registerRead_us);
 
        if (workingCounter == EC_NOFRAME)
        {
            GENERAL_ERROR("Sending and receiving EtherCAT frame timed out after %i µs (EC_NOFRAME).  "
                      "This timeout will most likely result in consecutive errors.",
                      timeouts.registerRead_us);
        }
 
        return {workingCounter, bufferIndex};
    }
 
    void
    BusIO::rtUpdatePDO()
    {
        GENERAL_TIMING_START(updatePDOtiming)
 
        pdoUpdateRequested.store(true, std::memory_order_relaxed);
 
        GENERAL_TIMING_START(send_processdata)
        ec_send_processdata();
        GENERAL_TIMING_CEND(send_processdata, TIMEOUT_UPDATE_PDO_US / 1000.0 + 0.05)
        GENERAL_TIMING_START(receive_processdata);
        lastWorkCounter = ec_receive_processdata(TIMEOUT_UPDATE_PDO_US);
        GENERAL_TIMING_CEND(receive_processdata, TIMEOUT_UPDATE_PDO_US / 1000.0 + 0.05)
        // Request Handler can continue to run
        pdoUpdateRequested.store(false, std::memory_order_relaxed);
 
        GENERAL_TIMING_CEND(updatePDOtiming, TIMEOUT_UPDATE_PDO_US / 1000.0 + 0.05)
    }
 
    void
    BusIO::requestHandlerLoop()
    {
        RTUtility::elevateThreadPriority(RTUtility::RT_THREAD_PRIORITY);
        RTUtility::pinThreadToCPU(1);
 
        while (requestHandlerThreadRunning.load() == true)
        {
            if (!pdoUpdateRequested.load(std::memory_order_relaxed))
            {
                handleNextRequest();
            }
            using namespace std::literals;
            std::this_thread::sleep_for(10us);
        }
    }
 
    bool
    BusIO::handleNextRequest()
    {
        auto request = requestQueues.getNextRequest();
        if (request)
        {
            TIMING_START(requestDuration)
            if (auto r = std::dynamic_pointer_cast<SDOUpdateRequest>(request))
            {
                handleSDOUpdateRequest(r);
                TIMING_CEND_COMMENT(requestDuration, "SDOUpdateRequest", 5)
            }
            else if (auto r = std::dynamic_pointer_cast<ChangeStateRequest>(request))
            {
                handleChangeStateRequest(r);
                TIMING_CEND_COMMENT(requestDuration, "ChangeStateRequest", 1)
            }
            else if (auto r = std::dynamic_pointer_cast<ReadStatesRequest>(request))
            {
                handleReadStatesRequest(r);
                TIMING_CEND_COMMENT(requestDuration, "ReadStatesRequest", 1)
            }
            else if (auto r = std::dynamic_pointer_cast<RegisterResetRequest>(request))
            {
                handleRegisterResetRequest(r);
                TIMING_CEND_COMMENT(requestDuration, "RegisterResetRequest", 1)
            }
            else if (auto r = std::dynamic_pointer_cast<RegisterReadRequest>(request))
            {
                handleRegisterReadRequest(r);
                TIMING_CEND_COMMENT(requestDuration, "RegisterReadRequest", 1)
            }
 
            requestQueues.postReply(std::move(request));
 
            return true;
        }
        return false;
    }
 
    void
    BusIO::handleChangeStateRequest(const std::shared_ptr<ChangeStateRequest>& request)
    {
        ec_slave[request->slaveIndex].state = request->state;
        int ret = ec_writestate(request->slaveIndex);
        if (ret == EC_NOFRAME)
        {
            GENERAL_ERROR("Writing EtherCAT state %s to slave at index %u (0 = all slaves) timed out "
                      "(EC_NOFRAME).  The timeout for this operation was %i µs, is hardcoded by "
                      "SOEM, and thus cannot be configured.",
                      request->slaveIndex,
                      request->state.c_str(),
                      /* Timeout for SOEM function ecx_writestate(...) = */ EC_TIMEOUTRET3);
 
            request->setFailed();
        }
        else
        {
            if (request->validate)
            {
                auto const start = armarx::rtNow();
                EtherCATState actualState =
                    ec_statecheck(request->slaveIndex, request->state, timeouts.stateCheck_us);
                auto const statecheckDuration = armarx::rtNow() - start;
 
                if (actualState != request->state)
                {
                    // We can only guess whether the timeout was reached or not, but it is very
                    // likely that it reached the timout if it took longer than it should (all
                    // overhead considered).
                    bool const probablyTimedOut =
                        statecheckDuration.toMicroSeconds() > timeouts.stateCheck_us;
 
                    GENERAL_WARNING(
                        "Could not validate EtherCAT state of slave at index %u (0 = all slaves).\n"
                        "Requested state is %s and the actual state is %s.\n"
                        "If the SOEM operation to check a slave's state timed out, the actual "
                        "state might just be the minimum state SOEM was able to derive within that "
                        "timeout.\n"
                        "The configured timeout is %i µs and the operation took %i µs%s.",
                        request->slaveIndex,
                        request->state.c_str(),
                        actualState.c_str(),
                        timeouts.stateCheck_us,
                        statecheckDuration.toMicroSeconds(),
                        probablyTimedOut ? ", so the operation probably timed out" : "");
                    request->setFailed();
                }
 
                request->setActualState(actualState);
            }
        }
    }
 
    void
    BusIO::handleReadStatesRequest(const std::shared_ptr<ReadStatesRequest>& request)
    {
        EtherCATState state = static_cast<std::uint16_t>(ec_readstate());
        request->setState(state);
    }
 
    void
    BusIO::handleRegisterResetRequest(const std::shared_ptr<RegisterResetRequest>& request)
    {
        static constexpr size_t lengthOfPreMadeFrame = 44;
        static const std::vector<size_t> slaveAddressOffsets{2, 28};
 
        static constexpr EtherCATFrame preMadeResetFrame{
            (lengthOfPreMadeFrame - 2) | 0x1000 /* Length of frame - 2 | PDU type */,
            {
                // clang-format off
                0x05 /* FPWR */, 0xff /* Index */, 0xff, 0xff /* Slave address */,
                0x00, 0x03 /* Error counters' address */, 0x0e, 0x00 /* Error counters' length */,
                0x00, 0x00 /* External event */, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                0x00, 0x00, 0x00, 0x00, 0x00, 0x00 /* Data area */, 0x00, 0x00 /* Working counter */,
 
                // There's a gap in the registers here that can't be written to,
                // so we have to split the frame into two PDUs.
 
                0x05 /* FPWR */, 0xff /* Index */, 0xff, 0xff /* Slave address */,
                0x10, 0x03 /* Error counters' address */, 0x04, 0x00 /* Error counters' length */,
                0x00, 0x00 /* External event */, 0x00, 0x00, 0x00, 0x00 /* Data area */,
                0x00, 0x00 /* Working counter */
                // clang-format on
            }};
 
        auto [workingCounter, bufferIndex] =
            sendAndReceiveEtherCATFrame(&preMadeResetFrame,
                                        lengthOfPreMadeFrame,
                                        ec_slave[request->slaveIndex].configadr,
                                        slaveAddressOffsets);
 
        if (workingCounter == EC_NOFRAME)
        {
            GENERAL_ERROR("Failed to reset error counters for slave at index %u.", request->slaveIndex);
            request->setFailed();
        }
 
        ecx_setbufstat(ecx_context.port, bufferIndex, EC_BUF_EMPTY);
    }
 
    void
    BusIO::handleRegisterReadRequest(const std::shared_ptr<RegisterReadRequest>& request)
    {
        auto [frameList, count] = request->getFrames();
 
        for (EtherCATFrameIterator it =
                 EtherCATFrameIterator(frameList, frameList->nextIndex, count);
             !it.atEnd();
             ++it)
        {
            auto [frame, metaData] = *it;
            auto [workingCounter, bufferIndex] =
                sendAndReceiveEtherCATFrame(frame, metaData->lengthOfFrame, 0, {});
            if (workingCounter != EC_NOFRAME)
            {
                // SOEM strips the Ethernet header for us in rxbuf, so we don't need
                // to account for it.
                std::memcpy(frame,
                            reinterpret_cast<EtherCATFrame*>(
                                &(ecx_context.port->rxbuf[bufferIndex])), // NOLINT
                            metaData->lengthOfFrame);
            }
            else
            {
                std::stringstream ss;
                ss << "EtherCATFrame containing requests for reading registers from following "
                      "slaves returned with EC_NOFRAME:\n";
                for (const auto& pdu : metaData->pdus)
                {
                    std::uint16_t slaveIndex = 0;
                    for (int i = 1; i <= ec_slavecount; i++)
                    {
                        if (ec_slave[i].configadr == pdu.slaveConfiguredAddress)
                        {
                            slaveIndex = static_cast<std::uint16_t>(i);
                            break;
                        }
                    }
 
                    ss << std::to_string(slaveIndex) << ", ";
                }
                ss.seekp(ss.str().length() - 2);
                ss << "\n This happened on frame " << it.getCurrentIndex() << " out of " << count << " requested Frames. \n";                ss.seekp(ss.str().length() - 2);
                GENERAL_ERROR("%s", ss.str().c_str());
 
                request->setFailed();
            }
            ecx_setbufstat(ecx_context.port, bufferIndex, EC_BUF_EMPTY);
        }
 
        request->updateRequestedRegisters();
    }
 
    void
    BusIO::handleSDOUpdateRequest(const std::shared_ptr<SDOUpdateRequest>& request)
    {
        if (!isSDOAccessAvailable())
        {
            request->setFailed();
            return;
        }
 
        IceUtil::Time start = armarx::rtNow();
        int wkc = -1;
        int len = static_cast<int>(*request->buflen);
        if (request->readRequest)
        {
            GENERAL_TIMING_START(sdo_read_request);
            wkc = ec_SDOread(request->sdoIdentifier.slaveIndex,
                             request->sdoIdentifier.index,
                             request->sdoIdentifier.subIndex,
                             request->completeAccess,
                             &len,
                             request->buf,
                             timeouts.sdoRead_us);
            *request->buflen = len;  // Write back actually read buffer length.
            GENERAL_TIMING_CEND(sdo_read_request, 1);
        }
        else
        {
            GENERAL_TIMING_START(sdo_write_request);
            wkc = ec_SDOwrite(request->sdoIdentifier.slaveIndex,
                              request->sdoIdentifier.index,
                              request->sdoIdentifier.subIndex,
                              request->completeAccess,
                              len,
                              request->buf,
                              timeouts.sdoWrite_us);
            GENERAL_TIMING_CEND(sdo_write_request, 1);
        }
        IceUtil::Time end = armarx::rtNow();
        if (wkc <= 0)
        {
            GENERAL_ERROR("%s for slave at index %u at 0x%X:%u failed:\n"
                      "Work counter: %u\n"
                      "SOEM-errorlist:\n%s",
                      request->readRequest ? "SDORead" : "SDOWrite",
                      request->sdoIdentifier.slaveIndex,
                      request->sdoIdentifier.index,
                      request->sdoIdentifier.subIndex,
                      wkc,
                      ec_elist2string());
 
            request->setFailed();
        }
    }
 
    BusIO::~BusIO()
    {
        requestHandlerThreadRunning.store(false);
        requestHandlerThread.join();
    }
 
    EtherCATState
    BusIO::changeStateOfSlave(uint16_t slaveIndex, EtherCATState state, bool validate)
    {
        EtherCATState actualState;
        requestQueues.postChangeStateRequest(slaveIndex, state, validate, &actualState);
        return actualState;
    }
 
    EtherCATState
    BusIO::changeStateOfBus(EtherCATState state, bool validate)
    {
        EtherCATState actualState;
        requestQueues.postChangeStateRequest(0, state, validate, &actualState);
        return actualState;
    }
 
    EtherCATState
    BusIO::readStates()
    {
        EtherCATState state;
        requestQueues.postReadStatesRequest(&state);
        return state;
    }
 
    bool
    BusIO::resetErrorRegisters(std::uint16_t slaveIndex)
    {
        return requestQueues.postRegisterResetRequest(slaveIndex);
    }
 
    bool
    BusIO::readRegisters(std::vector<RegisterDataList>& registerData)
    {
        return requestQueues.postRegisterReadRequest(registerData);
    }
 
    bool
    BusIO::readRegisters(EtherCATFrameList* frames, std::uint16_t amountFramesToRead)
    {
        return requestQueues.postRegisterReadRequest(frames, amountFramesToRead);
    }
 
    void
    BusIO::deactivateCOECA(std::uint16_t slaveIndex)
    {
        ARMARX_DEBUG << "Deactivation CoE Complete Access for slave at index " << slaveIndex;
        if (slaveIndex <= ec_slavecount)
        {
            std::uint8_t config = ec_slave[slaveIndex].CoEdetails;
            config &= ~ECT_COEDET_SDOCA;
            ec_slave[slaveIndex].CoEdetails = config;
        }
        else
        {
            GENERAL_WARNING("Trying to deactivate CoE Complete Access for slave at index %u which does "
                        "not exist on the bus.",
                        slaveIndex);
        }
    }
 
    void
    BusIO::setTimeouts(const Timeouts& timeouts)
    {
        this->timeouts = timeouts;
    }
 
} // namespace armarx::control::ethercat