WIP add rtipc
This commit is contained in:
Bjarne von Horn
parent
19afa78793
commit
4c953d5000
|
|
@ -48,13 +48,64 @@ Offset pdo::findEntry(uint16_t idx, uint8_t subindex) const
|
|||
return NotFound;
|
||||
}
|
||||
|
||||
void ec_domain::activate()
|
||||
ec_domain::ec_domain(rtipc *rtipc, const char *prefix) : rt_group(rtipc_create_group(rtipc, 1.0)), prefix(prefix)
|
||||
{
|
||||
}
|
||||
|
||||
int ec_domain::activate(int domain_id)
|
||||
{
|
||||
connected.resize(mapped_pdos.size());
|
||||
size_t idx = 0;
|
||||
for (const auto &pdo : mapped_pdos)
|
||||
{
|
||||
void *rt_pdo = nullptr;
|
||||
char buf[512];
|
||||
const auto fmt = snprintf(buf, sizeof(buf), "%s/%d/%08X/%04X", prefix, domain_id, 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);
|
||||
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);
|
||||
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;
|
||||
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)
|
||||
|
|
@ -64,31 +115,27 @@ ssize_t ec_domain::map(ec_slave_config const &config, unsigned int syncManager,
|
|||
}
|
||||
}
|
||||
const auto ans = data.size();
|
||||
mapped_pdos.emplace_back(ans, config.address, syncManager, pdo_index);
|
||||
data.resize(ans + config.sync_managers.at(syncManager).pdos.at(pdo_index).sizeInBytes());
|
||||
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;
|
||||
}
|
||||
|
||||
uint8_t *ecrt_domain_data(
|
||||
const ec_domain_t *domain)
|
||||
{
|
||||
return const_cast<uint8_t *>(domain->data.data());
|
||||
return domain->getData();
|
||||
}
|
||||
|
||||
int ecrt_domain_process(
|
||||
ec_domain_t *domain)
|
||||
{
|
||||
if (domain->data.empty())
|
||||
return 0;
|
||||
std::cout << ' ';
|
||||
for (const auto i : domain->data)
|
||||
std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)i;
|
||||
return 0;
|
||||
return domain->process();
|
||||
}
|
||||
int ecrt_domain_queue(
|
||||
ec_domain_t *domain)
|
||||
{
|
||||
return 0;
|
||||
return domain->queue();
|
||||
}
|
||||
|
||||
int ecrt_domain_state(
|
||||
|
|
@ -100,13 +147,23 @@ int ecrt_domain_state(
|
|||
return 0;
|
||||
}
|
||||
|
||||
int ec_master::activate()
|
||||
{
|
||||
int i = 0;
|
||||
for (auto &domain : domains)
|
||||
{
|
||||
if (domain.activate(i))
|
||||
return -1;
|
||||
++i;
|
||||
}
|
||||
return rtipc_prepare(rt_ipc.get());
|
||||
}
|
||||
|
||||
int ecrt_master_activate(
|
||||
ec_master_t *master /**< EtherCAT master. */
|
||||
)
|
||||
{
|
||||
for (auto &domain : master->domains)
|
||||
domain.activate();
|
||||
return 0;
|
||||
return master->activate();
|
||||
}
|
||||
|
||||
int ecrt_master_application_time(
|
||||
|
|
@ -121,8 +178,13 @@ ec_domain_t *ecrt_master_create_domain(
|
|||
ec_master_t *master /**< EtherCAT master. */
|
||||
)
|
||||
{
|
||||
master->domains.emplace_back();
|
||||
return &master->domains.back();
|
||||
return master->createDomain();
|
||||
}
|
||||
|
||||
ec_domain *ec_master::createDomain()
|
||||
{
|
||||
domains.emplace_back(rt_ipc.get(), "/FakeTaxi");
|
||||
return &domains.back();
|
||||
}
|
||||
|
||||
int ecrt_master_receive(
|
||||
|
|
@ -148,10 +210,20 @@ ec_slave_config_t *ecrt_master_slave_config(
|
|||
uint32_t vendor_id, /**< Expected vendor ID. */
|
||||
uint32_t product_code /**< Expected product code. */
|
||||
)
|
||||
{
|
||||
return master->slave_config(alias, position, vendor_id, product_code);
|
||||
}
|
||||
|
||||
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 = master->slaves.find(address);
|
||||
if (it != master->slaves.end())
|
||||
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;
|
||||
|
|
@ -163,7 +235,7 @@ ec_slave_config_t *ecrt_master_slave_config(
|
|||
}
|
||||
else
|
||||
{
|
||||
return &master->slaves.insert(std::make_pair<ec_address, ec_slave_config>(ec_address{address}, ec_slave_config{address, vendor_id, product_code})).first->second;
|
||||
return &slaves.insert(std::make_pair<ec_address, ec_slave_config>(ec_address{address}, ec_slave_config{address, vendor_id, product_code})).first->second;
|
||||
}
|
||||
}
|
||||
int ecrt_master_state(
|
||||
|
|
@ -171,7 +243,7 @@ int ecrt_master_state(
|
|||
ec_master_state_t *state /**< Structure to store the information. */
|
||||
)
|
||||
{
|
||||
state->slaves_responding = master->slaves.size();
|
||||
state->slaves_responding = master->getNoSlaves();
|
||||
state->link_up = 1;
|
||||
state->al_states = 8;
|
||||
return 0;
|
||||
|
|
@ -188,6 +260,11 @@ int ecrt_master_sync_slave_clocks(
|
|||
{
|
||||
return 0;
|
||||
}
|
||||
void ecrt_release_master(ec_master_t *master)
|
||||
{
|
||||
delete master;
|
||||
}
|
||||
|
||||
ec_master_t *ecrt_request_master(
|
||||
unsigned int master_index /**< Index of the master to request. */
|
||||
)
|
||||
|
|
@ -195,6 +272,10 @@ ec_master_t *ecrt_request_master(
|
|||
return new ec_master();
|
||||
}
|
||||
|
||||
ec_master::ec_master() : rt_ipc(rtipc_create("FakeTaxi", "/tmp/FakeTaxi"))
|
||||
{
|
||||
}
|
||||
|
||||
int ecrt_slave_config_complete_sdo(
|
||||
ec_slave_config_t *sc, /**< Slave configuration. */
|
||||
uint16_t index, /**< Index of the SDO to configure. */
|
||||
|
|
@ -202,6 +283,7 @@ int ecrt_slave_config_complete_sdo(
|
|||
size_t size /**< Size of the \a data. */
|
||||
)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
|
||||
ec_slave_config_t *sc, /**< Slave configuration. */
|
||||
|
|
@ -210,6 +292,7 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(
|
|||
size_t size /**< Data size to reserve. */
|
||||
)
|
||||
{
|
||||
return nullptr;
|
||||
}
|
||||
int ecrt_slave_config_dc(
|
||||
ec_slave_config_t *sc, /**< Slave configuration. */
|
||||
|
|
@ -289,6 +372,11 @@ int ecrt_slave_config_reg_pdo_entry(
|
|||
{
|
||||
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
|
||||
|
|
@ -309,6 +397,7 @@ int ecrt_slave_config_sdo(
|
|||
size_t size /**< Size of the \a data. */
|
||||
)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
void ecrt_write_lreal(void *data, double const value)
|
||||
|
|
|
|||
|
|
@ -23,11 +23,13 @@
|
|||
#pragma once
|
||||
|
||||
#include <ecrt.h>
|
||||
#include <rtipc.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <iomanip>
|
||||
#include <list>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
struct Offset
|
||||
|
|
@ -74,6 +76,7 @@ public:
|
|||
|
||||
uint16_t getAlias() const { return value >> 16; }
|
||||
uint16_t getPosition() const { return value & 0xFFFF; }
|
||||
uint32_t getCombined() const { return value; }
|
||||
|
||||
bool operator<(const ec_address &other) const noexcept
|
||||
{
|
||||
|
|
@ -105,27 +108,48 @@ struct ec_slave_config
|
|||
struct ec_domain
|
||||
{
|
||||
|
||||
private:
|
||||
struct PdoMap
|
||||
{
|
||||
size_t offset;
|
||||
size_t size_bytes;
|
||||
ec_address slave_address;
|
||||
unsigned int syncManager;
|
||||
uint16_t pdo_index;
|
||||
ec_direction_t dir;
|
||||
|
||||
PdoMap(
|
||||
size_t offset,
|
||||
size_t size_bytes,
|
||||
ec_address slave_address,
|
||||
unsigned int syncManager,
|
||||
uint16_t pdo_index)
|
||||
: offset(offset), slave_address(slave_address), syncManager(syncManager), pdo_index(pdo_index)
|
||||
uint16_t pdo_index,
|
||||
ec_direction_t dir)
|
||||
: offset(offset), size_bytes(size_bytes), slave_address(slave_address), syncManager(syncManager), pdo_index(pdo_index), dir(dir)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
std::vector<uint8_t> data;
|
||||
std::vector<unsigned char> connected;
|
||||
std::vector<PdoMap> mapped_pdos;
|
||||
rtipc_group *rt_group;
|
||||
const char *prefix;
|
||||
bool activated_ = false;
|
||||
|
||||
void activate();
|
||||
public:
|
||||
explicit ec_domain(struct rtipc *rtipc, const char *prefix);
|
||||
|
||||
uint8_t *getData() const
|
||||
{
|
||||
if (!activated_)
|
||||
return nullptr;
|
||||
return const_cast<uint8_t *>(data.data());
|
||||
}
|
||||
|
||||
int activate(int domain_id);
|
||||
int process();
|
||||
int queue();
|
||||
|
||||
ssize_t map(ec_slave_config const &config, unsigned int syncManager,
|
||||
uint16_t pdo_index);
|
||||
|
|
@ -133,6 +157,31 @@ struct ec_domain
|
|||
|
||||
struct ec_master
|
||||
{
|
||||
private:
|
||||
struct RtIpcDeleter
|
||||
{
|
||||
void operator()(struct rtipc *r) const
|
||||
{
|
||||
rtipc_exit(r);
|
||||
}
|
||||
};
|
||||
|
||||
std::list<ec_domain> domains;
|
||||
std::map<ec_address, ec_slave_config> slaves;
|
||||
std::unique_ptr<struct rtipc, RtIpcDeleter> rt_ipc;
|
||||
|
||||
public:
|
||||
ec_master();
|
||||
|
||||
int activate();
|
||||
ec_domain *createDomain();
|
||||
|
||||
int getNoSlaves() const { return slaves.size(); }
|
||||
|
||||
ec_slave_config_t *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. */
|
||||
);
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue