Mirrors/VILLASnode
1
0
Fork 0
mirror of https://git.rwth-aachen.de/acs/public/villas/node/ synced 2025-03-09 00:00:00 +01:00
Code Issues Releases Wiki Activity

ethercat: rewrite into multithreaded

Browse source
This commit is contained in:
Steffen Vogel 2020-01-25 17:38:22 +01:00
parent b0d862a84d
commit c9dfc547f4
5 changed files with 526 additions and 380 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

56
include/villas/nodes/ethercat.h → include/villas/nodes/ethercat.hpp
View file

@ -3,7 +3,8 @@
* @file
* @author Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
* @copyright 2018, Institute for Automation of Complex Power Systems, EONERC
* @author Divya Laxetti <divya.laxetti@rwth-aachen.de>
* @copyright 2018-2020, Institute for Automation of Complex Power Systems, EONERC
* @license GNU General Public License (version 3)
*
* VILLASnode
@ -30,15 +31,18 @@
#pragma once
#include <thread>
#include <villas/node.h>
#include <villas/pool.h>
#include <villas/task.h>
#include <villas/queue_signalled.h>
#include <villas/common.h>
#include <villas/io.h>
#include <villas/config.h>
// Include hard-coded Ethercat Bus configuration
#include <villas/nodes/ethercat_config.h>
#include <villas/nodes/ethercat_config.hpp>
#ifdef __cplusplus
extern "C" {
@ -51,28 +55,30 @@ extern "C" {
/** Internal data per ethercat node */
struct ethercat {
/* Settings */
double rate;
struct {
int num_channels;
unsigned num_channels;
double range;
unsigned position;
unsigned product_code; /**< Product ID of EtherCAT slave */
unsigned vendor_id; /**< Vendor ID of EtherCAT slave */
ec_slave_config_t *sc;
unsigned *offsets; /**< Offsets for PDO entries */
} in, out;
ec_domain_t *domain;
ec_pdo_entry_reg_t *domain_regs;
uint8_t *domain_pd; /**< Process data */
std::thread thread; /**< Cyclic task thread */
struct task task; /**< Periodic timer */
struct pool pool;
struct queue_signalled queue; /**< For samples which are received from WebSockets */
ec_domain_t *domain;
ec_slave_config_t *sc_in;
ec_slave_config_t *sc_out;
uint8_t *domain_pd;
// Offsets for PDO entries
unsigned int off_out_values[ETHERCAT_NUM_CHANNELS];
unsigned int off_in_values[ETHERCAT_NUM_CHANNELS];
ec_pdo_entry_reg_t domain_regs[2 * ETHERCAT_NUM_CHANNELS + 1];
std::atomic<struct sample *> send; /**< Last sample to be sent via EtherCAT */
};
/* Internal datastructures */
@ -83,6 +89,21 @@ int ethercat_type_start(struct super_node *sn);
/** @see node_type::type_stop */
int ethercat_type_stop();
/** @see node_type::init */
int ethercat_init(struct node *n);
/** @see node_type::destroy */
int ethercat_destroy(struct node *n);
/** @see node_type::parse */
int ethercat_parse(struct node *n, json_t *cfg);
/** @see node_type::check */
int ethercat_check(struct node *n);
/** @see node_type::prepare */
int ethercat_prepare(struct node *n);
/** @see node_type::open */
int ethercat_start(struct node *n);
@ -95,9 +116,6 @@ int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned
/** @see node_type::write */
int ethercat_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release);
/** @see node_type::parse */
int ethercat_parse(struct node *n, json_t *cfg);
/** @} */

14
include/villas/nodes/ethercat_config.h → include/villas/nodes/ethercat_config.hpp
View file

@ -3,7 +3,8 @@
* @file
* @author Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
* @copyright 2018, Institute for Automation of Complex Power Systems, EONERC
* @author Divya Laxetti <divya.laxetti@rwth-aachen.de>
* @copyright 2018-2020, Institute for Automation of Complex Power Systems, EONERC
* @license GNU General Public License (version 3)
*
* VILLASnode
@ -24,13 +25,7 @@
#pragma once
#define ETHERCAT_NUM_CHANNELS 8
#define ETHERCAT_VOLTAGE_RANGE 10.0
#define ETHERCAT_ALIAS 0
#define ETHERCAT_POS_COUPLER 0
#define ETHERCAT_POS_SLAVE_OUT 1
#define ETHERCAT_POS_SLAVE_IN 2
#include <ecrt.h>
#define ETHERCAT_VID_BECKHOFF 0x00000002
@ -46,9 +41,8 @@
#define ETHERCAT_PID_EL3008 0x0bc03052
#define ETHERCAT_PID_FC1100 0x044c0c62
#include <ecrt.h>
/*****************************************************************************/
/** @todo: Make PDO entry tables configurable */
/* Master 0, Slave 3, "EL4038"
* Vendor ID: 0x00000002

11
lib/nodes/CMakeLists.txt
View file

@ -20,16 +20,7 @@
# along with this program. If not, see <http://www.gnu.org/licenses/>.
###################################################################################
<<<<<<< HEAD
set(NODE_SRC)
=======
set(NODE_SRC
influxdb.c
stats.c
signal_generator.c
loopback.c
)
>>>>>>> ethercat: add ethercat plugin to cmake. removed some whitespaces.
if(LIBNL3_ROUTE_FOUND)
list(APPEND LIBRARIES PkgConfig::LIBNL3_ROUTE)
@ -172,7 +163,7 @@ endif()
# Enable Ethercat support
if(ETHERLAB_FOUND AND WITH_IO)
list(APPEND NODE_SRC ethercat.c)
list(APPEND NODE_SRC ethercat.cpp)
list(APPEND INCLUDE_DIRS ${ETHERLAB_INCLUDE_DIRS})
list(APPEND LIBRARIES ${ETHERLAB_LIBRARIES})
endif()

341
lib/nodes/ethercat.c
View file

@ -1,341 +0,0 @@
/** Node type: Ethercat
*
* @author Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
* @copyright 2018, Institute for Automation of Complex Power Systems, EONERC
* @license GNU General Public License (version 3)
*
* VILLASnode
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* This program 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*********************************************************************************/
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <villas/super_node.h>
#include <villas/timing.h>
#include <villas/utils.h>
#include <villas/buffer.h>
#include <villas/plugin.h>
#include <villas/log.h>
#include <villas/format_type.h>
#include <villas/formats/msg_format.h>
#include <villas/nodes/ethercat.h>
/** TODOs
*
* - Implement ethercat_parse()
*
"{ s: i, s: { s: i }, s: { } }",
"alias", &w->alias,
"in",
"num_channels", &w->in.num_channels,
"pid", ...
"vid", ...
"out",
- alias
- coupler_pos
- in.pos
- in.num_channels
- in.voltage_range
- in.vid
- in.pid
- out...
- coupler_vid
- coupler_pid
* - Implement ethercat_print()
* - Implement ethercat_destroy()
* - Replace printf() by calls to info(), warning(), error()
*/
/****************************************************************************/
/** Task period in ns. */
#define PERIOD_NS (1000000)
/****************************************************************************/
/* Constants */
#define NSEC_PER_SEC (1000000000)
#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS)
/****************************************************************************/
/* Forward declarations */
static struct plugin p;
static ec_master_t *master = NULL;
int ethercat_type_start(struct super_node *sn)
{
ec_slave_config_t *sc;
struct timespec_wakeup_time;
master = ecrt_request_master(0);
if (!master) {
return -1;
}
// Create configuration for bus coupler
sc = ecrt_master_slave_config(master, ETHERCAT_ALIAS, ETHERCAT_POS_COUPLER, ETHERCAT_VID_BECKHOFF, ETHERCAT_PID_EK1100);
if (!sc) {
return -1;
}
return 0;
}
int ethercat_start(struct node *n)
{
struct ethercat *w = (struct ethercat *) n->_vd;
// int ret = pool_init(&w->pool, DEFAULT_ETHERCAT_QUEUE_LENGTH, SAMPLE_LENGTH(DEFAULT_ETHERCAT_SAMPLE_LENGTH), &memory_hugepage);
// if (ret)
// return ret;
// ret = queue_signalled_init(&w->queue, DEFAULT_ETHERCAT_QUEUE_LENGTH, &memory_hugepage, 0);
// if (ret)
// return ret;
w->domain = ecrt_master_create_domain(master);
if (!w->domain) {
return -1;
}
// Prepare list of domain registers
for (int i = 0; i < ETHERCAT_NUM_CHANNELS; i++)
{
w->off_out_values[i] = 0;
w->off_in_values[i] = 0;
w->domain_regs[i].alias = ETHERCAT_ALIAS;
w->domain_regs[i].position = ETHERCAT_POS_SLAVE_OUT;
w->domain_regs[i].vendor_id = ETHERCAT_VID_BECKHOFF;
w->domain_regs[i].product_code = ETHERCAT_PID_EL4038;
w->domain_regs[i].index = 0x7000 + i * 0x10;
w->domain_regs[i].subindex = 0x1;
w->domain_regs[i].offset = w->off_out_values + i;
w->domain_regs[i+ETHERCAT_NUM_CHANNELS].alias = ETHERCAT_ALIAS;
w->domain_regs[i+ETHERCAT_NUM_CHANNELS].position = ETHERCAT_POS_SLAVE_IN;
w->domain_regs[i+ETHERCAT_NUM_CHANNELS].vendor_id = ETHERCAT_VID_BECKHOFF;
w->domain_regs[i+ETHERCAT_NUM_CHANNELS].product_code = ETHERCAT_PID_EL3008;
w->domain_regs[i+ETHERCAT_NUM_CHANNELS].index = 0x6000 + i * 0x10;
w->domain_regs[i+ETHERCAT_NUM_CHANNELS].subindex = 0x11;
w->domain_regs[i+ETHERCAT_NUM_CHANNELS].offset = w->off_in_values + i;
};
// End of list delimiter
memset(&w->domain_regs[2*ETHERCAT_NUM_CHANNELS], 0, sizeof(ec_pdo_entry_reg_t));
// Configure analog in
info("Configuring PDOs...\n");
if (!(w->sc_in = ecrt_master_slave_config(master, ETHERCAT_ALIAS, ETHERCAT_POS_SLAVE_IN, ETHERCAT_VID_BECKHOFF, ETHERCAT_PID_EL3008))) {
warning("Failed to get slave configuration.\n");
return -1;
}
if (ecrt_slave_config_pdos(w->sc_in, EC_END, slave_4_syncs)) {
error("Failed to configure PDOs.\n");
return -1;
}
// Configure analog out
if (!(w->sc_out = ecrt_master_slave_config(master, ETHERCAT_ALIAS, ETHERCAT_POS_SLAVE_OUT, ETHERCAT_VID_BECKHOFF, ETHERCAT_PID_EL4038))) {
warning("Failed to get slave configuration.\n");
return -1;
}
if (ecrt_slave_config_pdos(w->sc_out, EC_END, slave_3_syncs)) {
warning("Failed to configure PDOs.\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(w->domain, w->domain_regs)) {
error("PDO entry registration failed!\n");
return -1;
}
info("Activating master...\n");
if (ecrt_master_activate(master)) {
return -1;
}
if (!(w->domain_pd = ecrt_domain_data(w->domain))) {
return -1;
}
return 0;
}
int ethercat_stop(struct node *n)
{
int ret;
struct ethercat *w = (struct ethercat *) n->_vd;
info("releasing master\n");
ecrt_release_master(master);
info("have a nice day!\n");
ret = queue_signalled_destroy(&w->queue);
if (ret)
return ret;
ret = pool_destroy(&w->pool);
if (ret)
return ret;
return 0;
}
int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
struct ethercat *w = (struct ethercat *) n->_vd;
// int avail;
// struct sample *cpys[cnt];
// avail = queue_signalled_pull_many(&w->queue, (void **) cpys, cnt);
// if (avail < 0)
// return avail;
// Receive process data
ecrt_master_receive(master);
ecrt_domain_process(w->domain);
if (cnt < 1)
return 0;
// Read process data
for (int i = 0; i < MIN(ETHERCAT_NUM_CHANNELS, smps[0]->capacity); ++i) {
int16_t ain_value = EC_READ_S16(w->domain_pd + w->off_in_values[i]);
smps[0]->data[i].f = ETHERCAT_VOLTAGE_RANGE * (float) ain_value / INT16_MAX;
}
// sample_copy_many(smps, cpys, avail);
// sample_decref_many(cpys, avail);
return 1;
}
int ethercat_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
struct ethercat *w = (struct ethercat *) n->_vd;
/* Make copies of all samples */
// struct sample *cpys[cnt];
// int avail = sample_alloc_many(&w->pool, cpys, cnt);
// if (avail < cnt)
// warning("Pool underrun for node %s: avail=%u", node_name(n), avail);
// sample_copy_many(cpys, smps, avail);
if (cnt < 1)
return 0;
// Write process data
for (int i = 0; i < ETHERCAT_NUM_CHANNELS; ++i) {
float aout_voltage = smps[0]->data[i].f;
int16_t aout_value = aout_voltage / ETHERCAT_VOLTAGE_RANGE * INT16_MAX;
EC_WRITE_U16(w->domain_pd + w->off_out_values[i], aout_value);
}
// send process data
ecrt_domain_queue(w->domain);
ecrt_master_send(master);
//sample_decref_many(cpys, avail);
return 1;
}
int ethercat_parse(struct node *n, json_t *cfg)
{
struct ethercat *w = (struct ethercat *) n->_vd;
//const char *local, *remote;
//const char *layer = NULL;
const char *format = "villas.binary";
int ret;
json_error_t err;
ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: { s: i, s?: i, s?: i }, s?: { s: i, s?: i, s?: i } }",
"alias", &alias,
"out",
"num_channels", &w->in.num_channels,
"pid", &w->out.pid,
"vid", &w->out.vid,
"in",
"num_channels", &w->in.num_channels,
"pid", &w->in.pid,
"vid", &w->in.vid,
);
if (ret)
jerror(&err, "Failed to parse configuration of node %s", node_name(n));
/* Format */
w->format = format_type_lookup(format);
if (!w->format)
error("Invalid format '%s' for node %s", format, node_name(n));
return 0;
}
// int ethercat_poll_fds(struct node *n, int *fds)
// {
// struct ethercat *w = (struct ethercat *) n->_vd;
// fds[0] = queue_signalled_fd(&w->queue);
// return 1;
// }
static struct plugin p = {
.name = "ethercat",
.description = "Send and receive samples of an ethercat connection",
.type = PLUGIN_TYPE_NODE,
.node = {
.vectorize = 1, /* we only process a single sample per call */
.size = sizeof(struct ethercat),
.type.start = ethercat_type_start,
.start = ethercat_start,
.parse = ethercat_parse,
.stop = ethercat_stop,
.read = ethercat_read,
.write = ethercat_write,
// .poll_fds = ethercat_poll_fds
}
};
REGISTER_PLUGIN(&p)
LIST_INIT_STATIC(&p.node.instances)

