RokubiminiEthercatSlave.hpp

Go to the documentation of this file.

#pragma once

#include <diagnostic_updater/diagnostic_updater.h>
// soem_interface
#include <rokubimini_ethercat/soem_interface/EthercatBusBase.hpp>
#include <rokubimini_ethercat/soem_interface/EthercatSlaveBase.hpp>

#include <rokubimini/Reading.hpp>
#include <rokubimini/calibration/SensorCalibration.hpp>
#include <rokubimini/configuration/SensorConfiguration.hpp>
#include <rokubimini/configuration/ForceTorqueFilter.hpp>

#include <rokubimini_ethercat/PdoTypeEnum.hpp>
#include <rokubimini_ethercat/RxPdo.hpp>

#define MAX_FILE_SIZE_FIRMWARE (0x100000)

namespace rokubimini
{
namespace ethercat
{
const static std::map<uint8_t, std::string> STATE_TO_STRING = { { EC_STATE_INIT, "Initialization state" },
                                                                { EC_STATE_PRE_OP, "Pre-Operational state" },
                                                                { EC_STATE_SAFE_OP, "Safe-Operational state" },
                                                                { EC_STATE_OPERATIONAL, "Operational state" } };
class RokubiminiEthercatSlave : public rokubimini::soem_interface::EthercatSlaveBase
{
public:
  RokubiminiEthercatSlave() = delete;

  RokubiminiEthercatSlave(const std::string& name, rokubimini::soem_interface::EthercatBusBase* bus,
                          const uint32_t address, const PdoTypeEnum pdoTypeEnum);

  ~RokubiminiEthercatSlave() override = default;

  bool startup() override;

  void preSetupConfiguration();

  void updateRead() override;

  void updateWrite() override;

  void shutdown() override;

  void setState(const uint16_t state);

  bool waitForState(const uint16_t state);

  PdoInfo getCurrentPdoInfo() const override;

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

  PdoTypeEnum getCurrentPdoTypeEnum() const
  {
    return currentPdoTypeEnum_;
  }

  bool getSerialNumber(unsigned int& serialNumber) override;

  inline unsigned int fetchSerialNumber() const
  {
    return serialNumber_;
  }

  bool getForceTorqueSamplingRate(int& samplingRate);

  bool setForceTorqueFilter(const configuration::ForceTorqueFilter& filter);

  bool setAccelerationFilter(const unsigned int filter);

  bool setAngularRateFilter(const unsigned int filter);

  bool setAccelerationRange(const unsigned int range);

  bool setAngularRateRange(const unsigned int range);

  bool setForceTorqueOffset(const Eigen::Matrix<double, 6, 1>& forceTorqueOffset);

  bool setSensorConfiguration(const configuration::SensorConfiguration& sensorConfiguration);

  bool setSensorCalibration(const calibration::SensorCalibration& sensorCalibration);

  bool setConfigMode();

  bool setRunMode();

  bool saveConfigParameter();

  void getReading(rokubimini::Reading& reading) const;

  bool firmwareUpdate(const std::string& filePath, const std::string& fileName, const uint32_t& password);

  bool isRunning()
  {
    return isRunning_;
  }

  void createDataFlagsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat);

  void updateConnectionStatus(diagnostic_updater::DiagnosticStatusWrapper& stat);

  bool runsAsync() const
  {
    return runsAsync_;
  }

  void setRunsAsync(bool runsAsync)
  {
    runsAsync_ = runsAsync;
  }

  void resetDataFlags();

  void updateDataFlags();

private:
  bool configurePdo(const PdoTypeEnum pdoTypeEnum);

  bool sendCalibrationMatrixEntry(const uint8_t subId, const double entry);

  bool readFileToBuffer(const std::string& filePath);

  uint8_t getState();

  uint16_t getALStatusCode();

  std::string getSlaveStateString(uint8_t state);

  using PdoInfos = std::map<PdoTypeEnum, PdoInfo>;

  const std::string name_;

  PdoInfos pdoInfos_;

  std::atomic<PdoTypeEnum> pdoTypeEnum_;

  std::atomic<PdoTypeEnum> currentPdoTypeEnum_;

  Reading reading_;

  int fileSize_;

  char* fileBuffer_;

  std::atomic<bool> isRunning_;

  std::atomic<bool> runsAsync_;

  Statusword currentDataFlagsDiagnostics_;

  mutable std::recursive_mutex dataFlagsMutex_;
};

using RokubiminiEthercatSlavePtr = std::shared_ptr<RokubiminiEthercatSlave>;

}  // namespace ethercat
}  // namespace rokubimini