BusIO.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarX
 * @author     Stefan Reither( stefan dot reither at kit dot edu)
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
#include <atomic>
#include <cstdint>
#include <mutex>
#include <thread>
#include <array>
 
#include "ESI.h"
#include "RequestQueue.h"
#include "SlaveRegisters.h"
#include "Timeouts.h"
 
namespace armarx::control::ethercat
{
    class ChangeStateRequest;
    class ReadStatesRequest;
    class SDOUpdateRequest;
    class RegisterResetRequest;
    class RegisterReadRequest;
 
    struct EtherCATFrame;
    struct EtherCATFrameList;
 
    /**
     * @class BusIO
     * @ingroup Library-ethercat
     * @brief Brief description of class BusIO.
     *
     * Detailed description of class BusIO.
     */
    class BusIO : public ESIHandler
    {
    public:
        virtual ~BusIO();
 
        /**
         * Performs a SDO write to a single entry to the slave with the give slaveIndex
         * and returns true if it succeeds.
         * Fails if the bus has not been started yet.
         *
         * Blocking until the write has been performed or a timeout is reached.
         *
         * @param [IN] slaveIndex the slavenumber of the slave on the bus
         * @param [IN] index the index of the entry where the value is written to
         * @param [IN] subIndex the subindex of the entry
         * @param [IN] value the value that will be written to the slave
         * @param [IN] completeAccess with this flag you can activate writing in complete access
         *             mode
         * @return true when write was successful otherwise false
         */
        template <typename T, typename = std::enable_if_t<std::is_integral_v<T>>>
        bool
        writeSDOEntry(std::uint16_t slaveIndex,
                      std::uint16_t index,
                      std::uint8_t subIndex,
                      T value,
                      bool completeAccess = false)
        {
            return generalSDOWrite(
                slaveIndex, index, subIndex, sizeof(value), &value, completeAccess);
        }
 
        /**
         * Performs a SDO write to the slave with the given slaveIndex and returns true if it
         * succeeds.
         * Fails if the bus has not been started yet.
         *
         * Blocking until the write has been performed or a timeout is reached.
         *
         * @param [IN] slaveIndex the slavenumber of the slave on the bus
         * @param [IN] index the index of the entry where the value is written to
         * @param [IN] subIndex the subindex of the entry
         * @param [IN] buflen length of the buffer containing the data
         * @param [IN] buf buffer containing the data to write
         * @param [IN] completeAccess with this flag you can activate writing in complete access
         *             mode
         * @return true when write was successful otherwise false
         */
        bool
        writeSDOByteBuffer(std::uint16_t slaveIndex,
                           std::uint16_t index,
                           std::uint8_t subIndex,
                           int buflen,
                           const unsigned char* buf,
                           bool completeAccess = false)
        {
            return generalSDOWrite(slaveIndex, index, subIndex, buflen, buf, completeAccess);
        }
 
        /**
         * Performs a SDO read of an single entry from the slave and returns true if it
         * succeeds.
         * Fails if the bus has not been started yet.
         *
         * Blocking until the write has been performed or a timeout is reached.
         *
         * @param [IN] slaveIndex the index of the slave it will read from
         * @param [IN] index the index of the object dictonary it will read from
         * @param [IN] subIndex the sub index of the entry
         * @param [OUT] result the read data
         * @param [IN] completeAccess with this flag you can activate writing in complete access
         *             mode
         * @return true when write was successful otherwise false
         */
        template <typename T, typename = std::enable_if_t<std::is_integral_v<T>>>
        bool
        readSDOEntry(std::uint16_t slaveIndex,
                     std::uint16_t index,
                     std::uint8_t subIndex,
                     T& result,
                     bool completeAccess = false)
        {
            int buflen = sizeof(result);
            return generalSDORead(
                slaveIndex, index, subIndex, &buflen, &result, completeAccess);
        }
 
        /**
         * Performs a SDO read from the slave and returns true if it
         * succeeds.
         * Fails if the bus has not been started yet.
         *
         * Blocking until the write has been performed or a timeout is reached.
         *
         * @param [IN] slaveIndex the index of the slave it will read from
         * @param [IN] index the index of the object dictonary it will read from
         * @param [IN] subIndex the sub index of the entry
         * @param [IN] buflen amount of bytes to read
         * @param [OUT] buf buffer which contains the read data afterwards
         * @param [IN] completeAccess with this flag you can activate writing in complete access
         *             mode
         * @return true when write was successful otherwise false
         */
        bool
        readSDOByteBuffer(std::uint16_t slaveIndex,
                          std::uint16_t index,
                          std::uint8_t subIndex,
                          int* buflen,
                          unsigned char* buf,
                          bool completeAccess = false)
        {
            return generalSDORead(slaveIndex, index, subIndex, buflen, buf, completeAccess);
        }
 
