SlaveRegisterReadingScheduler.cpp

Go to the documentation of this file.

#include "SlaveRegisterReadingScheduler.h"
 
#include <algorithm>
#include <cmath>
#include <cstring> // for memcpy
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
 
#include <armarx/control/ethercat/Bus.h>
#include <armarx/control/ethercat/bus_io/EtherCATFrame.h>
 
#include "ethercat.h"
 
namespace armarx::control::ethercat
{
 
    EtherCATFrameList*
    SlaveRegisterReadingScheduler::createEtherCATFrameListFromRegisterDataList(
        const std::vector<RegisterDataList>* requestedRegisters)
    {
        ARMARX_CHECK_NOT_NULL(requestedRegisters);
 
        std::vector<std::pair<std::uint16_t, std::vector<std::pair<int, int>>>> intervals;
 
        for (const auto& registerList : *requestedRegisters)
        {
            ARMARX_CHECK_LESS_EQUAL(registerList.slaveIndex, ec_slavecount);
            auto frameIntervals = createFrameIntervals(registerList);
            intervals.push_back({registerList.slaveIndex, frameIntervals});
        }
 
        return createEtherCATFrameList(intervals);
    }
 
    void
    SlaveRegisterReadingScheduler::updateRegisterDataFromEtherCATFrameList(
        const EtherCATFrameList* frameList,
        std::vector<RegisterDataList>* requestedRegisters)
    {
        ARMARX_CHECK_NOT_NULL(requestedRegisters);
        ARMARX_CHECK_NOT_NULL(frameList);
 
        auto getRegisterDataForSlave =
            [&requestedRegisters](std::uint16_t slaveConfiguredIndex) -> RegisterDataList&
        {
            std::uint16_t slaveIndex = 0;
            for (int i = 1; i <= ec_slavecount; i++)
            {
                if (ec_slave[i].configadr == slaveConfiguredIndex)
                {
                    slaveIndex = static_cast<std::uint16_t>(i);
                    break;
                }
            }
 
            auto it = std::find_if(requestedRegisters->begin(),
                                   requestedRegisters->end(),
                                   [&slaveIndex](const RegisterDataList& list)
                                   { return list.slaveIndex == slaveIndex; });
            return *it;
        };
 
        using RegisterDataIterator =
            std::map<datatypes::RegisterEnum, datatypes::RegisterEnumTypeContainer>::iterator;
 
        auto copyDataFromPduEntryToRegisterDataContainer =
            [&](const std::uint8_t* dataPtr, RegisterDataIterator registerDataEntry)
        {
            std::uint64_t result = 0;
 
            std::size_t bitlength = datatypes::registerMap.at(registerDataEntry->first).bitLength;
            std::uint8_t subAddress = datatypes::getRegisterSubAddress(registerDataEntry->first);
            switch (getRegisterByteLength(registerDataEntry->first))
            {
                case 1:
                    result = (*dataPtr) >> subAddress & ((0x1 << bitlength) - 1);
                    registerDataEntry->second =
                        static_cast<datatypes::EtherCATDataType::UNSIGNED8>(result);
                    break;
                case 2:
                    std::memcpy(&result, dataPtr, sizeof(datatypes::EtherCATDataType::UNSIGNED16));
                    registerDataEntry->second =
                        static_cast<datatypes::EtherCATDataType::UNSIGNED16>(result);
                    break;
                case 4:
                    std::memcpy(&result, dataPtr, sizeof(datatypes::EtherCATDataType::UNSIGNED32));
                    registerDataEntry->second =
                        static_cast<datatypes::EtherCATDataType::UNSIGNED32>(result);
                    break;
                case 8:
                    std::memcpy(&result, dataPtr, sizeof(datatypes::EtherCATDataType::UNSIGNED64));
                    registerDataEntry->second =
                        static_cast<datatypes::EtherCATDataType::UNSIGNED64>(result);
                    break;
            }
        };
 
 
        ARMARX_TRACE;
 
        for (const auto& [frame, metaData] : frameList->list)
        {
            const uint8_t* frameData = reinterpret_cast<const uint8_t*>(&frame); // NOLINT
            for (const auto& pdu : metaData.pdus)
            {
                RegisterDataList& data = getRegisterDataForSlave(pdu.slaveConfiguredAddress);
 
                std::uint16_t wkc = 0;
                std::memcpy(&wkc, frameData + pdu.workingCounterOffset, sizeof(uint16_t)); // NOLINT
                if (wkc == 0)
                {
                    continue;
                }
 
                RegisterDataIterator registerDataIt = data.registerData.begin();
 
                bool skipped = false;
                for (const auto& [type, offset] : pdu.registerOffsets)
                {
                    // Skip entries in registerData, that are not part of this pdu
                    if (!skipped)
                    {
                        while (registerDataIt->first < type)
                            registerDataIt++;
                        // If the current register data entry points NOT to a subaddress of the
                        // current pdu entry, we have to go one step back
                        if (datatypes::getRegisterAddress(registerDataIt->first) >
                            datatypes::getRegisterAddress(type))
                            registerDataIt--;
                        skipped = true;
                    }
 
                    if (registerDataIt->first == type)
                    {
                        // We found the registerData entry to fill with the data of this pdu entry
                        copyDataFromPduEntryToRegisterDataContainer(frameData + offset,
                                                                    registerDataIt);
                        registerDataIt++;
                    }
 
                    while (datatypes::getRegisterAddress(registerDataIt->first) ==
                           datatypes::getRegisterAddress(type))
                    {
                        // The registerData entry is a subaddress of the current pdu entry
                        copyDataFromPduEntryToRegisterDataContainer(frameData + offset,
                                                                    registerDataIt);
                        registerDataIt++;
                    }
                }
            }
        }
    }
 
