EthercatBusBase.hpp

Go to the documentation of this file.

#pragma once

#include <cassert>
// std
#include <atomic>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

// soem
#include <soem/ethercat.h>

// soem_interface
#include <rokubimini_ethercat/soem_interface/EthercatSlaveBase.hpp>
#include <rokubimini_ethercat/soem_interface/common/ThreadSleep.hpp>

#include <ros/ros.h>
#include <ros/console.h>

namespace rokubimini
{
namespace soem_interface
{
class EthercatBusBase
{
public:
  using PdoSizePair = std::pair<uint16_t, uint16_t>;
  using PdoSizeMap = std::unordered_map<std::string, PdoSizePair>;

  EthercatBusBase() = delete;
  explicit EthercatBusBase(const std::string& name);

  ~EthercatBusBase() = default;

  const std::string& getName() const
  {
    return name_;
  }

  const ros::Time& getUpdateReadStamp() const
  {
    return updateReadStamp_;
  }

  const ros::Time& getUpdateWriteStamp() const
  {
    return updateWriteStamp_;
  }

  static bool busIsAvailable(const std::string& name);

  static void printAvailableBusses();

  bool busIsAvailable() const;

  int getNumberOfSlaves() const;

  bool addSlave(const EthercatSlaveBasePtr& slave);

  bool startup(const bool sizeCheck = true);

  void updateRead();

  void updateWrite();

  void shutdown();

  void setState(const uint16_t state, const uint16_t slave = 0);

  bool waitForState(const uint16_t state, const uint16_t slave = 0, const unsigned int maxRetries = 40,
                    const double retrySleep = 0.001);

  std::string getErrorString(ec_errort error);

  void printALStatus(const uint16_t slave = 0);

  bool checkForSdoErrors(const uint16_t slave, const uint16_t index);

  void syncDistributedClock0(const uint16_t slave, const bool activate, const double cycleTime,
                             const double cycleShift);

  PdoSizeMap getHardwarePdoSizes();

  PdoSizePair getHardwarePdoSizes(const uint16_t slave);

  template <typename Value>
  bool sendSdoWrite(const uint16_t slave, const uint16_t index, const uint8_t subindex, const bool completeAccess,
                    const Value value)
  {
    assert(static_cast<int>(slave) <= getNumberOfSlaves());
    const int size = sizeof(Value);
    Value value_copy = value;  // copy value to make it modifiable
    int wkc = 0;
    {
      std::lock_guard<std::recursive_mutex> guard(contextMutex_);
      wkc = ecx_SDOwrite(&ecatContext_, slave, index, subindex, static_cast<boolean>(completeAccess), size, &value_copy,
                         EC_TIMEOUTRXM);
    }
    if (wkc <= 0)
    {
      ROS_ERROR_STREAM("Slave " << slave << ": Working counter too low (" << wkc << ") for writing SDO (ID: 0x"
                                << std::setfill('0') << std::setw(4) << std::hex << index << ", SID 0x"
                                << std::setfill('0') << std::setw(2) << std::hex << static_cast<uint16_t>(subindex)
                                << ").");
      return false;
    }
    return true;
  }

  template <typename Value>
  bool sendSdoRead(const uint16_t slave, const uint16_t index, const uint8_t subindex, const bool completeAccess,
                   Value& value)
  {
    assert(static_cast<int>(slave) <= getNumberOfSlaves());
    int size = sizeof(Value);
    int wkc = 0;
    {
      std::lock_guard<std::recursive_mutex> guard(contextMutex_);
      wkc = ecx_SDOread(&ecatContext_, slave, index, subindex, static_cast<boolean>(completeAccess), &size, &value,
                        EC_TIMEOUTRXM);
    }
    if (wkc <= 0)
    {
      ROS_ERROR_STREAM("Slave " << slave << ": Working counter too low (" << wkc << ") for reading SDO (ID: 0x"
                                << std::setfill('0') << std::setw(4) << std::hex << index << ", SID 0x"
                                << std::setfill('0') << std::setw(2) << std::hex << static_cast<uint16_t>(subindex)
                                << ").");
      return false;
    }
    if (size != sizeof(Value))
    {
      ROS_ERROR_STREAM("Slave " << slave << ": Size mismatch (expected " << sizeof(Value) << " bytes, read " << size
                                << " bytes) for reading SDO (ID: 0x" << std::setfill('0') << std::setw(4) << std::hex
                                << index << ", SID 0x" << std::setfill('0') << std::setw(2) << std::hex
                                << static_cast<uint16_t>(subindex) << ").");
      return false;
    }
    return true;
  }