        EtherCATState
        changeStateOfSlave(std::uint16_t slaveIndex, EtherCATState state, bool validate = true);
        EtherCATState changeStateOfBus(EtherCATState state, bool validate = true);
 
        EtherCATState readStates();
 
        bool resetErrorRegisters(std::uint16_t slaveIndex);
 
        bool readRegisters(std::vector<RegisterDataList>& registerData);
        bool readRegisters(EtherCATFrameList* frames, std::uint16_t amountFramesToRead = 0);
 
        /**
         * This deactivates the Complete access mode in CoE for the given slave.
         * For Elmo's it is necessary to deactivate the CA mode otherwise SOEM isn't able to bring
         * them into OP-Mode
         * @param slave the slave for which the CA mode will be deactivated
         */
        void deactivateCOECA(std::uint16_t slaveIndex);
 
        void setTimeouts(Timeouts const& timeouts);
 
    protected:
        BusIO();
 
        /**
         * @brief Updates the PDO of all slaves.
         * This function has priority over all other BusIO functions and can only be executed by the
         * Bus itself.
         */
        void rtUpdatePDO();
 
        bool sdoAccessAvailable = false;
        bool pdoAccessAvailable = false;
 
        Timeouts timeouts;
 
        int lastWorkCounter = 0;
 
        /** @brief IOmap the IO map where the process data are mapped in */
        using IOMap = std::array<char, 4096>;
        alignas(alignof(std::max_align_t)) IOMap ioMap;
 
 
    private:
        bool generalSDOWrite(std::uint16_t slaveIndex,
                             std::uint16_t index,
                             std::uint8_t subIndex,
                             std::uint16_t buflen,
                             const void* buf,
                             bool completeAccess = false);
 
        bool generalSDORead(std::uint16_t slaveIndex,
                            std::uint16_t index,
                            std::uint8_t subIndex,
                            int* buflen,
                            void* buf,
                            bool completeAccess = false);
 
        bool isSDOAccessAvailable() const;
 
        /*!
         * \brief Sends an EtherCAT frame over the EtherCAT bus, receives it and returns the
         * index of the SOEM buffer the resulting frame was placed in along with the working
         * counter returned by SOEM.
         *
         * This method will automatically place the given slave address at the given offsets
         * in the final EtherCAT frame in the buffer. Pass an empty offset vector if you do not
         * require this.
         * \param frame the EtherCAT frame to send over the bus
         * \param frameLength the length of the EtherCAT frame in bytes
         * \param slaveConfiguredAddress the configured address of the slave to send the frame to,
         * if it needs to be set in the frame
         * \param slaveAddressOffsets the offsets in the EtherCAT frame to write the configured
         * address of the slave to
         * \return the working counter of the frame as returned by SOEM and the index of the SOEM
         * rx frame buffer the received frame has been placed in.
         */
        std::pair<int, int>
        sendAndReceiveEtherCATFrame(const EtherCATFrame* frame,
                                    size_t frameLength,
                                    uint16_t slaveConfiguredAddress,
                                    const std::vector<size_t>& slaveAddressOffsets);
 
        RequestQueue requestQueues;
 
        std::thread requestHandlerThread;
        std::atomic_bool requestHandlerThreadRunning{true};
        std::atomic_bool pdoUpdateRequested{false};
 
        void requestHandlerLoop();
 
        bool handleNextRequest();
        void handleSDOUpdateRequest(const std::shared_ptr<SDOUpdateRequest>& request);
        void handleChangeStateRequest(const std::shared_ptr<ChangeStateRequest>& request);
        void handleReadStatesRequest(const std::shared_ptr<ReadStatesRequest>& request);
        void handleRegisterResetRequest(const std::shared_ptr<RegisterResetRequest>& request);
        void handleRegisterReadRequest(const std::shared_ptr<RegisterReadRequest>& request);
    };
} // namespace armarx::control::ethercat