    SlaveRegisterReadingScheduler::SlaveRegisterReadingScheduler(
        uint16_t slaveCount,
        unsigned int updatePeriodInMS,
        std::vector<datatypes::RegisterEnum> registerList) :
        updatePeriodInMS(updatePeriodInMS)
    {
        for (std::uint16_t i = 1; i < slaveCount + 1; i++)
        {
            RegisterDataList list;
            list.slaveIndex = i;
            for (const auto& type : registerList)
            {
                list.registerData.insert({type, datatypes::RegisterEnumTypeContainer()});
            }
            registerData.push_back(list);
        }
 
        frameList = createEtherCATFrameListFromRegisterDataList(&registerData);
 
        errorRegisterUpdateThread = std::thread(&SlaveRegisterReadingScheduler::updateLoop, this);
    }
 
    SlaveRegisterReadingScheduler::~SlaveRegisterReadingScheduler()
    {
        errorRegisterUpdateThreadRunning.store(false);
        errorRegisterUpdateThread.join();
    }
 
    const std::vector<RegisterDataList>&
    SlaveRegisterReadingScheduler::getRegisterData()
    {
        return registerData;
    }
 
    void
    SlaveRegisterReadingScheduler::startReadingNextRegisters()
    {
        if (readyToReadNextRegisters.load(std::memory_order_relaxed))
        {
            cv_readNextRegisters.notify_all();
        }
    }
 
    bool
    SlaveRegisterReadingScheduler::allRegistersUpdated() const
    {
        return areAllRegistersUpdated.load(std::memory_order_relaxed);
    }
 
    void
    SlaveRegisterReadingScheduler::updateLoop()
    {
        CycleUtil c(updatePeriodInMS);
        while (errorRegisterUpdateThreadRunning.load() == true)
        {
            std::unique_lock<std::mutex> lock{mutex_readNextRegisters};
            cv_readNextRegisters.wait(lock);
            readyToReadNextRegisters.store(false, std::memory_order_relaxed);
 
            Bus::getBus().readRegisters(frameList, 1);
            frameList->nextIndex = (frameList->nextIndex + 1) % frameList->list.size();
 
            if (frameList->nextIndex == 0)
            {
                SlaveRegisterReadingScheduler::updateRegisterDataFromEtherCATFrameList(
                    frameList, &registerData);
                areAllRegistersUpdated.store(true, std::memory_order_relaxed);
            }
            else
            {
                areAllRegistersUpdated.store(false, std::memory_order_relaxed);
            }
 
 
            if (allRegistersUpdated())
            {
                c.waitForCycleDuration();
            }
 
            readyToReadNextRegisters.store(true, std::memory_order_relaxed);
        }
    }
 