  int getExpectedWorkingCounter(const uint16_t slave = 0) const;

  bool workingCounterIsOk() const;

  template <typename TxPdo>
  void readTxPdo(const uint16_t slave, TxPdo& txPdo) const
  {
    assert(static_cast<int>(slave) <= getNumberOfSlaves());
    std::lock_guard<std::recursive_mutex> guard(contextMutex_);
    assert(sizeof(TxPdo) == ecatContext_.slavelist[slave].Ibytes);
    memcpy(&txPdo, ecatContext_.slavelist[slave].inputs, sizeof(TxPdo));
  }

  template <typename RxPdo>
  void writeRxPdo(const uint16_t slave, const RxPdo& rxPdo)
  {
    assert(static_cast<int>(slave) <= getNumberOfSlaves());
    std::lock_guard<std::recursive_mutex> guard(contextMutex_);
    assert(sizeof(RxPdo) == ecatContext_.slavelist[slave].Obytes);
    memcpy(ecatContext_.slavelist[slave].outputs, &rxPdo, sizeof(RxPdo));
  }

  int writeFile(const uint16_t slave, const std::string& fileName, const uint32_t& password, const int fileSize,
                char* fileBuffer, int timeout = EC_TIMEOUTSTATE * 10);
  bool writeFirmware(const uint16_t slave, const std::string& fileName, const uint32_t& password, const int fileSize,
                     char* fileBuffer);

  bool isRunning()
  {
    return isRunning_;
  }

protected:
  std::string name_;

  std::vector<EthercatSlaveBasePtr> slaves_;

  bool sentProcessData_{ false };

  std::atomic<int> wkc_;

  std::atomic<bool> isRunning_;

  ros::Time updateReadStamp_;
  ros::Time updateWriteStamp_;

  const unsigned int ecatConfigMaxRetries_{ 5 };
  const double ecatConfigRetrySleep_{ 1.0 };

  // EtherCAT input/output mapping of the slaves within the datagrams.
  char ioMap_[4096];

  // EtherCAT context data elements:

  // Port reference.
  ecx_portt ecatPort_;
  // List of slave data. Index 0 is reserved for the master, higher indices for the slaves.
  ec_slavet ecatSlavelist_[EC_MAXSLAVE];
  // Number of slaves found in the network.
  int ecatSlavecount_{ 0 };
  // Slave group structure.
  ec_groupt ecatGrouplist_[EC_MAXGROUP];
  // Internal, reference to EEPROM cache buffer.
  uint8 ecatEsiBuf_[EC_MAXEEPBUF];
  // Internal, reference to EEPROM cache map.
  uint32 ecatEsiMap_[EC_MAXEEPBITMAP];
  // Internal, reference to error list.
  ec_eringt ecatEList_;
  // Internal, reference to processdata stack buffer info.
  ec_idxstackT ecatIdxStack_;
  // Boolean indicating if an error is available in error stack.
  boolean ecatError_{ FALSE };
  // Reference to last DC time from slaves.
  int64 ecatDcTime_{ 0 };
  // Internal, SM buffer.
  ec_SMcommtypet ecatSmCommtype_[EC_MAX_MAPT];
  // Internal, PDO assign list.
  ec_PDOassignt ecatPdoAssign_[EC_MAX_MAPT];
  // Internal, PDO description list.
  ec_PDOdesct ecatPdoDesc_[EC_MAX_MAPT];
  // Internal, SM list from EEPROM.
  ec_eepromSMt ecatSm_;
  // Internal, FMMU list from EEPROM.
  ec_eepromFMMUt ecatFmmu_;

  mutable std::recursive_mutex contextMutex_;
  // EtherCAT context data.
  // Note: SOEM does not use dynamic memory allocation (new/delete). Therefore
  // all context pointers must be null or point to an existing member.
  ecx_contextt ecatContext_ = { &ecatPort_,
                                &ecatSlavelist_[0],
                                &ecatSlavecount_,
                                EC_MAXSLAVE,
                                &ecatGrouplist_[0],
                                EC_MAXGROUP,
                                &ecatEsiBuf_[0],
                                &ecatEsiMap_[0],
                                0,
                                &ecatEList_,
                                &ecatIdxStack_,
                                &ecatError_,
                                0,
                                0,
                                &ecatDcTime_,
                                &ecatSmCommtype_[0],
                                &ecatPdoAssign_[0],
                                &ecatPdoDesc_[0],
                                &ecatSm_,
                                &ecatFmmu_,
                                nullptr };
};

using EthercatBusBasePtr = std::shared_ptr<EthercatBusBase>;

}  // namespace soem_interface
}  // namespace rokubimini