Mirrors_Framework/ethercat
1
0
Fork 0
mirror of https://gitlab.com/etherlab.org/ethercat.git synced 2026-02-05 11:29:52 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity
Files
stable-1.6
ethercat/fake_lib/fakeethercat.cpp
Florian Pose 2543cc55bd Version 1.6.8: Fixed FAKE_EC_HOMEDIR usage.
2025-10-06 17:15:04 +02:00

1514 lines
42 KiB
C++
Raw Permalink Blame History

/*****************************************************************************
*
* Copyright (C) 2024 Bjarne von Horn, Ingenieurgemeinschaft IgH
* 2025 Florian Pose <fp@igh.de>
*
* This file is part of the IgH EtherCAT master userspace library.
*
* The IgH EtherCAT master userspace library is free software; you can
* redistribute it and/or modify it under the terms of the GNU Lesser General
* Public License as published by the Free Software Foundation; version 2.1
* of the License.
*
* The IgH EtherCAT master userspace library 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with the IgH EtherCAT master userspace library. If not, see
* <http://www.gnu.org/licenses/>.
*
****************************************************************************/
#include "fakeethercat.h"
#include <cstring>
#include <fstream>
#include <ios>
#include <iterator>
#include <sstream>
#include <string>
#include <unordered_set>
#include <utility>
#include <sys/stat.h>
/****************************************************************************/
static std::ostream &operator<<(std::ostream &os, const sdo_address &a)
{
os << std::setfill('0') << std::hex << std::setw(6) << a.getCombined();
return os;
}
static std::ostream &operator<<(std::ostream &os, const ec_address &a)
{
os << std::setfill('0') << std::hex << std::setw(8) << a.getCombined();
return os;
}
static void add_spaces(std::ostream &out, int const num)
{
for (int i = 0; i < num; ++i) {
out << ' ';
}
}
template <typename Map, typename Func> static void
map2Json(std::ostream &out, const Map &map, Func &&print_func, int indent = 0)
{
indent += 4;
out << "{";
bool is_first = true;
for (const auto &kv : map) {
if (is_first) {
out << '\n';
is_first = false;
}
else {
out << ",\n";
}
add_spaces(out, indent);
out << "\"0x" << std::hex << std::setfill('0')
<< std::setw(2 * sizeof(typename Map::key_type));
out << kv.first << "\": ";
print_func(out, kv.second);
}
out << '\n';
add_spaces(out, indent - 4);
out << "}";
}
/*****************************************************************************
* Pdo
****************************************************************************/
size_t pdo::sizeInBytes() const
{
size_t ans = 0;
for (const auto &entry : entries) {
ans += entry.bit_length;
}
return (ans + 7) / 8;
}
Offset pdo::findEntry(uint16_t idx, uint8_t subindex) const
{
size_t offset_bits = 0;
for (const auto &entry : entries) {
if (entry.index == idx && entry.subindex == subindex) {
return Offset(offset_bits / 8, offset_bits % 8);
}
offset_bits += entry.bit_length;
}
return NotFound;
}
Offset pdo::findEntryByPos(unsigned int entry_pos) const
{
size_t offset_bits = 0;
unsigned int pos {0};
for (const auto &entry : entries) {
if (pos == entry_pos) {
return Offset(offset_bits / 8, offset_bits % 8);
}
offset_bits += entry.bit_length;
pos++;
}
return NotFound;
}
/*****************************************************************************
* Common
****************************************************************************/
unsigned int ecrt_version_magic(void)
{
return ECRT_VERSION_MAGIC;
}
ec_master_t *ecrt_request_master(
unsigned int master_index /**< Index of the master to request. */
)
{
return new ec_master(master_index);
}
ec_master_t *ecrt_open_master(
unsigned int master_index /**< Index of the master to request. */
)
{
return new ec_master(master_index);
}
void ecrt_release_master(ec_master_t *master)
{
delete master;
}
/*****************************************************************************
* Master
****************************************************************************/
int ecrt_master_reserve(ec_master_t *master /**< EtherCAT master */
)
{
return 0;
}
static const char *getPrefix()
{
if (const auto ans = getenv("FAKE_EC_PREFIX")) {
return ans;
}
return "/FakeEtherCAT";
}
ec_domain *ec_master::createDomain()
{
domains.emplace_back(rt_ipc.get(), getPrefix(), this);
return &domains.back();
}
ec_domain_t *
ecrt_master_create_domain(ec_master_t *master /**< EtherCAT master. */
)
{
return master->createDomain();
}
ec_slave_config_t *ec_master::slave_config(
uint16_t alias, /**< Slave alias. */
uint16_t position, /**< Slave position. */
uint32_t vendor_id, /**< Expected vendor ID. */
uint32_t product_code /**< Expected product code. */
)
{
const ec_address address {alias, position};
const auto it = slaves.find(address);
if (it != slaves.end()) {
if (it->second.vendor_id == vendor_id
&& it->second.product_code == product_code) {
return &it->second;
}
else {
std::cerr << "Attempted to reconfigure slave (" << alias << ","
<< position << ")!\n";
return nullptr;
}
}
else {
return &slaves.insert(std::make_pair<ec_address, ec_slave_config>(
ec_address {address},
ec_slave_config {
address,
vendor_id,
product_code}))
.first->second;
}
}
ec_slave_config_t *ecrt_master_slave_config(
ec_master_t *master, /**< EtherCAT master */
uint16_t alias, /**< Slave alias. */
uint16_t position, /**< Slave position. */
uint32_t vendor_id, /**< Expected vendor ID. */
uint32_t product_code /**< Expected product code. */
)
{
return master->slave_config(alias, position, vendor_id, product_code);
}
int ecrt_master_select_reference_clock(
ec_master_t *master, /**< EtherCAT master. */
ec_slave_config_t *sc /**< Slave config of the slave to use as the
* reference slave (or NULL). */
)
{
return 0;
}
int ecrt_master(
ec_master_t *master, /**< EtherCAT master */
ec_master_info_t *master_info /**< Structure that will output
the information */
)
{
master_info->slave_count = 0;
master_info->link_up = 1;
master_info->scan_busy = 0;
master_info->app_time = 0;
return 0;
}
int ecrt_master_scan_progress(
ec_master_t *master, /**< EtherCAT master */
ec_master_scan_progress_t *progress /**< Structure that will output
the progress information. */
)
{
progress->scan_index = progress->slave_count = master->getNoSlaves();
return 0;
}
int ecrt_master_get_slave(
ec_master_t *master, /**< EtherCAT master */
uint16_t slave_position, /**< Slave position. */
ec_slave_info_t *slave_info /**< Structure that will output the
information */
)
{
slave_info->position = slave_position;
slave_info->vendor_id = 0x00000000;
slave_info->product_code = 0x00000000;
slave_info->revision_number = 0x00000000;
slave_info->serial_number = 0x00000000;
slave_info->alias = 0x0000;
slave_info->current_on_ebus = 0;
for (unsigned int i = 0; i < EC_MAX_PORTS; i++) {
slave_info->ports[i].desc = EC_PORT_NOT_IMPLEMENTED;
slave_info->ports[i].link.link_up = 0;
slave_info->ports[i].link.loop_closed = 1;
slave_info->ports[i].link.signal_detected = 0;
slave_info->ports[i].next_slave = 0;
slave_info->ports[i].delay_to_next_dc = 0;
}
slave_info->al_state = EC_AL_STATE_OP;
slave_info->error_flag = 0;
slave_info->sync_count = 8;
slave_info->sdo_count = 0;
slave_info->name[0] = 0;
return 0;
}
int ecrt_master_get_sync_manager(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint8_t sync_index, /**< Sync manager index. Must be less
than #EC_MAX_SYNC_MANAGERS. */
ec_sync_info_t *sync /**< Pointer to output structure. */
)
{
sync->index = sync_index;
sync->dir = EC_DIR_INVALID;
sync->n_pdos = 0;
sync->pdos = nullptr;
sync->watchdog_mode = EC_WD_DEFAULT;
return 0;
}
int ecrt_master_get_pdo(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint8_t sync_index, /**< Sync manager index. Must be less
than #EC_MAX_SYNC_MANAGERS. */
uint16_t pos, /**< Zero-based PDO position. */
ec_pdo_info_t *pdo /**< Pointer to output structure. */
)
{
return -ENOENT;
}
int ecrt_master_get_pdo_entry(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint8_t sync_index, /**< Sync manager index. Must be less
than #EC_MAX_SYNC_MANAGERS. */
uint16_t pdo_pos, /**< Zero-based PDO position. */
uint16_t entry_pos, /**< Zero-based PDO entry position. */
ec_pdo_entry_info_t *entry /**< Pointer to output structure. */
)
{
return -ENOENT;
}
int ecrt_master_sdo_download(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint16_t index, /**< Index of the SDO. */
uint8_t subindex, /**< Subindex of the SDO. */
const uint8_t *data, /**< Data buffer to download. */
size_t data_size, /**< Size of the data buffer. */
uint32_t *abort_code /**< Abort code of the SDO download. */
)
{
return 0;
}
int ecrt_master_sdo_download_complete(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint16_t index, /**< Index of the SDO. */
const uint8_t *data, /**< Data buffer to download. */
size_t data_size, /**< Size of the data buffer. */
uint32_t *abort_code /**< Abort code of the SDO download. */
)
{
return 0;
}
int ecrt_master_sdo_upload(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint16_t index, /**< Index of the SDO. */
uint8_t subindex, /**< Subindex of the SDO. */
uint8_t *target, /**< Target buffer for the upload. */
size_t target_size, /**< Size of the target buffer. */
size_t *result_size, /**< Uploaded data size. */
uint32_t *abort_code /**< Abort code of the SDO upload. */
)
{
*result_size = 0;
return 0;
}
int ecrt_master_write_idn(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint8_t drive_no, /**< Drive number. */
uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
const uint8_t *data, /**< Pointer to data to write. */
size_t data_size, /**< Size of data to write. */
uint16_t *error_code /**< Pointer to variable, where an SoE error code
can be stored. */
)
{
return 0;
}
int ecrt_master_read_idn(
ec_master_t *master, /**< EtherCAT master. */
uint16_t slave_position, /**< Slave position. */
uint8_t drive_no, /**< Drive number. */
uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */
uint8_t *target, /**< Pointer to memory where the read data can be
stored. */
size_t target_size, /**< Size of the memory \a target points to. */
size_t *result_size, /**< Actual size of the received data. */
uint16_t *error_code /**< Pointer to variable, where an SoE error code
can be stored. */
)
{
*result_size = 0;
return 0;
}
int ec_master::activate()
{
for (auto &domain : domains) {
if (domain.activate()) {
return -1;
}
}
{
std::ofstream out(rt_ipc_dir + "/" + rt_ipc_name + "_slaves.json");
if (!out.is_open()) {
std::cerr << "could not dump json.\n";
return -1;
}
out << "{\n \"slaves\": ";
map2Json(
out,
slaves,
[](std::ostream &out, const ec_slave_config &slave) {
slave.dumpJson(out, 8);
},
4);
out << "\n}\n";
}
return rtipc_prepare(rt_ipc.get());
}
int ecrt_master_activate(ec_master_t *master /**< EtherCAT master. */
)
{
try {
return master->activate();
}
catch (const std::exception &e) {
std::cerr << "Could not activate: " << e.what() << '\n';
return -1;
}
}
int ec_master::deactivate()
{
rt_ipc.reset(rtipc_create(rt_ipc_name.c_str(), rt_ipc_dir.c_str()));
domains.clear();
slaves.clear();
return 0;
}
int ecrt_master_deactivate(ec_master_t *master /**< EtherCAT master. */
)
{
return master->deactivate();
}
int ecrt_master_set_send_interval(
ec_master_t *master, /**< EtherCAT master. */
size_t send_interval /**< Send interval in us */
)
{
return 0;
}
int ecrt_master_send(ec_master_t *master /**< EtherCAT master. */
)
{
return 0;
}
int ecrt_master_receive(ec_master_t *master /**< EtherCAT master. */
)
{
return 0;
}
int ecrt_master_state(
const ec_master_t *master, /**< EtherCAT master. */
ec_master_state_t *state /**< Structure to store the information. */
)
{
state->slaves_responding = master->getNoSlaves();
state->link_up = 1;
state->al_states = 8;
return 0;
}
int ecrt_master_link_state(
const ec_master_t *master, /**< EtherCAT master. */
unsigned int dev_idx, /**< Index of the device (0 = main device,
1 = first backup device, ...). */
ec_master_link_state_t *state /**< Structure to store the information.
*/
)
{
state->slaves_responding = master->getNoSlaves();
state->al_states = 4;
state->link_up = 1;
return 0;
}
int ecrt_master_application_time(
ec_master_t *master, /**< EtherCAT master. */
uint64_t app_time /**< Application time. */
)
{
return 0;
}
int ecrt_master_sync_reference_clock(
ec_master_t *master /**< EtherCAT master. */
)
{
return 0;
}
int ecrt_master_sync_reference_clock_to(
ec_master_t *master, /**< EtherCAT master. */
uint64_t sync_time /**< Sync reference clock to this time. */
)
{
return 0;
}
int ecrt_master_sync_slave_clocks(ec_master_t *master /**< EtherCAT master. */
)
{
return 0;
}
int ecrt_master_reference_clock_time(
const ec_master_t *master, /**< EtherCAT master. */
uint32_t *time /**< Pointer to store the queried system time. */
)
{
*time = 0;
return 0;
}
int ecrt_master_sync_monitor_queue(
ec_master_t *master /**< EtherCAT master. */
)
{
return 0;
}
uint32_t ecrt_master_sync_monitor_process(
const ec_master_t *master /**< EtherCAT master. */
)
{
return 32;
}
int ecrt_master_reset(ec_master_t *master /**< EtherCAT master. */
)
{
return 0;
}
static int mkpath(const std::string &file_path)
{
if (file_path.empty()) {
return 0;
}
std::size_t offset = 0;
do {
offset = file_path.find('/', offset + 1);
const auto subpath = file_path.substr(0, offset);
if (mkdir(subpath.c_str(), 0755) == -1) {
if (errno != EEXIST) {
return -1;
}
}
} while (offset != std::string::npos);
return 0;
}
static std::string getRtIpcDir(int idx)
{
std::string ans("/tmp/FakeEtherCAT");
if (const auto e = getenv("FAKE_EC_HOMEDIR")) {
ans = e;
}
ans += std::string("/") + std::to_string(idx);
mkpath(ans);
return ans;
}
static const char *getName()
{
static const char *default_name = "FakeEtherCAT";
if (const auto ans = getenv("FAKE_EC_NAME")) {
return ans;
}
std::cerr << "\nThe environment variable \"FAKE_EC_NAME\" is not set.\n"
<< "Using the default value \"" << default_name << "\".\n"
<< "Please consider to set unique names when using multiple"
<< " instances.\n\n";
return default_name;
}
ec_master::ec_master(int id):
rt_ipc_dir(getRtIpcDir(id)),
rt_ipc_name(getName()),
rt_ipc(rtipc_create(rt_ipc_name.c_str(), rt_ipc_dir.c_str())),
id_(id)
{}
/*****************************************************************************
* Slave Configuration
****************************************************************************/
int ecrt_slave_config_sync_manager(
ec_slave_config_t *sc, /**< Slave configuration. */
uint8_t sync_index, /**< Sync manager index. Must be less
than #EC_MAX_SYNC_MANAGERS. */
ec_direction_t direction, /**< Input/Output. */
ec_watchdog_mode_t watchdog_mode /** Watchdog mode. */
)
{
auto &syncManager = sc->sync_managers[sync_index];
syncManager.dir = direction;
return 0;
}
int ecrt_slave_config_watchdog(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t watchdog_divider, /**< Number of 40 ns intervals (register
0x0400). Used as a base unit for all
slave watchdogs^. If set to zero, the
value is not written, so the default is
used. */
uint16_t watchdog_intervals /**< Number of base intervals for sync
manager watchdog (register 0x0420). If
set to zero, the value is not written,
so the default is used. */
)
{
return 0;
}
int ecrt_slave_config_pdo_assign_add(
ec_slave_config_t *sc, /**< Slave configuration. */
uint8_t sync_index, /**< Sync manager index. Must be less
than #EC_MAX_SYNC_MANAGERS. */
uint16_t index /**< Index of the PDO to assign. */
)
{
auto &syncManager = sc->sync_managers[sync_index];
syncManager.pdos[index];
return 0;
}
int ecrt_slave_config_pdo_assign_clear(
ec_slave_config_t *sc, /**< Slave configuration. */
uint8_t sync_index /**< Sync manager index. Must be less
than #EC_MAX_SYNC_MANAGERS. */
)
{
auto &syncManager = sc->sync_managers[sync_index];
syncManager.pdos.clear();
return 0;
}
int ecrt_slave_config_pdo_mapping_add(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t pdo_index, /**< Index of the PDO. */
uint16_t entry_index, /**< Index of the PDO entry to add to the PDO's
mapping. */
uint8_t entry_subindex, /**< Subindex of the PDO entry to add to the
PDO's mapping. */
uint8_t entry_bit_length /**< Size of the PDO entry in bit. */
)
{
for (auto smIt = sc->sync_managers.begin();
smIt != sc->sync_managers.end();
++smIt) {
auto pdo_it = smIt->second.pdos.find(pdo_index);
if (pdo_it == smIt->second.pdos.end()) {
continue;
}
ec_pdo_entry_info_t entry_info;
entry_info.index = entry_index;
entry_info.subindex = entry_subindex;
entry_info.bit_length = entry_bit_length;
pdo_it->second.entries.push_back(entry_info);
return 0;
}
std::cerr << __func__ << "(): PDO " << std::hex << pdo_index
<< " not found." << std::endl;
return -1;
}
int ecrt_slave_config_pdo_mapping_clear(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t pdo_index /**< Index of the PDO. */
)
{
for (auto smIt = sc->sync_managers.begin();
smIt != sc->sync_managers.end();
++smIt) {
auto pdo_it = smIt->second.pdos.find(pdo_index);
if (pdo_it == smIt->second.pdos.end()) {
continue;
}
pdo_it->second.entries.clear();
return 0;
}
std::cerr << __func__ << "(): PDO " << std::hex << pdo_index
<< " not found." << std::endl;
return -1;
}
int ecrt_slave_config_pdos(
ec_slave_config_t *sc, /**< Slave configuration. */
unsigned int n_syncs, /**< Number of sync manager configurations in
\a syncs. */
const ec_sync_info_t syncs[] /**< Array of sync manager
configurations. */
)
{
if (!syncs) {
return 0;
}
for (unsigned int sync_idx = 0; sync_idx < n_syncs; ++sync_idx) {
if (syncs[sync_idx].index == 0xff) {
return 0;
}
auto &syncManager = sc->sync_managers[syncs[sync_idx].index];
syncManager.dir = syncs[sync_idx].dir;
for (unsigned int i = 0; i < syncs[sync_idx].n_pdos; ++i) {
const auto &in_pdo = syncs[sync_idx].pdos[i];
if (in_pdo.n_entries == 0 || !in_pdo.entries) {
std::cerr << "Default mapping not supported.";
return -1;
}
auto &out_pdo = syncManager.pdos[in_pdo.index];
for (unsigned int pdo_entry_idx = 0;
pdo_entry_idx < in_pdo.n_entries;
++pdo_entry_idx) {
out_pdo.entries.push_back(in_pdo.entries[pdo_entry_idx]);
}
}
}
return 0;
}
int ecrt_slave_config_reg_pdo_entry(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t entry_index, /**< Index of the PDO entry to register. */
uint8_t entry_subindex, /**< Subindex of the PDO entry to register. */
ec_domain_t *domain, /**< Domain. */
unsigned int *bit_position /**< Optional address if bit addressing
is desired */
)
{
for (auto sync_it : sc->sync_managers) {
for (auto pdo_it : sync_it.second.pdos) {
const auto offset =
pdo_it.second.findEntry(entry_index, entry_subindex);
if (offset != NotFound) {
const auto domain_offset =
domain->map(*sc, sync_it.first, pdo_it.first);
if (domain_offset != -1) {
if (bit_position) {
*bit_position = offset.bits;
}
else if (offset.bits) {
std::cerr << "Pdo Entry is not byte aligned"
<< " but bit offset is ignored!\n";
return -1;
}
return domain_offset + offset.bytes;
}
else {
return -1;
}
}
}
}
return -1; // offset
}
int ecrt_slave_config_reg_pdo_entry_pos(
ec_slave_config_t *sc, /**< Slave configuration. */
uint8_t sync_index, /**< Sync manager index. */
unsigned int pdo_pos, /**< Position of the PDO inside the SM. */
unsigned int entry_pos, /**< Position of the entry inside the PDO. */
ec_domain_t *domain, /**< Domain. */
unsigned int *bit_position /**< Optional address if bit addressing
is desired */
)
{
auto syncIt {sc->sync_managers.find(sync_index)};
if (syncIt == sc->sync_managers.end()) {
return -1;
}
auto pdo_it {syncIt->second.pdos.find(pdo_pos)};
if (pdo_it == syncIt->second.pdos.end()) {
return -1;
}
const auto offset = pdo_it->second.findEntryByPos(entry_pos);
if (offset != NotFound) {
return -1;
}
const auto domain_offset = domain->map(*sc, sync_index, pdo_it->first);
if (domain_offset == -1) {
return -1;
}
if (bit_position) {
*bit_position = offset.bits;
}
else if (offset.bits) {
std::cerr << "Pdo Entry is not byte aligned"
<< " but bit offset is ignored!\n";
return -1;
}
return domain_offset + offset.bytes;
}
int ecrt_slave_config_dc(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t assign_activate, /**< AssignActivate word. */
uint32_t sync0_cycle, /**< SYNC0 cycle time [ns]. */
int32_t sync0_shift, /**< SYNC0 shift time [ns]. */
uint32_t sync1_cycle, /**< SYNC1 cycle time [ns]. */
int32_t sync1_shift /**< SYNC1 shift time [ns]. */
)
{
return 0;
}
int ecrt_slave_config_sdo(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t index, /**< Index of the SDO to configure. */
uint8_t subindex, /**< Subindex of the SDO to configure. */
const uint8_t *data, /**< Pointer to the data. */
size_t size /**< Size of the \a data. */
)
{
sc->sdos[sdo_address {index, subindex}] =
std::basic_string<uint8_t>(data, data + size);
return 0;
}
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_complete_sdo(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t index, /**< Index of the SDO to configure. */
const uint8_t *data, /**< Pointer to the data. */
size_t size /**< Size of the \a data. */
)
{
return ecrt_slave_config_sdo(sc, index, 0, data, size);
}
int ecrt_slave_config_emerg_size(
ec_slave_config_t *sc, /**< Slave configuration. */
size_t elements /**< Number of records of the CoE emergency ring. */
)
{
return 0;
}
int ecrt_slave_config_emerg_pop(
ec_slave_config_t *sc, /**< Slave configuration. */
uint8_t *target /**< Pointer to target memory (at least
EC_COE_EMERGENCY_MSG_SIZE bytes). */
)
{
return -ENOENT;
}
int ecrt_slave_config_emerg_clear(
ec_slave_config_t *sc /**< Slave configuration. */
)
{
return 0;
}
int ecrt_slave_config_emerg_overruns(
const ec_slave_config_t *sc /**< Slave configuration. */
)
{
return 0;
}
ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
ec_slave_config_t *sc, /**< Slave configuration. */
uint16_t index, /**< SDO index. */
uint8_t subindex, /**< SDO subindex. */
size_t size /**< Data size to reserve. */
)
{
return nullptr;
}
ec_soe_request_t *ecrt_slave_config_create_soe_request(
ec_slave_config_t *sc, /**< Slave configuration. */
uint8_t drive_no, /**< Drive number. */
uint16_t idn, /**< Sercos ID-Number. */
size_t size /**< Data size to reserve. */
)
{
return nullptr;
}
ec_voe_handler_t *ecrt_slave_config_create_voe_handler(
ec_slave_config_t *sc, /**< Slave configuration. */
size_t size /**< Data size to reserve. */
)
{
return nullptr;
}
ec_reg_request_t *ecrt_slave_config_create_reg_request(
ec_slave_config_t *sc, /**< Slave configuration. */
size_t size /**< Data size to reserve. */
)
{
return nullptr;
}
int ecrt_slave_config_state(
const ec_slave_config_t *sc, /**< Slave configuration */
ec_slave_config_state_t *state /**< State object to write to. */
)
{
state->online = 1;
state->operational = 1;
state->al_state = EC_AL_STATE_OP;
return 0;
}
int ecrt_slave_config_idn(
ec_slave_config_t *sc, /**< Slave configuration. */
uint8_t drive_no, /**< Drive number. */
uint16_t idn, /**< SoE IDN. */
ec_al_state_t state, /**< AL state in which to write
the IDN (PREOP or SAFEOP). */
const uint8_t *data, /**< Pointer to the data. */
size_t size /**< Size of the \a data. */
)
{
return 0;
}
int ecrt_slave_config_flag(
ec_slave_config_t *sc, /**< Slave configuration. */
const char *key, /**< Key as null-terminated ASCII string. */
int32_t value /**< Value to store. */
)
{
return 0;
}
int ecrt_slave_config_eoe_mac_address(
ec_slave_config_t *sc, /**< Slave configuration. */
const unsigned char *mac_address /**< MAC address. */
)
{
return 0;
}
int ecrt_slave_config_eoe_ip_address(
ec_slave_config_t *sc, /**< Slave configuration. */
struct in_addr ip_address /**< IPv4 address. */
)
{
return 0;
}
int ecrt_slave_config_eoe_subnet_mask(
ec_slave_config_t *sc, /**< Slave configuration. */
struct in_addr subnet_mask /**< IPv4 subnet mask. */
)
{
return 0;
}
int ecrt_slave_config_eoe_default_gateway(
ec_slave_config_t *sc, /**< Slave configuration. */
struct in_addr gateway_address /**< Gateway's IPv4 address. */
)
{
return 0;
}
int ecrt_slave_config_eoe_dns_address(
ec_slave_config_t *sc, /**< Slave configuration. */
struct in_addr dns_address /**< IPv4 address of the DNS server. */
)
{
return 0;
}
int ecrt_slave_config_eoe_hostname(
ec_slave_config_t *sc, /**< Slave configuration. */
const char *name /**< Zero-terminated host name. */
)
{
return 0;
}
int ecrt_slave_config_state_timeout(
ec_slave_config_t *sc, /**< Slave configuration. */
ec_al_state_t from_state, /**< Initial state. */
ec_al_state_t to_state, /**< Target state. */
unsigned int timeout_ms /**< Timeout in [ms]. */
)
{
return 0;
}
/*****************************************************************************
* Domain
****************************************************************************/
int ecrt_domain_reg_pdo_entry_list(
ec_domain_t *domain, /**< Domain. */
const ec_pdo_entry_reg_t *pdo_entry_regs /**< Array of PDO
registrations. */
)
{
const ec_pdo_entry_reg_t *reg;
ec_slave_config_t *sc;
int ret;
for (reg = pdo_entry_regs; reg->index; reg++) {
if (!(sc = ecrt_master_slave_config(
domain->getMaster(),
reg->alias,
reg->position,
reg->vendor_id,
reg->product_code))) {
return -ENOENT;
}
if ((ret = ecrt_slave_config_reg_pdo_entry(
sc,
reg->index,
reg->subindex,
domain,
reg->bit_position))
< 0) {
return ret;
}
*reg->offset = ret;
}
return 0;
}
size_t ecrt_domain_size(const ec_domain_t *domain /**< Domain. */
)
{
return domain->getSize();
}
uint8_t *ecrt_domain_data(const ec_domain_t *domain)
{
return domain->getData();
}
int ecrt_domain_process(ec_domain_t *domain)
{
return domain->process();
}
int ecrt_domain_queue(ec_domain_t *domain)
{
return domain->queue();
}
int ecrt_domain_state(
const ec_domain_t *domain, /**< Domain. */
ec_domain_state_t *state /**< Pointer to a state object to
store the information. */
)
{
state->working_counter = domain->getNumSlaves();
state->redundancy_active = 0;
state->wc_state = EC_WC_COMPLETE;
return 0;
}
ec_domain::ec_domain(rtipc *rtipc, const char *prefix, ec_master_t *master):
rt_group(rtipc_create_group(rtipc, 1.0)),
prefix(prefix),
master(master)
{}
int ec_domain::activate()
{
std::unordered_set<uint32_t> slaves;
connected.resize(mapped_pdos.size());
size_t idx = 0;
for (const auto &pdo : mapped_pdos) {
slaves.insert(pdo.slave_address.getCombined());
void *rt_pdo = nullptr;
char buf[512];
const auto fmt = snprintf(
buf,
sizeof(buf),
"%s/%d/%08X/%04X",
prefix,
master->getId(),
pdo.slave_address.getCombined(),
pdo.pdo_index);
if (fmt < 0 || fmt >= (int) sizeof(buf)) {
return -ENOBUFS;
}
switch (pdo.dir) {
case EC_DIR_OUTPUT:
rt_pdo = rtipc_txpdo(
rt_group,
buf,
rtipc_uint8_T,
data.data() + pdo.offset,
pdo.size_bytes);
std::cerr << "Registering " << buf << " as Output\n";
break;
case EC_DIR_INPUT:
rt_pdo = rtipc_rxpdo(
rt_group,
buf,
rtipc_uint8_T,
data.data() + pdo.offset,
pdo.size_bytes,
connected.data() + idx);
std::cerr << "Registering " << buf << " as Input\n";
break;
default:
std::cerr << "Unknown direction " << pdo.dir << '\n';
return -1;
}
if (!rt_pdo) {
std::cerr << "Failed to register RtIPC PDO\n";
return -1;
}
++idx;
}
activated_ = true;
numSlaves = slaves.size();
return 0;
}
int ec_domain::process()
{
rtipc_rx(rt_group);
return 0;
}
int ec_domain::queue()
{
rtipc_tx(rt_group);
return 0;
}
ssize_t ec_domain::map(
ec_slave_config const &config,
unsigned int syncManager,
uint16_t pdo_index)
{
if (activated_) {
return -1;
}
for (const auto &pdo : mapped_pdos) {
if (pdo.slave_address == config.address
&& syncManager == pdo.syncManager && pdo_index == pdo.pdo_index) {
// already mapped;
return pdo.offset;
}
}
const auto ans = data.size();
const auto size = config.sync_managers.at(syncManager)
.pdos.at(pdo_index)
.sizeInBytes();
mapped_pdos.emplace_back(
ans,
size,
config.address,
syncManager,
pdo_index,
config.sync_managers.at(syncManager).dir);
data.resize(ans + size);
return ans;
}
/*****************************************************************************
* SDO requests
****************************************************************************/
int ecrt_sdo_request_index(
ec_sdo_request_t *req, /**< SDO request. */
uint16_t index, /**< SDO index. */
uint8_t subindex /**< SDO subindex. */
)
{
return 0;
}
int ecrt_sdo_request_timeout(
ec_sdo_request_t *req, /**< SDO request. */
uint32_t timeout /**< Timeout in milliseconds. Zero
means no timeout. */
)
{
return 0;
}
uint8_t *
ecrt_sdo_request_data(const ec_sdo_request_t *req /**< SDO request. */
)
{
return nullptr;
}
size_t
ecrt_sdo_request_data_size(const ec_sdo_request_t *req /**< SDO request. */
)
{
return 0;
}
ec_request_state_t
ecrt_sdo_request_state(ec_sdo_request_t *req /**< SDO request. */
)
{
return EC_REQUEST_ERROR;
}
int ecrt_sdo_request_write(ec_sdo_request_t *req /**< SDO request. */
)
{
return 0;
}
int ecrt_sdo_request_read(ec_sdo_request_t *req /**< SDO request. */
)
{
return 0;
}
/*****************************************************************************
* SoE request
****************************************************************************/
int ecrt_soe_request_idn(
ec_soe_request_t *req, /**< IDN request. */
uint8_t drive_no, /**< SDO index. */
uint16_t idn /**< SoE IDN. */
)
{
return 0;
}
int ecrt_soe_request_timeout(
ec_soe_request_t *req, /**< SoE request. */
uint32_t timeout /**< Timeout in milliseconds. Zero
means no timeout. */
)
{
return 0;
}
uint8_t *
ecrt_soe_request_data(const ec_soe_request_t *req /**< SoE request. */
)
{
return nullptr;
}
size_t
ecrt_soe_request_data_size(const ec_soe_request_t *req /**< SoE request. */
)
{
return 0;
}
ec_request_state_t
ecrt_soe_request_state(ec_soe_request_t *req /**< SoE request. */
)
{
return EC_REQUEST_ERROR;
}
int ecrt_soe_request_write(ec_soe_request_t *req /**< SoE request. */
)
{
return 0;
}
int ecrt_soe_request_read(ec_soe_request_t *req /**< SoE request. */
)
{
return 0;
}
/*****************************************************************************
* VoE handler
****************************************************************************/
int ecrt_voe_handler_send_header(
ec_voe_handler_t *voe, /**< VoE handler. */
uint32_t vendor_id, /**< Vendor ID. */
uint16_t vendor_type /**< Vendor-specific type. */
)
{
return 0;
}
int ecrt_voe_handler_received_header(
const ec_voe_handler_t *voe, /**< VoE handler. */
uint32_t *vendor_id, /**< Vendor ID. */
uint16_t *vendor_type /**< Vendor-specific type. */
)
{
*vendor_id = 0;
*vendor_type = 0;
return 0;
}
uint8_t *
ecrt_voe_handler_data(const ec_voe_handler_t *voe /**< VoE handler. */
)
{
return nullptr;
}
size_t
ecrt_voe_handler_data_size(const ec_voe_handler_t *voe /**< VoE handler. */
)
{
return 0;
}
int ecrt_voe_handler_write(
ec_voe_handler_t *voe, /**< VoE handler. */
size_t size /**< Number of bytes to write (without the VoE header). */
)
{
return 0;
}
int ecrt_voe_handler_read(ec_voe_handler_t *voe /**< VoE handler. */
)
{
return 0;
}
int ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe /**< VoE handler. */
)
{
return 0;
}
ec_request_state_t
ecrt_voe_handler_execute(ec_voe_handler_t *voe /**< VoE handler. */
)
{
return EC_REQUEST_ERROR;
}
/*****************************************************************************
* Register request
****************************************************************************/
uint8_t *
ecrt_reg_request_data(const ec_reg_request_t *req /**< Register request. */
)
{
return nullptr;
}
ec_request_state_t
ecrt_reg_request_state(const ec_reg_request_t *req /**< Register request. */
)
{
return EC_REQUEST_ERROR;
}
int ecrt_reg_request_write(
ec_reg_request_t *req, /**< Register request. */
uint16_t address, /**< Register address. */
size_t size /**< Size to write. */
)
{
return 0;
}
int ecrt_reg_request_read(
ec_reg_request_t *req, /**< Register request. */
uint16_t address, /**< Register address. */
size_t size /**< Size to write. */
)
{
return 0;
}
/*****************************************************************************
* Floating-point read functions
****************************************************************************/
void ecrt_write_lreal(void *data, double const value)
{
memcpy(data, &value, sizeof(value));
}
void ecrt_write_real(void *data, float const value)
{
memcpy(data, &value, sizeof(value));
}
float ecrt_read_real(const void *data)
{
float ans;
memcpy(&ans, data, sizeof(ans));
return ans;
}
double ecrt_read_lreal(const void *data)
{
double ans;
memcpy(&ans, data, sizeof(ans));
return ans;
}
/****************************************************************************/
void ec_slave_config::dumpJson(std::ostream &out, int indent) const
{
out << "{\n";
indent += 4;
add_spaces(out, indent);
out << "\"vendor_id\": " << std::dec << vendor_id << ",\n";
add_spaces(out, indent);
out << "\"product_id\": " << product_code << ",\n";
add_spaces(out, indent);
out << "\"sdos\": ";
map2Json(
out,
sdos,
[](std::ostream &out, const std::basic_string<uint8_t> &value) {
out << "\"0x";
for (const auto s : value) {
out << std::hex << std::setfill('0') << std::setw(2)
<< (unsigned) s;
}
out << '"';
},
indent);
out << '\n';
add_spaces(out, indent - 4);
out << "}";
}
/****************************************************************************/
Reference in New Issue View Git Blame Copy Permalink