    std::vector<int>
    SlaveRegisterReadingScheduler::registerReadMapToAddressList(const RegisterDataList& toRead)
    {
        std::vector<int> addressesToRead;
        for (const auto& [registerType, _] : toRead.registerData)
        {
            // The higher bytes are used to index into a byte in our register numbering scheme
            // - We don't need that here.
            static constexpr int addressMask = 0xFFFF;
            static constexpr float byteLength = 8.0;
 
            int baseAddress = static_cast<int>(registerType) & addressMask;
 
            for (int i = 0; // NOLINTNEXTLINE(bugprone-narrowing-conversions)
                 i < std::ceil(datatypes::registerMap.at(registerType).bitLength / byteLength);
                 ++i)
            {
                addressesToRead.push_back(baseAddress + i);
            }
        }
        std::sort(addressesToRead.begin(), addressesToRead.end());
 
        addressesToRead.erase(std::unique(addressesToRead.begin(), addressesToRead.end()),
                              addressesToRead.end());
 
        return addressesToRead;
    }
 
    std::vector<std::pair<int, int>>
    SlaveRegisterReadingScheduler::createFrameIntervals(const RegisterDataList& toRead)
    {
        if (toRead.registerData.empty())
        {
            return {};
        }
 
        std::vector<int> addressesToRead(registerReadMapToAddressList(toRead));
 
        std::vector<std::pair<int, int>> pduIntervals;
        int currentIntervalStart = -1;
        int currentIntervalEnd = 0;
 
        for (int address : addressesToRead)
        {
            // If we've started accumulating an interval && adding the next address
            // adds fewer bytes than just starting a new PDU && our PDU hasn't grown too large
            if (currentIntervalStart >= 0 && address - currentIntervalEnd - 1 < pduOverhead &&
                address + 1 - currentIntervalStart + pduOverhead <
                    static_cast<int>(maxTotalPDULength))
            {
                currentIntervalEnd = address + 1;
            }
            else
            {
                if (currentIntervalStart >= 0)
                {
                    pduIntervals.push_back({currentIntervalStart, currentIntervalEnd});
                }
                currentIntervalStart = address;
                currentIntervalEnd = address + 1;
            }
        }
        pduIntervals.push_back({currentIntervalStart, currentIntervalEnd});
        return pduIntervals;
    }
 
    PDUMetaData
    SlaveRegisterReadingScheduler::createPDUMetaData(std::tuple<uint16_t, int, int> pduInterval,
                                                     size_t pduOffset)
    {
        PDUMetaData result;
        result.slaveConfiguredAddress = std::get<0>(pduInterval);
        size_t dataLength = std::get<2>(pduInterval) - std::get<1>(pduInterval);
        result.workingCounterOffset = pduOffset + sizeof(EtherCATPDU) - 1 + dataLength;
 
        // This is kinda ugly and also somewhat inefficient. I just haven't found a nice way
        // to get the required information from the top-level call all the way down here.
        for (int address = std::get<1>(pduInterval); address < std::get<2>(pduInterval); ++address)
        {
            datatypes::RegisterEnum addressAsEnum = static_cast<datatypes::RegisterEnum>(address);
            if (datatypes::registerMap.find(addressAsEnum) != datatypes::registerMap.end())
            {
                result.registerOffsets[addressAsEnum] =
                    address - std::get<1>(pduInterval) + sizeof(EtherCATPDU) - 1 + pduOffset;
            }
        }
        return result;
    }
 
