BusErrorHandler.cpp

Go to the documentation of this file.

#include "BusErrorHandler.h"
 
#include <variant>
 
#include <ethercat.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
 
#include <armarx/control/ethercat/Timing.h>
 
#include "Bus.h"
#include "ErrorReporting.h"
#include "EtherCATState.h"
#include "SlaveInterface.h"
 
namespace armarx::control::ethercat
{
    static constexpr unsigned int workerCounterWatchdogTimeout = 20000;
 
    BusErrorHandler::BusErrorHandler(Bus* bus) :
        bus(bus), workerCounterWatchdog(workerCounterWatchdogTimeout)
    {
        slaveReinitializingThreadRunning.store(true);
        slaveReinitializingThread = std::thread(&BusErrorHandler::slaveReinitializingLoop, this);
    }
 
    BusErrorHandler::~BusErrorHandler()
    {
        slaveReinitializingThreadRunning.store(false);
        slaveReinitializingThread.join();
    }
 
    void
    BusErrorHandler::rtHandleBusErrors()
    {
        ARMARX_CHECK(expectedWorkCounter > 0);
        rtUpdateSlaveStates();
    }
 
    void
    BusErrorHandler::rtHandleSlaveErrors()
    {
        BUS_TIMING_START(bus_rtHandleSlaveErrors_getErrorReporter)
        auto reporter = Reporting::getErrorReporting().getErrorReporter();
        BUS_TIMING_CEND(bus_rtHandleSlaveErrors_getErrorReporter, bus->iterationCount, 0.3)
 
        for (const auto& slave : bus->getSlaves())
        {
            //try to clear error if there exist some, the rest of the slaves can run normal
            SLAVE_TIMING_START(bus_rtHandleSlaveErrors_slaveHasError)
            auto hasError = slave->hasError();
            SLAVE_TIMING_CEND(
                slave->getSlaveIdentifier(), bus_rtHandleSlaveErrors_slaveHasError, 0.3)
            if (hasError)
            {
 
                SLAVE_TIMING_START(bus_rtHandleSlaveErrors_slaveHandleError)
                if (!slave->handleErrors())
                {
                    SLAVE_ERROR_LOCAL(
                        reporter, slave->getSlaveIdentifier(), "Unhandled error in slave")
                        .deactivateSpam(1);
                }
                SLAVE_TIMING_CEND(
                    slave->getSlaveIdentifier(), bus_rtHandleSlaveErrors_slaveHandleError, 0.3)
            }
        }
 
        slaveErrorFound = reporter.hasErrors();
    }
 
    bool
    BusErrorHandler::hasError() const
    {
        return busErrorFound || slaveErrorFound;
    }
 
    bool
    BusErrorHandler::isReinitializationActive() const
    {
        return !allSlavesReinitialized.load();
    }
 
    bool
    BusErrorHandler::isSlaveLostOrDuringReinitialization(SlaveInterface* slave) const
    {
        return slaveStates.at(slave) != SlaveState::Operational;
    }
 
    void
    BusErrorHandler::init(int wkc)
    {
        expectedWorkCounter = wkc;
 
        for (const auto& s : bus->getSlaves())
        {
            slaveStates.insert({s.get(), SlaveState::Operational});
        }
    }
 
