diff --git a/fake_lib/fakeethercat.cpp b/fake_lib/fakeethercat.cpp index 6694f09d..2d6ed317 100644 --- a/fake_lib/fakeethercat.cpp +++ b/fake_lib/fakeethercat.cpp @@ -331,7 +331,7 @@ int ecrt_slave_config_complete_sdo( size_t size /**< Size of the \a data. */ ) { - return -1; + return ecrt_slave_config_sdo(sc, index, 0, data, size); } ec_sdo_request_t *ecrt_slave_config_create_sdo_request( ec_slave_config_t *sc, /**< Slave configuration. */ @@ -437,6 +437,40 @@ int ecrt_slave_config_reg_pdo_entry( return -1; // offset } +int ecrt_slave_config_sdo8( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint8_t value /**< Value to set. */ +) +{ + return ecrt_slave_config_sdo(sc, sdo_index, sdo_subindex, &value, 1); +} + +int ecrt_slave_config_sdo16( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint16_t const value /**< Value to set. */ +) +{ + uint8_t buf[sizeof(value)]; + memcpy(&buf, &value, sizeof(value)); + return ecrt_slave_config_sdo(sc, sdo_index, sdo_subindex, buf, sizeof(buf)); +} + +int ecrt_slave_config_sdo32( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint32_t const value /**< Value to set. */ +) +{ + uint8_t buf[sizeof(value)]; + memcpy(&buf, &value, sizeof(value)); + return ecrt_slave_config_sdo(sc, sdo_index, sdo_subindex, buf, sizeof(buf)); +} + int ecrt_slave_config_sdo( ec_slave_config_t *sc, /**< Slave configuration. */ uint16_t index, /**< Index of the SDO to configure. */ @@ -445,7 +479,8 @@ int ecrt_slave_config_sdo( size_t size /**< Size of the \a data. */ ) { - return -1; + sc->sdos[sdo_address{index, subindex}] = std::basic_string(data, data + size); + return 0; } void ecrt_write_lreal(void *data, double const value) diff --git a/fake_lib/fakeethercat.h b/fake_lib/fakeethercat.h index 1ae95045..2a26d5d8 100644 --- a/fake_lib/fakeethercat.h +++ b/fake_lib/fakeethercat.h @@ -30,6 +30,7 @@ #include #include #include +#include #include struct Offset @@ -89,12 +90,40 @@ public: } }; + +class sdo_address +{ + uint32_t value; + +public: + sdo_address(uint16_t index, /**< Slave alias. */ + uint8_t subindex /**< Slave position. */) + : value(static_cast(index) << 8 | subindex) + { + } + + uint16_t getIndex() const { return value >> 8; } + uint8_t getSubIndex() const { return value & 0xFF; } + uint32_t getCombined() const { return value; } + + bool operator<(const sdo_address &other) const noexcept + { + return value < other.value; + } + + bool operator==(const sdo_address &other) const noexcept + { + return value == other.value; + } +}; + struct ec_slave_config { ec_address address; uint32_t vendor_id; /**< Expected vendor ID. */ uint32_t product_code; /**< Expected product code. */ std::map sync_managers; + std::map> sdos; ec_slave_config( ec_address address,