    std::pair<EtherCATFrame, EtherCATFrameMetaData>
    SlaveRegisterReadingScheduler::createEtherCATFrame(
        std::vector<std::tuple<uint16_t, int, int>> slaveAssignedPDUIntervals)
    {
        EtherCATFrame frame{};
        EtherCATFrameMetaData metaData{};
 
        // Remember where we will write the next PDU
        uint8_t* currentLocation = static_cast<uint8_t*>(frame.pduArea);
        for (auto it = slaveAssignedPDUIntervals.begin(); it != slaveAssignedPDUIntervals.end();
             ++it)
        {
            uint16_t dataLength = std::get<2>(*it) - std::get<1>(*it);
            if (currentLocation + dataLength + pduOverhead // NOLINT
                >= static_cast<uint8_t*>(frame.pduArea + maxTotalPDULength)) // NOLINT
            {
                throw std::length_error(
                    "Total PDU length exceeded maximum EtherCAT frame capacity.");
            }
 
            EtherCATPDU* pdu = new (currentLocation) EtherCATPDU; // NOLINT
 
            pdu->slaveConfiguredAddress = std::get<0>(*it);
            pdu->registerAddress = std::get<1>(*it);
 
            if (it + 1 != slaveAssignedPDUIntervals.end())
            {
                static constexpr uint16_t hasNextPDU = 1 << 15;
                pdu->dataLengthAndNext = dataLength | hasNextPDU;
            }
            else
            {
                pdu->dataLengthAndNext = dataLength;
            }
 
            metaData.pdus.push_back(createPDUMetaData(
                *it, currentLocation - reinterpret_cast<uint8_t*>(&frame))); // NOLINT
 
            currentLocation += sizeof(EtherCATPDU) - 1 + dataLength + sizeof(uint16_t); // NOLINT
        }
        uint16_t frameLength =
            currentLocation - static_cast<uint8_t*>(frame.pduArea) + sizeof(uint16_t);
        metaData.lengthOfFrame = frameLength;
 
        static constexpr uint16_t frameTypePDUs = 0x1000;
        frame.lengthAndType = (frameLength - 2) | frameTypePDUs;
 
        return {frame, metaData};
    }
 
    EtherCATFrameList*
    SlaveRegisterReadingScheduler::createEtherCATFrameList(
        const std::vector<std::pair<std::uint16_t, std::vector<std::pair<int, int>>>>& pduIntervals)
    {
        // We need the slave addresses later when generating the frames,
        // so we add them in here.
        std::vector<std::tuple<std::uint16_t, int, int>> slaveAssignedPDUIntervals;
        for (const auto& [slaveIndex, intervals] : pduIntervals)
        {
            std::uint16_t slaveConfiguredAddress = ec_slave[slaveIndex].configadr;
            for (const std::pair<int, int>& interval : intervals)
            {
                slaveAssignedPDUIntervals.push_back(
                    {slaveConfiguredAddress, interval.first, interval.second});
            }
        }
 
        // TODO: This most likely leaks memory
        EtherCATFrameList* frameList = new EtherCATFrameList(); // NOLINT
        int frameTotalSize = 0;
        std::vector<std::tuple<uint16_t, int, int>> nextFrameIntervals;
 
        // We add in PDUs until we can't fit the next, then start a new frame.
        for (const auto& interval : slaveAssignedPDUIntervals)
        {
            int nextIntervalLength = std::get<2>(interval) - std::get<1>(interval);
            if (frameTotalSize + nextIntervalLength + pduOverhead <
                static_cast<int>(maxTotalPDULength))
            {
                nextFrameIntervals.push_back(interval);
                frameTotalSize += nextIntervalLength + pduOverhead;
            }
            else
            {
                frameList->list.push_back(createEtherCATFrame(nextFrameIntervals));
                nextFrameIntervals.clear();
                frameTotalSize = 0;
            }
        }
        frameList->list.push_back(createEtherCATFrame(nextFrameIntervals));
        return frameList;
    }
 
} // namespace armarx::control::ethercat