    void
    BusErrorHandler::rtUpdateSlaveStates()
    {
        if (bus->lastWorkCounter < expectedWorkCounter)
        {
            if (workerCounterWatchdog.expired())
            {
                if (bus->lastWorkCounter == EC_NOFRAME && !allSlavesReinitialized.load())
                {
                    // Panic!! Bus unsable?
                    BUS_WARNING(bus->iterationCount,
                                "The last work counter is EC_NOFRAME not all slaves have been "
                                "reinitialized yet!");
                }
                else if (bus->lastWorkCounter != secondLastUnexpectedWorkCounter)
                {
                    BUS_INFO(bus->iterationCount,
                             "Last Work Counter: %i, Second Last Unexpected Work Counter: %i, "
                             "Expected Work Counter: %i",
                             bus->lastWorkCounter,
                             secondLastUnexpectedWorkCounter,
                             expectedWorkCounter);
                    secondLastUnexpectedWorkCounter = bus->lastWorkCounter;
                    // Lost slaves or found slaves again or slaves are not in op
 
                    // Check for lost slaves and mark them
                    rtMarkLostSlavesInSOEMStruct();
 
                    std::stringstream lost_ss;
                    std::stringstream redisc_ss;
                    bool print_lost = false;
                    bool print_redisc = false;
                    lost_ss << "The following slaves have been lost: \n";
                    redisc_ss << "The following slaves have been rediscovered: \n";
                    for (const auto& slave : bus->getSlaves())
                    {
                        if (ec_slave[slave->getSlaveIdentifier().slaveIndex].islost)
                        {
                            if (slaveStates.at(slave.get()) != SlaveState::Lost)
                            {
                                lost_ss << slave->getSlaveIdentifier().getNameAsCStr() << "("
                                        << slave->getSlaveIdentifier().slaveIndex << ")\n";
                                busErrorFound = true;
                                print_lost = true;
                            }
 
                            slaveStates.at(slave.get()) = SlaveState::Lost;
                            allSlavesReinitialized.store(false);
                        }
                        else
                        {
                            if (slaveStates.at(slave.get()) == SlaveState::Lost)
                            {
                                slaveStates.at(slave.get()) = SlaveState::Reinitializating;
                                print_redisc = true;
                                redisc_ss << slave->getSlaveIdentifier().getNameAsCStr() << "("
                                          << slave->getSlaveIdentifier().slaveIndex << ")\n";
                            }
                        }
                    }
                    if (print_lost)
                    {
                        BUS_ERROR(bus->iterationCount, lost_ss.str().c_str());
                    }
                    if (print_redisc)
                    {
                        BUS_INFO(bus->iterationCount, redisc_ss.str().c_str());
                    }
 
                    // Read bus states and add all slaves that need reinitialization to list
                    if (bus->readStates() != EtherCATState::invalid)
                    {
                        for (const auto& slave : bus->getSlaves())
                        {
                            if (EtherCATState(
                                    ec_slave[slave->getSlaveIdentifier().slaveIndex].state) !=
                                    EtherCATState::op &&
                                ec_slave[slave->getSlaveIdentifier().slaveIndex].islost == 0 &&
                                (slaveStates.at(slave.get()) != SlaveState::Reinitializating))
                            {
                                slaveStates.at(slave.get()) = SlaveState::Reinitializating;
 
                                // For some slaves, specifically the hands, it might happen that
                                // they need to be reinitialized, but a technical link loss never
                                // occurred.  The EtherCAT adapter of the hands is on the wrist and
                                // is connected via an SPI connection to the hand micro controller.
                                // This SPI connection is prone to noise etc. and can be
                                // interrupted.  This results in the hand freezing from a user
                                // perspective, while the slave (the EtherCAT chip on the wrist
                                // board) is still responding and connected.  Thus, there will never
                                // be a link loss in the error registers, but it still must be
                                // reinitialized.  This happens after a watchdog in the hand
                                // firmware detects that the SPI connection was interrupted and
                                // restarts the firmware.  This will also reinitialize the slave.
                                // From the bus' perspecive it can be seen that at some point the
                                // slave will leave op state and transition to init back again.
                                // If this happens, then this branch will be executed.  The bus
                                // acknowledge that the slave needs reinitialization, but because of
                                // no link loss, no action for reinitialization would be taken.
                                // Setting the variable `allSlavesReinitialized` to false as in the
                                // next line will trigger this reinitialization similar to the case
                                // where a link loss was detected via the errorr registers (see
                                // above).
                                allSlavesReinitialized.store(false);
                                // Trigger SS2 since hands need to be re-tared.
                                busErrorFound = true;
 
                                BUS_ERROR(bus->iterationCount,
                                          "Slave at index %u of type %s needs reinitialization!",
                                          slave->getSlaveIdentifier().slaveIndex,
                                          slave->getSlaveIdentifier().getNameAsCStr());
                            }
                        }
                    }
                }
            }
        }
        else
        {
            busErrorFound = false;
            workerCounterWatchdog.reset();
            secondLastUnexpectedWorkCounter = bus->lastWorkCounter;
        }
 
        return;
    }
 
    void
    BusErrorHandler::rtMarkLostSlavesInSOEMStruct() const
    {
        std::uint16_t w = 0;
        const int slaveCount = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE);
        if (slaveCount == EC_NOFRAME)
        {
            return;
        }
 
        if (slaveCount < ec_slavecount)
        {
            BUS_WARNING(bus->iterationCount,
                        "Number of Slaves (%i) in SOEM struct is less than configured Slaves (%i).",
                        slaveCount,
                        ec_slavecount);
            // Create RegisterDataList with request to read register CONFIGURED_STATION_ADDRESS
            std::vector<RegisterDataList> registers;
            for (auto i = 1; i <= ec_slavecount; i++)
            {
                registers.push_back(
                    RegisterDataList{static_cast<std::uint16_t>(i),
                                     {{datatypes::RegisterEnum::CONFIGURED_STATION_ADDRESS,
                                       datatypes::RegisterEnumTypeContainer()}}});
            }
 
            // Read Registers
            bus->readRegisters(registers);
 
            // Iterate over read registers and check which slaves did not answer
            std::stringstream ss;
            ss << "Slaves with the following indexes are not reachable: \n";
            for (const auto& dataList : registers)
            {
                auto data =
                    dataList.registerData.at(datatypes::RegisterEnum::CONFIGURED_STATION_ADDRESS);
                if (std::holds_alternative<datatypes::EtherCATDataType::UNSIGNED16>(data) &&
                    ec_slave[dataList.slaveIndex].configadr ==
                        std::get<datatypes::EtherCATDataType::UNSIGNED16>(data))
                {
                    ec_slave[dataList.slaveIndex].islost = false;
                }
                else
                {
                    ss << dataList.slaveIndex << ", ";
                    ec_slave[dataList.slaveIndex].islost = true;
                }
            }
            ss.seekp(ss.str().length() - 2);
            ss << "\n The total number of registers read is " << registers.size() << "\n";
            BUS_ERROR(bus->iterationCount, "%s", ss.str().c_str());
        }
        else
        {
            // All slaves are found
            for (int i = 1; i <= ec_slavecount; i++)
            {
                ec_slave[i].islost = false;
            }
        }
    }
 
    void
    BusErrorHandler::reinitializeSlaves()
    {
        // Update ec_slave with the current slave states
        bus->readStates();
 
        for (const auto& [slave, state] : slaveStates)
        {
            if (state != SlaveState::Reinitializating)
            {
                continue;
            }
 
            std::uint16_t index =
                static_cast<std::uint16_t>(slave->getSlaveIdentifier().slaveIndex);
 
            bool slaveReadyAgain = false;
            //            TIMING_START(handleFound_switchEtherCATState)
            switch (EtherCATState(ec_slave[index].state))
            {
                case EtherCATState::invalid:
                case EtherCATState::init:
                    // This state gets reached when the corresponding slave did a full power cycle
                    // and needs to be initialized from scratch ...
                    // The first step is to recover the slave to validate that the slave is the same
                    // as we lost at that position in the bus.
                    if (ec_recover_slave(index, 10000))
                    {
                        // Since the slave is in fact the same as before,
                        // we can change its state to preOp
                        bus->changeStateOfSlave(index, EtherCATState::preOp, false);
                    }
                    else
                    {
                        BUS_ERROR(bus->iterationCount,
                                  "Could not recover slave: %s",
                                  slave->getSlaveIdentifier().getNameAsCStr());
                    }
 
                    //                    TIMING_END_COMMENT(handleFound_switchEtherCATState,
                    //                                       "ReinitializeSlaves - EtherCATState: init (" +
                    //                                           slave->getSlaveIdentifier().getName() + ")")
                    break;
 
                case EtherCATState::preOp:
                    // SOEM already provides a function for reconfiguring lost slaves.
                    // That function executes a hook for switching from preOp to safeOp which we have to
                    // set to a lambda which executes our default functions for that transition:
                    ec_slave[index].PO2SOconfig = [](std::uint16_t index) -> int
                    {
                        Bus& bus = Bus::getBus();
 
                        auto localSlave = bus.getSlaveAtIndex(index);
 
                        localSlave->doMappings();
                        localSlave->prepareForSafeOp();
                        localSlave->finishPreparingForSafeOp();
 
                        return 0;
                    };
                    // After executing this function, the corresponding slave will be in safeOp
                    ec_reconfig_slave(index, 10000);
 
                    // Better reset that hook, just to be sure
                    ec_slave[index].PO2SOconfig = nullptr;
                    //                    TIMING_END_COMMENT(handleFound_switchEtherCATState,
                    //                                       "ReinitializeSlaves - EtherCATState: preOp (" +
                    //                                           slave->getSlaveIdentifier().getName() + ")")
                    break;
 
                case EtherCATState::safeOp:
                    // Execute hooks for safeOp to op, similar to the normal bus initialization.
                    slave->prepareForOp();
                    slave->finishPreparingForOp();
 
                    bus->changeStateOfSlave(index, EtherCATState::op, false);
 
                    //                    TIMING_END_COMMENT(handleFound_switchEtherCATState,
                    //                                       "ReinitializeSlaves - EtherCATState: safeOp (" +
                    //                                           slave->getSlaveIdentifier().getName() + ")")
                    break;
 
                case EtherCATState::safeOpError:
                    // This state is reached if the slaves have not been updated for a long time
                    // (>50ms) and the sync manager watchdog gets triggered.
                    // The only way to recover from that is to reinit the slave by switching its
                    // state to init
                    ec_recover_slave(index, 10000);
 
                    bus->changeStateOfSlave(index, EtherCATState::init, false);
 
                    //                    TIMING_END_COMMENT(handleFound_switchEtherCATState,
                    //                                       "ReinitializeSlaves - EtherCATState: safeOpError (" +
                    //                                           slave->getSlaveIdentifier().getName() + ")")
                    break;
 
                case EtherCATState::op:
                    if (slave->prepareForRun())
                    {
                        slaveReadyAgain = true;
                    }
                    //                    TIMING_END_COMMENT(handleFound_switchEtherCATState,
                    //                                       "ReinitializeSlaves - EtherCATState: op (" +
                    //                                           slave->getSlaveIdentifier().getName() + ")")
                    break;
 
                default:
                    break;
            }
 
            // Slave was successfully recovered, we can remove it from the list
            if (slaveReadyAgain)
            {
                slaveStates.at(slave) = SlaveState::Operational;
 
                ARMARX_IMPORTANT
                    << "Slave at index " << slave->getSlaveIdentifier().slaveIndex << " of type "
                    << slave->getSlaveIdentifier().getName()
                    << " has been successfully reinitialized. Current bus iteration number is "
                    << bus->iterationCount;
 
                allSlavesReinitialized.store(areAllSlavesReinitialized());
            }
        }
    }
 
    void
    BusErrorHandler::slaveReinitializingLoop()
    {
        while (slaveReinitializingThreadRunning.load())
        {
            while (!allSlavesReinitialized.load())
            {
                reinitializeSlaves();
            }
 
            using namespace std::literals;
            std::this_thread::sleep_for(100us);
        }
    }
 
    bool
    BusErrorHandler::areAllSlavesReinitialized() const
    {
        bool r = false;
        for (const auto& [slave, state] : slaveStates)
        {
            r |= (state == SlaveState::Reinitializating);
        }
        return !r;
    }
 
    BusErrorHandler::Watchdog::Watchdog(std::uint32_t minDurationUS) :
        minDuration(minDurationUS), lastTime(armarx::rtNow())
    {
    }
 
    bool
    BusErrorHandler::Watchdog::expired() const
    {
        return (armarx::rtNow() - lastTime).toMicroSeconds() > minDuration;
    }
 
    void
    BusErrorHandler::Watchdog::reset()
    {
        lastTime = armarx::rtNow();
    }
 
} // namespace armarx::control::ethercat