soem.h

Go to the documentation of this file.

#pragma once
 
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>
#include <variant>
#include <vector>
 
#include "base.h"
#include "ethercat.h"
#include "loguru.h"
 
namespace mm::comm::soem {

std::map<std::string, std::string> mapMacAddressesToInterfaces();

inline std::string uiConfigWithDefaultPdoMapping = R"(
{
  "pdoMapping": {
    "rx": {
      "0x1600": [
        "0x60400010",
        "0x60600008",
        "0x60710010",
        "0x607a0020",
        "0x60ff0020",
        "0x60b20010",
        "0x27010020"
      ],
      "0x1601": [
        "0x60fe0120",
        "0x60fe0220"
      ],
      "0x1602": [
        "0x27030020",
        "0x60b10020"
      ]
    },
    "tx": {
      "0x1a00": [
        "0x60410010",
        "0x60610008",
        "0x60640020",
        "0x606c0020",
        "0x60770010",
        "0x60f40020",
        "0x21110220",
        "0x21130220"
      ],
      "0x1a01": [
        "0x24010010",
        "0x24020010",
        "0x24030010",
        "0x24040010",
        "0x27020020"
      ],
      "0x1a02": [
        "0x60fd0020"
      ],
      "0x1a03": [
        "0x27040020",
        "0x20f00020",
        "0x60fc0020",
        "0x606b0020",
        "0x60740010",
        "0x60790020"
      ]
    }
  }
})";

int setPdoMappingFromUiConfig(ecx_contextt* context, uint16 slave);

void checkPdoMapping(ecx_contextt* context, uint16 slave);

uint16_t getSlaveState(ecx_contextt* context, uint16_t slave,
                       bool forceRefresh);

std::string convertSlaveStateToString(int state);

bool transitionToHigherSlaveState(ecx_contextt* context, uint16_t slave,
                                  int targetState);

void updateMailboxSyncManagersOnNextState(ecx_contextt* context, uint16_t slave,
                                          int targetState);

void configureDetectedSmmModule(ecx_contextt* context, uint16_t slave);

bool setSlaveState(ecx_contextt* context, uint16_t slave, int targetState);

mm::comm::base::PdoMappings readPdoMappings(ecx_contextt* context,
                                            uint16_t slave);

void logPdoMappingEntries(
    const std::vector<mm::comm::base::PdoMappingEntry>& entries);

void logIoMap(uint8_t* ioMap, size_t totalBytes);

struct Fieldbus {
  ecx_contextt context;
  std::string iface;
  uint8 group;
  int roundtripTime;
 
  /* Used by the context */
  uint8 map[4096];
  ecx_portt port;
  ec_slavet slavelist[EC_MAXSLAVE];
  int slavecount;
  ec_groupt grouplist[EC_MAXGROUP];
  uint8 esibuf[EC_MAXEEPBUF];
  uint32 esimap[EC_MAXEEPBITMAP];
  ec_eringt elist;
  ec_idxstackT idxstack;
  boolean ecaterror;
  int64 DCtime;
  ec_SMcommtypet SMcommtype[EC_MAX_MAPT];
  ec_PDOassignt PDOassign[EC_MAX_MAPT];
  ec_PDOdesct PDOdesc[EC_MAX_MAPT];
  ec_eepromSMt eepSM;
  ec_eepromFMMUt eepFMMU;
};

void initFieldbus(std::unique_ptr<Fieldbus>& fieldbus,
                  const std::string& iface);

class Slave {
 public:
  explicit Slave(ecx_contextt& context, uint16_t position);

  uint16_t getPosition() const;

  uint32_t getVendorId() const;

  uint32_t getProductCode() const;

  uint32_t getRevisionNumber() const;

  int getState(bool forceRefresh = true) const;

  bool setState(int targetState, int timeoutMs = 0, int intervalMs = 100);

  void loadParameters(bool readValues = false);

  void logParameters() const;

  void logRxPdoMappedParameters();

  void logTxPdoMappedParameters();

  void clearParameters();

  std::unordered_map<mm::comm::base::Parameter::Address,
                     mm::comm::base::Parameter>&
  getParametersMap();

  mm::comm::base::Parameter& findParameter(uint16_t index, uint8_t subindex);