484
lib/nodes/ethercat.cpp Normal file
View file

@ -0,0 +1,484 @@
/** Node type: Ethercat
*
* @author Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
* @author Divya Laxetti <divya.laxetti@rwth-aachen.de>
* @copyright 2018-2020, Institute for Automation of Complex Power Systems, EONERC
* @license GNU General Public License (version 3)
*
* VILLASnode
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* any later version.
*
* This program 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*********************************************************************************/
#include <sstream>
#include <villas/super_node.hpp>
#include <villas/utils.hpp>
#include <villas/plugin.h>
#include <villas/log.h>
#include <villas/exceptions.hpp>
#include <villas/nodes/ethercat.hpp>
using namespace villas;
/* Forward declartions */
static struct plugin p;
/* Constants */
#define NSEC_PER_SEC (1000000000)
#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS)
/* Global state and config */
int master_id = 0;
int alias = 0;
static ec_master_t *master = nullptr;
struct coupler {
int position;
int vendor_id;
int product_code;
ec_slave_config_t *sc;
} coupler = {
.position = 0,
.vendor_id = ETHERCAT_VID_BECKHOFF,
.product_code = ETHERCAT_PID_EK1100,
.sc = nullptr
};
static void ethercat_cyclic_task(struct node *n)
{
struct sample *smp;
struct ethercat *w = (struct ethercat *) n->_vd;
while (true) {
task_wait(&w->task);
/* Receive process data */
ecrt_master_receive(master);
ecrt_domain_process(w->domain);
/* Receive process data */
smp = sample_alloc(&w->pool);
if (!smp) {
warning("Pool underrun in node %s", node_name(n));
continue;
}
smp->length = MIN(w->in.num_channels, smp->capacity);
smp->flags = (int) SampleFlags::HAS_DATA;
smp->signals = &n->in.signals;
/* Read process data */
for (unsigned i = 0; i < smp->length; i++) {
int16_t ain_value = EC_READ_S16(w->domain_pd + w->in.offsets[i]);
smp->data[i].f = w->in.range * (float) ain_value / INT16_MAX;
}
queue_signalled_push(&w->queue, smp);
/* Write process data */
smp = w->send.exchange(nullptr);
for (unsigned i = 0; i < w->out.num_channels; i++) {
int16_t aout_value = (smp->data[i].f / w->out.range) * INT16_MAX;
EC_WRITE_S16(w->domain_pd + w->out.offsets[i], aout_value);
}
sample_decref(smp);
// send process data
ecrt_domain_queue(w->domain);
ecrt_master_send(master);
}
}
int ethercat_type_start(villas::node::SuperNode *sn)
{
int ret;
json_error_t err;
json_t *cfg = sn->getConfig();
if (cfg) {
ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?:i, s?: { s?: { s?: i, s?: i, s?: i } } }",
"ethernet",
"master", &master_id,
"alias", &alias,
"coupler",
"position", &coupler.position,
"product_code", &coupler.product_code,
"vendor_id", &coupler.vendor_id
);
if (ret)
jerror(&err, "Failed to parse EtherCAT configuration");
}
master = ecrt_request_master(master_id);
if (!master)
return -1;
/* Create configuration for bus coupler */
coupler.sc = ecrt_master_slave_config(master, alias, coupler.position, coupler.vendor_id, coupler.product_code);
if (!coupler.sc)
return -1;
return 0;
}
int ethercat_type_stop()
{
info("Releasing EtherCAT master");
ecrt_release_master(master);
return 0;
}
int ethercat_parse(struct node *n, json_t *cfg)
{
struct ethercat *w = (struct ethercat *) n->_vd;
int ret;
json_error_t err;
ret = json_unpack_ex(cfg, &err, 0, "{ s?: F, s?: { s: i, s?: F, s?: i, s?: i, s?: i }, s?: { s: i, s?: F, s?: i, s?: i, s?: i } }",
"rate", &w->rate,
"out",
"num_channels", &w->out.num_channels,
"range", &w->out.range,
"position", &w->out.position,
"product_code", &w->out.product_code,
"vendor_id", &w->out.vendor_id,
"in",
"num_channels", &w->in.num_channels,
"range", &w->in.range,
"position", &w->in.position,
"product_code", &w->in.product_code,
"vendor_id", &w->in.vendor_id
);
if (ret)
jerror(&err, "Failed to parse configuration of node %s", node_name(n));
return 0;
}
char * ethercat_print(struct node *n)
{
struct ethercat *w = (struct ethercat *) n->_vd;
std::stringstream ss;
ss << "alias=" << alias;
ss << ", in.range=" << w->in.range << ", in.num_channels=" << w->in.num_channels;
ss << ", out.range=" << w->out.range << ", out.num_channels=" << w->out.num_channels;
return strdup(ss.str().c_str());
}
int ethercat_check(struct node *n)
{
struct ethercat *w = (struct ethercat *) n->_vd;
/* Some parts of the configuration are still hard-coded for this specific setup */
if (w->in.product_code != ETHERCAT_PID_EL3008 ||
w->in.vendor_id != ETHERCAT_VID_BECKHOFF ||
w->out.product_code != ETHERCAT_PID_EL4038 ||
w->out.vendor_id != ETHERCAT_VID_BECKHOFF ||
coupler.product_code != ETHERCAT_PID_EK1100 ||
coupler.vendor_id != ETHERCAT_VID_BECKHOFF
)
return -1;
return 0;
}
int ethercat_prepare(struct node *n)
{
struct ethercat *w = (struct ethercat *) n->_vd;
w->in.offsets = new unsigned[w->in.num_channels];
if (!w->in.offsets)
throw RuntimeError("Failed to allocate memory");
w->out.offsets = new unsigned[w->out.num_channels];
if (!w->out.offsets)
throw RuntimeError("Failed to allocate memory");
w->domain_regs = new ec_pdo_entry_reg_t[w->in.num_channels + w->out.num_channels + 1];
if (!w->domain_regs)
throw RuntimeError("Failed to allocate memory");
memset(w->domain_regs, 0, (w->in.num_channels + w->out.num_channels + 1) * sizeof(ec_pdo_entry_reg_t));
/* Prepare list of domain registers */
int o = 0;
for (unsigned i = 0; i < w->out.num_channels; i++) {
w->out.offsets[i] = 0;
w->domain_regs[o].alias = alias;
w->domain_regs[o].position = w->out.position;
w->domain_regs[o].vendor_id = w->out.vendor_id;
w->domain_regs[o].product_code = w->out.product_code;
w->domain_regs[o].index = 0x7000 + i * 0x10;
w->domain_regs[o].subindex = 0x1;
w->domain_regs[o].offset = w->out.offsets + i;
o++;
};
/* Prepare list of domain registers */
for (unsigned i = 0; i < w->in.num_channels; i++) {
w->in.offsets[i] = 0;
w->domain_regs[o].alias = alias;
w->domain_regs[o].position = w->in.position;
w->domain_regs[o].vendor_id = w->in.vendor_id;
w->domain_regs[o].product_code = w->in.product_code;
w->domain_regs[o].index = 0x6000 + i * 0x10;
w->domain_regs[o].subindex = 0x11;
w->domain_regs[o].offset = w->in.offsets + i;
o++;
};
w->domain = ecrt_master_create_domain(master);
if (!w->domain)
return -1;
return 0;
}
int ethercat_start(struct node *n)
{
int ret;
struct ethercat *w = (struct ethercat *) n->_vd;
/* Configure analog in */
w->in.sc = ecrt_master_slave_config(master, alias, w->in.position, w->in.vendor_id, w->in.product_code);
if (!w->in.sc) {
warning("Failed to get slave configuration.");
return -1;
}
ret = ecrt_slave_config_pdos(w->in.sc, EC_END, slave_4_syncs);
if (ret) {
error("Failed to configure PDOs.");
return -1;
}
/* Configure analog out */
w->out.sc = ecrt_master_slave_config(master, alias, w->out.position, w->out.vendor_id, w->out.product_code);
if (!w->out.sc) {
warning("Failed to get slave configuration.");
return -1;
}
ret = ecrt_slave_config_pdos(w->out.sc, EC_END, slave_3_syncs);
if (ret) {
warning("Failed to configure PDOs.");
return -1;
}
ret = ecrt_domain_reg_pdo_entry_list(w->domain, w->domain_regs);
if (ret) {
error("PDO entry registration failed!");
return -1;
}
/** @todo Check that master is not already active... */
ret = ecrt_master_activate(master);
if (ret)
return -1;
w->domain_pd = ecrt_domain_data(w->domain);
if (!w->domain_pd)
return -1;
/* Start cyclic timer */
ret = task_init(&w->task, w->rate, CLOCK_REALTIME);
if (ret)
return -1;
/* Start cyclic task */
w->thread = std::thread(ethercat_cyclic_task, n);
return 0;
}
int ethercat_stop(struct node *n)
{
struct ethercat *w = (struct ethercat *) n->_vd;
w->thread.join();
return 0;
}
int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
struct ethercat *w = (struct ethercat *) n->_vd;
int avail;
struct sample *cpys[cnt];
avail = queue_signalled_pull_many(&w->queue, (void **) cpys, cnt);
if (avail < 0)
warning("Pool underrun for node %s: avail=%u", node_name(n), avail);
sample_copy_many(smps, cpys, avail);
sample_decref_many(cpys, avail);
return avail;
}
int ethercat_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
struct ethercat *w = (struct ethercat *) n->_vd;
if (cnt < 1)
return cnt;
struct sample *smp = smps[0];
sample_incref(smp);
struct sample *old = w->send.exchange(smp);
if (old)
sample_decref(old);
return 1;
}
int ethercat_init(struct node *n)
{
int ret;
struct ethercat *w = (struct ethercat *) n->_vd;
w->pool.state = State::DESTROYED;
/* Default values */
w->rate = 1000;
w->in.num_channels = 8;
w->in.range = 10.0;
w->in.position = 2;
w->in.product_code = ETHERCAT_PID_EL3008;
w->in.vendor_id = ETHERCAT_VID_BECKHOFF;
w->in.sc = nullptr;
w->in.offsets = nullptr;
w->out.num_channels = 8;
w->out.range = 10.0;
w->out.position = 1;
w->out.product_code = ETHERCAT_PID_EL4038;
w->out.vendor_id = ETHERCAT_VID_BECKHOFF;
w->out.sc = nullptr;
w->out.offsets = nullptr;
w->domain = nullptr;
w->domain_pd = nullptr;
w->domain_regs = nullptr;
/* Placement new for C++ objects */
new (&w->send) std::atomic<struct sample *>();
new (&w->thread) std::thread();
ret = pool_init(&w->pool, DEFAULT_ETHERCAT_QUEUE_LENGTH, SAMPLE_LENGTH(DEFAULT_ETHERCAT_SAMPLE_LENGTH));
if (ret)
return ret;
ret = queue_signalled_init(&w->queue, DEFAULT_ETHERCAT_QUEUE_LENGTH);
if (ret)
return ret;
return 0;
}
int ethercat_destroy(struct node *n)
{
int ret;
struct ethercat *w = (struct ethercat *) n->_vd;
if (w->domain_regs)
delete[] w->domain_regs;
if (w->in.offsets)
delete[] w->in.offsets;
if (w->out.offsets)
delete[] w->out.offsets;
ret = task_destroy(&w->task);
if (ret)
return ret;
ret = queue_signalled_destroy(&w->queue);
if (ret)
return ret;
ret = pool_destroy(&w->pool);
if (ret)
return ret;
/** @todo Destroy domain? */
return 0;
}
int ethercat_poll_fds(struct node *n, int *fds)
{
struct ethercat *w = (struct ethercat *) n->_vd;
fds[0] = queue_signalled_fd(&w->queue);
return 1;
}
__attribute__((constructor(110)))
static void register_plugin() {
if (plugins.state == State::DESTROYED)
vlist_init(&plugins);
p.name = "ethercat";
p.description = "Send and receive samples of an ethercat connection";
p.type = PluginType::NODE;
p.node.vectorize = 1; /* we only process a single sample per call */
p.node.size = sizeof(struct ethercat);
p.node.type.start = ethercat_type_start;
p.node.type.stop = ethercat_type_stop;
p.node.parse = ethercat_parse;
p.node.print = ethercat_print;
p.node.check = ethercat_check;
p.node.init = ethercat_init;
p.node.destroy = ethercat_destroy;
p.node.prepare = ethercat_prepare;
p.node.start = ethercat_start;
p.node.stop = ethercat_stop;
p.node.read = ethercat_read;
p.node.write = ethercat_write;
p.node.poll_fds = ethercat_poll_fds;
vlist_init(&p.node.instances);
vlist_push(&plugins, &p);
}
__attribute__((destructor(110)))
static void deregister_plugin() {
if (plugins.state != State::DESTROYED)
vlist_remove_all(&plugins, &p);
}