  template <typename T = std::vector<uint8_t>>
  T upload(uint16_t index, uint8_t subindex, bool refresh = true) {
    // Lock the mailbox mutex to ensure thread safety
    std::lock_guard<std::mutex> lock(mailboxMutex_);
 
    // Check if the device is in a valid state for uploading
    auto state = getState(false);
    if (state == EC_STATE_INIT || state == EC_STATE_BOOT) {
      throw std::runtime_error(
          "To upload object dictionary entries, the device must be in the (2) "
          "PRE-OPERATIONAL, (4) SAFE-OPERATIONAL, or (8) OPERATIONAL state. "
          "The current state is " +
          convertSlaveStateToString(state) + ".");
    }
 
    // Find the previously loaded parameter
    auto& parameter = findParameter(index, subindex);
 
    // Refresh data if needed (SDO read)
    if (refresh) {
      const std::unique_ptr<std::uint8_t[]> data =
          std::make_unique<std::uint8_t[]>(parameter.byteLength);
      auto wkc = ecx_SDOread(&context_, position_, parameter.index,
                             parameter.subindex, false, &parameter.byteLength,
                             data.get(), EC_TIMEOUTRXM * 3);
      if (wkc <= 0) {
        LOG_F(ERROR, "Device %d: SDO read failed for %#04x:%02x! WKC: %d",
              position_, parameter.index, parameter.subindex, wkc);
        auto id = mm::comm::base::makeParameterId(parameter.index,
                                                  parameter.subindex);
        throw std::runtime_error("Device " + std::to_string(position_) +
                                 ": SDO read failed for " + id +
                                 "! WKC: " + std::to_string(wkc));
      } else {
        std::vector<uint8_t> receivedData(data.get(),
                                          data.get() + parameter.byteLength);
        if (!parameter.trySetValue(receivedData)) {
          LOG_F(ERROR,
                "Device %d: Failed to set the value of parameter %#04x:%02x!",
                position_, parameter.index, parameter.subindex);
        }
      }
    }
 
    // Return the appropriate type
    if constexpr (std::is_same_v<T, std::vector<uint8_t>>) {
      return parameter.data;  // parameter.data should be std::vector<uint8_t>
    } else {
      // Otherwise, try to get the value as the requested type T
      auto value = parameter.getValue();
      if (auto ptr = std::get_if<T>(&value)) {
        return *ptr;
      } else {
        throw std::bad_variant_access();  // Type mismatch
      }
    }
  }

  template <typename T = std::vector<uint8_t>>
  bool download(uint16_t index, uint8_t subindex, const T& value) {
    std::lock_guard<std::mutex> lock(mailboxMutex_);
 
    // Check if device is in valid state for download
    auto state = getState(false);
    if (state == EC_STATE_INIT || state == EC_STATE_BOOT) {
      throw std::runtime_error(
          "To download object dictionary entries, the device must be in the "
          "(2) PRE-OPERATIONAL, (4) SAFE-OPERATIONAL, or (8) OPERATIONAL "
          "state. The current state is " +
          convertSlaveStateToString(state) + ".");
    }
 
    auto& parameter = findParameter(index, subindex);
 
    // Convert value to raw byte data
    std::vector<uint8_t> data;
    if constexpr (std::is_same_v<T, std::vector<uint8_t>>) {
      data = value;
    } else {
      // Set the value in the parameter model
      if (!parameter.trySetValue(value)) {
        LOG_F(ERROR,
              "Device %d: Failed to set the value of parameter %#04x:%02x!",
              position_, parameter.index, parameter.subindex);
        return false;
      }
      data = parameter.data;  // Use the parameter's byte representation
    }
 
    // Check if data size matches the expected parameter size
    if (data.size() != parameter.byteLength) {
      LOG_F(ERROR, "Data size mismatch: expected %d, got %zu",
            parameter.byteLength, data.size());
      throw std::runtime_error("Data size does not match the parameter size!");
    }
 
    // Perform the SDO write
    auto wkc = ecx_SDOwrite(&context_, position_, parameter.index,
                            parameter.subindex, false, parameter.byteLength,
                            data.data(), EC_TIMEOUTRXM * 3);
 
    // Check write result
    if (wkc <= 0) {
      LOG_F(ERROR, "Device %d: SDO write failed for %#04x:%02x! WKC: %d",
            position_, parameter.index, parameter.subindex, wkc);
      return false;
    }
 
    return true;
  }

  mm::comm::base::PdoMappings readPdoMappings() const;

  void updatePdoMappings();

  void logPdoMappings() const;

  mm::comm::base::PdoMappings& getPdoMappings();

  void updateOutputs();

  void updateParametersFromInputs();
 
  std::vector<uint8_t> readFile(
      const std::string& filename,
      const std::chrono::steady_clock::duration expiryTime);
 
 private:
  ecx_contextt& context_;    
  const uint16_t position_;  
  std::mutex mailboxMutex_;  
  std::unordered_map<mm::comm::base::Parameter::Address,
                     mm::comm::base::Parameter>
      parametersMap_;  
  mm::comm::base::PdoMappings
      pdoMappings_;  
};
 
class Master {
 public:
  explicit Master();

  explicit Master(const std::string& iface);

  void init(const std::string& iface);

  void deinit();

  ~Master();

  Fieldbus& getFieldbus() const;

  std::string getInterfaceName() const;

  size_t getIoMapSize() const;

  int initSlaves();

  const std::vector<std::unique_ptr<Slave>>& getSlaves() const;

  inline int expectedWkc() const {
    return fieldbus_->context.grouplist[fieldbus_->group].outputsWKC * 2 +
           fieldbus_->context.grouplist[fieldbus_->group].inputsWKC;
  }

  int roundtrip();

  void exchangeProcessDataAndUpdateParameters();

  bool start();
 
 private:
  std::unique_ptr<Fieldbus> fieldbus_;

  std::vector<std::unique_ptr<Slave>> slaves_;
};
};  // namespace mm::comm::soem