1
0
Fork 0
mirror of https://git.rwth-aachen.de/acs/public/villas/node/ synced 2025-03-09 00:00:00 +01:00

ethercat: bugfixes

This commit is contained in:
Divya Laxetti 2020-01-20 14:03:49 +01:00 committed by Steffen Vogel
parent 692da638af
commit 69256b0d79
5 changed files with 388 additions and 378 deletions

View file

@ -23,19 +23,23 @@
find_path(ETHERLAB_INCLUDE_DIR
NAMES ecrt.h
PATHS
/opt/etherlab/include
)
find_library(ETHERLAB_LIBRARY
NAMES ethercat
PATHS
/opt/etherlab/lib
)
include(FindPackageHandleStandardArgs)
# handle the QUIETLY and REQUIRED arguments and set VILLASNODE_FOUND to TRUE
# if all listed variables are TRUE
find_package_handle_standard_args(Etherlab DEFAULT_MSG
ETHERLAB_LIBRARY ETHERLAB_INCLUDE_DIR)
find_package_handle_standard_args(Etherlab DEFAULT_MSG ETHERLAB_LIBRARY ETHERLAB_INCLUDE_DIR)
mark_as_advanced(ETHERLAB_INCLUDE_DIR ETHERLAB_LIBRARY)
set(ETHERLAB_LIBRARIES ${ETHERLAB_LIBRARY})
set(ETHERLAB_INCLUDE_DIRS ${ETHERLAB_INCLUDE_DIR})
set(ETHERLAB_FOUND TRUE)

View file

@ -37,20 +37,42 @@
#include <villas/io.h>
#include <villas/config.h>
// Include hard-coded Ethercat Bus configuration
#include <villas/nodes/ethercat_config.h>
#ifdef __cplusplus
extern "C" {
#endif
#include <ecrt.h>
#define DEFAULT_ETHERCAT_QUEUE_LENGTH (DEFAULT_QUEUE_LENGTH * 64)
#define DEFAULT_ETHERCAT_SAMPLE_LENGTH DEFAULT_SAMPLE_LENGTH
/* Forward declaration */
struct lws;
/** Internal data per ethercat node */
struct ethercat {
/* Settings */
struct {
int num_channels;
} in, out;
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];
};
/* Internal datastructures */
@ -68,9 +90,6 @@ int ethercat_start(struct node *n);
/** @see node_type::close */
int ethercat_stop(struct node *n);
/** @see node_type::close */
int ethercat_destroy(struct node *n);
/** @see node_type::read */
int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release);

View file

@ -0,0 +1,195 @@
/** Configuration for Ethercat Node-type
*
* @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
* @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/>.
*********************************************************************************/
#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
#define ETHERCAT_VID_BECKHOFF 0x00000002
#define ETHERCAT_PID_EK1100 0x044c2c52
#define ETHERCAT_PID_EL2004 0x07d43052
#define ETHERCAT_PID_EL2032 0x07f03052
#define ETHERCAT_PID_EL3152 0x0c503052
#define ETHERCAT_PID_EL3102 0x0c1e3052
#define ETHERCAT_PID_EL4102 0x10063052
// CWD Bus
#define ETHERCAT_PID_EL4038 0x0fc63052
#define ETHERCAT_PID_EL3008 0x0bc03052
#define ETHERCAT_PID_FC1100 0x044c0c62
#include <ecrt.h>
/*****************************************************************************/
/* Master 0, Slave 3, "EL4038"
* Vendor ID: 0x00000002
* Product code: 0x0fc63052
* Revision number: 0x00140000
*/
static ec_pdo_entry_info_t slave_3_pdo_entries[] = {
{0x7000, 0x01, 16}, /* Analog output */
{0x7010, 0x01, 16}, /* Analog output */
{0x7020, 0x01, 16}, /* Analog output */
{0x7030, 0x01, 16}, /* Analog output */
{0x7040, 0x01, 16}, /* Analog output */
{0x7050, 0x01, 16}, /* Analog output */
{0x7060, 0x01, 16}, /* Analog output */
{0x7070, 0x01, 16}, /* Analog output */
};
static ec_pdo_info_t slave_3_pdos[] = {
{0x1600, 1, slave_3_pdo_entries + 0}, /* AO RxPDO-Map Outputs Ch.1 */
{0x1601, 1, slave_3_pdo_entries + 1}, /* AO RxPDO-Map Outputs Ch.2 */
{0x1602, 1, slave_3_pdo_entries + 2}, /* AO RxPDO-Map Outputs Ch.3 */
{0x1603, 1, slave_3_pdo_entries + 3}, /* AO RxPDO-Map Outputs Ch.4 */
{0x1604, 1, slave_3_pdo_entries + 4}, /* AO RxPDO-Map Outputs Ch.5 */
{0x1605, 1, slave_3_pdo_entries + 5}, /* AO RxPDO-Map Outputs Ch.6 */
{0x1606, 1, slave_3_pdo_entries + 6}, /* AO RxPDO-Map Outputs Ch.7 */
{0x1607, 1, slave_3_pdo_entries + 7}, /* AO RxPDO-Map Outputs Ch.8 */
};
static ec_sync_info_t slave_3_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 8, slave_3_pdos + 0, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{0xff}
};
/* Master 0, Slave 4, "EL3008"
* Vendor ID: 0x00000002
* Product code: 0x0bc03052
* Revision number: 0x00150000
*/
static ec_pdo_entry_info_t slave_4_pdo_entries[] = {
{0x6000, 0x01, 1}, /* Underrange */
{0x6000, 0x02, 1}, /* Overrange */
{0x6000, 0x03, 2}, /* Limit 1 */
{0x6000, 0x05, 2}, /* Limit 2 */
{0x6000, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6000, 0x0f, 1}, /* TxPDO State */
{0x6000, 0x10, 1}, /* TxPDO Toggle */
{0x6000, 0x11, 16}, /* Value */
{0x6010, 0x01, 1}, /* Underrange */
{0x6010, 0x02, 1}, /* Overrange */
{0x6010, 0x03, 2}, /* Limit 1 */
{0x6010, 0x05, 2}, /* Limit 2 */
{0x6010, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6010, 0x0f, 1}, /* TxPDO State */
{0x6010, 0x10, 1}, /* TxPDO Toggle */
{0x6010, 0x11, 16}, /* Value */
{0x6020, 0x01, 1}, /* Underrange */
{0x6020, 0x02, 1}, /* Overrange */
{0x6020, 0x03, 2}, /* Limit 1 */
{0x6020, 0x05, 2}, /* Limit 2 */
{0x6020, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6020, 0x0f, 1}, /* TxPDO State */
{0x6020, 0x10, 1}, /* TxPDO Toggle */
{0x6020, 0x11, 16}, /* Value */
{0x6030, 0x01, 1}, /* Underrange */
{0x6030, 0x02, 1}, /* Overrange */
{0x6030, 0x03, 2}, /* Limit 1 */
{0x6030, 0x05, 2}, /* Limit 2 */
{0x6030, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6030, 0x0f, 1}, /* TxPDO State */
{0x6030, 0x10, 1}, /* TxPDO Toggle */
{0x6030, 0x11, 16}, /* Value */
{0x6040, 0x01, 1}, /* Underrange */
{0x6040, 0x02, 1}, /* Overrange */
{0x6040, 0x03, 2}, /* Limit 1 */
{0x6040, 0x05, 2}, /* Limit 2 */
{0x6040, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6040, 0x0f, 1}, /* TxPDO State */
{0x6040, 0x10, 1}, /* TxPDO Toggle */
{0x6040, 0x11, 16}, /* Value */
{0x6050, 0x01, 1}, /* Underrange */
{0x6050, 0x02, 1}, /* Overrange */
{0x6050, 0x03, 2}, /* Limit 1 */
{0x6050, 0x05, 2}, /* Limit 2 */
{0x6050, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6050, 0x0f, 1}, /* TxPDO State */
{0x6050, 0x10, 1}, /* TxPDO Toggle */
{0x6050, 0x11, 16}, /* Value */
{0x6060, 0x01, 1}, /* Underrange */
{0x6060, 0x02, 1}, /* Overrange */
{0x6060, 0x03, 2}, /* Limit 1 */
{0x6060, 0x05, 2}, /* Limit 2 */
{0x6060, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6060, 0x0f, 1}, /* TxPDO State */
{0x6060, 0x10, 1}, /* TxPDO Toggle */
{0x6060, 0x11, 16}, /* Value */
{0x6070, 0x01, 1}, /* Underrange */
{0x6070, 0x02, 1}, /* Overrange */
{0x6070, 0x03, 2}, /* Limit 1 */
{0x6070, 0x05, 2}, /* Limit 2 */
{0x6070, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6070, 0x0f, 1}, /* TxPDO State */
{0x6070, 0x10, 1}, /* TxPDO Toggle */
{0x6070, 0x11, 16}, /* Value */
};
static ec_pdo_info_t slave_4_pdos[] = {
{0x1a00, 10, slave_4_pdo_entries + 0}, /* AI TxPDO-Map Standard Ch.1 */
{0x1a02, 10, slave_4_pdo_entries + 10}, /* AI TxPDO-Map Standard Ch.2 */
{0x1a04, 10, slave_4_pdo_entries + 20}, /* AI TxPDO-Map Standard Ch.3 */
{0x1a06, 10, slave_4_pdo_entries + 30}, /* AI TxPDO-Map Standard Ch.4 */
{0x1a08, 10, slave_4_pdo_entries + 40}, /* AI TxPDO-Map Standard Ch.5 */
{0x1a0a, 10, slave_4_pdo_entries + 50}, /* AI TxPDO-Map Standard Ch.6 */
{0x1a0c, 10, slave_4_pdo_entries + 60}, /* AI TxPDO-Map Standard Ch.7 */
{0x1a0e, 10, slave_4_pdo_entries + 70}, /* AI TxPDO-Map Standard Ch.8 */
};
static ec_sync_info_t slave_4_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 8, slave_4_pdos + 0, EC_WD_DISABLE},
{0xff}
};

View file

@ -171,10 +171,10 @@ if(WITH_NODE_EXAMPLE)
endif()
# Enable Ethercat support
if(ETHERCAT_FOUND AND WITH_IO)
if(ETHERLAB_FOUND AND WITH_IO)
list(APPEND NODE_SRC ethercat.c)
list(APPEND INCLUDE_DIRS ${ETHERCAT_INCLUDE_DIRS})
list(APPEND LIBRARIES ${ETHERCAT_LIBRARIES})
list(APPEND INCLUDE_DIRS ${ETHERLAB_INCLUDE_DIRS})
list(APPEND LIBRARIES ${ETHERLAB_LIBRARIES})
endif()
add_library(nodes STATIC ${NODE_SRC})

View file

@ -1,4 +1,4 @@
//** Node type: Ethercat
/** Node type: Ethercat
*
* @author Niklas Eiling <niklas.eiling@eonerc.rwth-aachen.de>
* @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
@ -33,23 +33,50 @@
#include <villas/utils.h>
#include <villas/buffer.h>
#include <villas/plugin.h>
#include <villas/nodes/ethercat.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()
*/
#include "ecrt.h"
/****************************************************************************/
/** Task period in ns. */
#define PERIOD_NS (1000000)
#define MAX_SAFE_STACK (8 * 1024) /* The maximum stack size which is
guranteed safe to access without
faulting */
/****************************************************************************/
/* Constants */
@ -60,231 +87,22 @@
/* Forward declarations */
static struct plugin p;
static ec_master_t *master = NULL;
static ec_domain_t *domain1 = NULL;
static ec_slave_config_t *sc_ana_in = NULL;
static ec_slave_config_t *sc_ana_out = NULL;
// process data
static uint8_t *domain1_pd = NULL;
#define BusCouplerPos 0, 0
#define AnaOutSlavePos 0, 1
#define AnaInSlavePos 0, 2
#define Beckhoff_EK1100 0x00000002, 0x044c2c52
#define Beckhoff_EL2004 0x00000002, 0x07d43052
#define Beckhoff_EL2032 0x00000002, 0x07f03052
#define Beckhoff_EL3152 0x00000002, 0x0c503052
#define Beckhoff_EL3102 0x00000002, 0x0c1e3052
#define Beckhoff_EL4102 0x00000002, 0x10063052
//CWD Bus
#define Beckhoff_EL4038 0x00000002, 0x0fc63052
#define Beckhoff_EL3008 0x00000002, 0x0bc03052
#define Beckhoff_FC1100 0x00000002, 0x044c0c62
// offsets for PDO entries
static unsigned int off_ana_out_values[8] = {0};
static unsigned int off_ana_in_values[8] = {0};
const static ec_pdo_entry_reg_t domain1_regs[] = {
{AnaOutSlavePos, Beckhoff_EL4038, 0x7000, 0x01, off_ana_out_values},
{AnaOutSlavePos, Beckhoff_EL4038, 0x7010, 0x01, off_ana_out_values+1},
{AnaOutSlavePos, Beckhoff_EL4038, 0x7020, 0x01, off_ana_out_values+2},
{AnaOutSlavePos, Beckhoff_EL4038, 0x7030, 0x01, off_ana_out_values+3},
{AnaOutSlavePos, Beckhoff_EL4038, 0x7040, 0x01, off_ana_out_values+4},
{AnaOutSlavePos, Beckhoff_EL4038, 0x7050, 0x01, off_ana_out_values+5},
{AnaOutSlavePos, Beckhoff_EL4038, 0x7060, 0x01, off_ana_out_values+6},
{AnaOutSlavePos, Beckhoff_EL4038, 0x7070, 0x01, off_ana_out_values+7},
{AnaInSlavePos, Beckhoff_EL3008, 0x6000, 0x11, off_ana_in_values},
{AnaInSlavePos, Beckhoff_EL3008, 0x6010, 0x11, off_ana_in_values+1},
{AnaInSlavePos, Beckhoff_EL3008, 0x6020, 0x11, off_ana_in_values+2},
{AnaInSlavePos, Beckhoff_EL3008, 0x6030, 0x11, off_ana_in_values+3},
{AnaInSlavePos, Beckhoff_EL3008, 0x6040, 0x11, off_ana_in_values+4},
{AnaInSlavePos, Beckhoff_EL3008, 0x6050, 0x11, off_ana_in_values+5},
{AnaInSlavePos, Beckhoff_EL3008, 0x6060, 0x11, off_ana_in_values+6},
{AnaInSlavePos, Beckhoff_EL3008, 0x6070, 0x11, off_ana_in_values+7},
{}
};
static unsigned int counter = 0;
//static unsigned int blink = 0;
/*****************************************************************************/
/* Master 0, Slave 3, "EL4038"
* Vendor ID: 0x00000002
* Product code: 0x0fc63052
* Revision number: 0x00140000
*/
static ec_pdo_entry_info_t slave_3_pdo_entries[] = {
{0x7000, 0x01, 16}, /* Analog output */
{0x7010, 0x01, 16}, /* Analog output */
{0x7020, 0x01, 16}, /* Analog output */
{0x7030, 0x01, 16}, /* Analog output */
{0x7040, 0x01, 16}, /* Analog output */
{0x7050, 0x01, 16}, /* Analog output */
{0x7060, 0x01, 16}, /* Analog output */
{0x7070, 0x01, 16}, /* Analog output */
};
static ec_pdo_info_t slave_3_pdos[] = {
{0x1600, 1, slave_3_pdo_entries + 0}, /* AO RxPDO-Map Outputs Ch.1 */
{0x1601, 1, slave_3_pdo_entries + 1}, /* AO RxPDO-Map Outputs Ch.2 */
{0x1602, 1, slave_3_pdo_entries + 2}, /* AO RxPDO-Map Outputs Ch.3 */
{0x1603, 1, slave_3_pdo_entries + 3}, /* AO RxPDO-Map Outputs Ch.4 */
{0x1604, 1, slave_3_pdo_entries + 4}, /* AO RxPDO-Map Outputs Ch.5 */
{0x1605, 1, slave_3_pdo_entries + 5}, /* AO RxPDO-Map Outputs Ch.6 */
{0x1606, 1, slave_3_pdo_entries + 6}, /* AO RxPDO-Map Outputs Ch.7 */
{0x1607, 1, slave_3_pdo_entries + 7}, /* AO RxPDO-Map Outputs Ch.8 */
};
static ec_sync_info_t slave_3_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 8, slave_3_pdos + 0, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{0xff}
};
/* Master 0, Slave 4, "EL3008"
* Vendor ID: 0x00000002
* Product code: 0x0bc03052
* Revision number: 0x00150000
*/
static ec_pdo_entry_info_t slave_4_pdo_entries[] = {
{0x6000, 0x01, 1}, /* Underrange */
{0x6000, 0x02, 1}, /* Overrange */
{0x6000, 0x03, 2}, /* Limit 1 */
{0x6000, 0x05, 2}, /* Limit 2 */
{0x6000, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6000, 0x0f, 1}, /* TxPDO State */
{0x6000, 0x10, 1}, /* TxPDO Toggle */
{0x6000, 0x11, 16}, /* Value */
{0x6010, 0x01, 1}, /* Underrange */
{0x6010, 0x02, 1}, /* Overrange */
{0x6010, 0x03, 2}, /* Limit 1 */
{0x6010, 0x05, 2}, /* Limit 2 */
{0x6010, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6010, 0x0f, 1}, /* TxPDO State */
{0x6010, 0x10, 1}, /* TxPDO Toggle */
{0x6010, 0x11, 16}, /* Value */
{0x6020, 0x01, 1}, /* Underrange */
{0x6020, 0x02, 1}, /* Overrange */
{0x6020, 0x03, 2}, /* Limit 1 */
{0x6020, 0x05, 2}, /* Limit 2 */
{0x6020, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6020, 0x0f, 1}, /* TxPDO State */
{0x6020, 0x10, 1}, /* TxPDO Toggle */
{0x6020, 0x11, 16}, /* Value */
{0x6030, 0x01, 1}, /* Underrange */
{0x6030, 0x02, 1}, /* Overrange */
{0x6030, 0x03, 2}, /* Limit 1 */
{0x6030, 0x05, 2}, /* Limit 2 */
{0x6030, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6030, 0x0f, 1}, /* TxPDO State */
{0x6030, 0x10, 1}, /* TxPDO Toggle */
{0x6030, 0x11, 16}, /* Value */
{0x6040, 0x01, 1}, /* Underrange */
{0x6040, 0x02, 1}, /* Overrange */
{0x6040, 0x03, 2}, /* Limit 1 */
{0x6040, 0x05, 2}, /* Limit 2 */
{0x6040, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6040, 0x0f, 1}, /* TxPDO State */
{0x6040, 0x10, 1}, /* TxPDO Toggle */
{0x6040, 0x11, 16}, /* Value */
{0x6050, 0x01, 1}, /* Underrange */
{0x6050, 0x02, 1}, /* Overrange */
{0x6050, 0x03, 2}, /* Limit 1 */
{0x6050, 0x05, 2}, /* Limit 2 */
{0x6050, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6050, 0x0f, 1}, /* TxPDO State */
{0x6050, 0x10, 1}, /* TxPDO Toggle */
{0x6050, 0x11, 16}, /* Value */
{0x6060, 0x01, 1}, /* Underrange */
{0x6060, 0x02, 1}, /* Overrange */
{0x6060, 0x03, 2}, /* Limit 1 */
{0x6060, 0x05, 2}, /* Limit 2 */
{0x6060, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6060, 0x0f, 1}, /* TxPDO State */
{0x6060, 0x10, 1}, /* TxPDO Toggle */
{0x6060, 0x11, 16}, /* Value */
{0x6070, 0x01, 1}, /* Underrange */
{0x6070, 0x02, 1}, /* Overrange */
{0x6070, 0x03, 2}, /* Limit 1 */
{0x6070, 0x05, 2}, /* Limit 2 */
{0x6070, 0x07, 1}, /* Error */
{0x0000, 0x00, 1}, /* Gap */
{0x0000, 0x00, 6}, /* Gap */
{0x6070, 0x0f, 1}, /* TxPDO State */
{0x6070, 0x10, 1}, /* TxPDO Toggle */
{0x6070, 0x11, 16}, /* Value */
};
static ec_pdo_info_t slave_4_pdos[] = {
{0x1a00, 10, slave_4_pdo_entries + 0}, /* AI TxPDO-Map Standard Ch.1 */
{0x1a02, 10, slave_4_pdo_entries + 10}, /* AI TxPDO-Map Standard Ch.2 */
{0x1a04, 10, slave_4_pdo_entries + 20}, /* AI TxPDO-Map Standard Ch.3 */
{0x1a06, 10, slave_4_pdo_entries + 30}, /* AI TxPDO-Map Standard Ch.4 */
{0x1a08, 10, slave_4_pdo_entries + 40}, /* AI TxPDO-Map Standard Ch.5 */
{0x1a0a, 10, slave_4_pdo_entries + 50}, /* AI TxPDO-Map Standard Ch.6 */
{0x1a0c, 10, slave_4_pdo_entries + 60}, /* AI TxPDO-Map Standard Ch.7 */
{0x1a0e, 10, slave_4_pdo_entries + 70}, /* AI TxPDO-Map Standard Ch.8 */
};
static ec_sync_info_t slave_4_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 8, slave_4_pdos + 0, EC_WD_DISABLE},
{0xff}
};
int ethercat_type_start(struct super_node *sn)
{
ec_slave_config_t *sc;
struct timespec_wakeup_time;
int ret = 0;
void stack_prefault(void)
{
unsigned char dummy[MAX_SAFE_STACK];
memset(dummy, 0, MAX_SAFE_STACK);
}
master = ecrt_request_master(0);
if(!master) {
master = ecrt_request_master(0);
if (!master) {
return -1;
}
domain1 = ecrt_master_create_domain(master);
if(!domain1){
return -1;
}
//Create configuration for bus coupler
sc = ecrt_master_slave_config(master, BusCouplerPos< Beckhoff_EK1100);
if(!sc){
// 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;
}
@ -293,57 +111,83 @@ int ethercat_type_start(struct super_node *sn)
int ethercat_start(struct node *n)
{
int ret;
struct ethercat *w = (struct ethercat *) n->_vd;
ret = pool_init(&w->pool, DEFAULT_ETHERCAT_QUEUE_LENGTH, SAMPLE_LENGTH(DEFAULT_ETHERCAT_SAMPLE_LENGTH), &memory_hugepage);
if (ret)
return ret;
// 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;
// ret = queue_signalled_init(&w->queue, DEFAULT_ETHERCAT_QUEUE_LENGTH, &memory_hugepage, 0);
// if (ret)
// return ret;
//Configure analog in
printf("Configuring PDOs...\n");
if (!(sc_ana_in = ecrt_master_slave_config(
master, AnaInSlavePos, Beckhoff_EL3008))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
w->domain = ecrt_master_create_domain(master);
if (!w->domain) {
return -1;
}
if (ecrt_slave_config_pdos(sc_ana_in, EC_END, slave_4_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
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;
//Configure analog out
if (!(sc_ana_out = ecrt_master_slave_config(
master, AnaOutSlavePos, Beckhoff_EL4038))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
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;
if (ecrt_slave_config_pdos(sc_ana_out, EC_END, slave_3_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
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;
};
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
// End of list delimiter
memset(&w->domain_regs[2*ETHERCAT_NUM_CHANNELS], 0, sizeof(ec_pdo_entry_reg_t));
printf("Activating master...\n");
if (ecrt_master_activate(master)) {
return -1;
}
// Configure analog in
printf("Configuring PDOs...\n");
if (!(w->sc_in = ecrt_master_slave_config(master, ETHERCAT_ALIAS, ETHERCAT_POS_SLAVE_IN, ETHERCAT_VID_BECKHOFF, ETHERCAT_PID_EL3008))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
if (!(domain1_pd = ecrt_domain_data(domain1))) {
return -1;
}
if (ecrt_slave_config_pdos(w->sc_in, EC_END, slave_4_syncs)) {
fprintf(stderr, "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))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
if (ecrt_slave_config_pdos(w->sc_out, EC_END, slave_3_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(w->domain, w->domain_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
printf("Activating master...\n");
if (ecrt_master_activate(master)) {
return -1;
}
if (!(w->domain_pd = ecrt_domain_data(w->domain))) {
return -1;
}
return 0;
}
@ -352,9 +196,9 @@ int ethercat_stop(struct node *n)
{
int ret;
struct ethercat *w = (struct ethercat *) n->_vd;
printf("releasing master"\n);
ecrt_release_master(master);
printf("have a nice day!\n")
printf("releasing master\n");
ecrt_release_master(master);
printf("have a nice day!\n");
ret = queue_signalled_destroy(&w->queue);
if (ret)
@ -367,144 +211,92 @@ int ethercat_stop(struct node *n)
return 0;
}
int ethercat_destroy(struct node *n)
{
struct websocket *w = (struct websocket *) n->_vd;
int ret;
/* TODO: destroy ethercat connection */
if (ret)
return ret;
return 0;
}
int ethercat_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
int avail;
struct ethercat *w = (struct ethercat *) n->_vd;
struct sample *cpys[cnt];
avail = queue_signalled_pull_many(&w->queue, (void **) cpys, cnt);
if (avail < 0)
return avail;
// int avail;
// struct sample *cpys[cnt];
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// avail = queue_signalled_pull_many(&w->queue, (void **) cpys, cnt);
// if (avail < 0)
// return avail;
#if 1
//read process data
for(int i=0; i<8; ++i) {
int16_t ain_value + EC_READ_S16(domain1_pd + Off_ana_in_values[i]);
// 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]);
float ain_voltage = 10.0 * (float) ain_value/INT16_MAX;
printf("AnaIn(%d): value=%f Volts\n", i, ain_voltage);
}
printf("\n");
#endif
smps[0]->data[i].f = ETHERCAT_VOLTAGE_RANGE * (float) ain_value / INT16_MAX;
}
// sample_copy_many(smps, cpys, avail);
// sample_decref_many(cpys, avail);
sample_copy_many(smps, cpys, avail);
sample_decref_many(cpys, avail);
return avail;
return 1;
}
int ethercat_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
int avail;
struct ethercat *w = (struct ethercat *) n->_vd;
struct sample *cpys[cnt];
/* Make copies of all samples */
avail = sample_alloc_many(&w->pool, cpys, cnt);
if (avail < cnt)
warn("Pool underrun for node %s: avail=%u", node_name(n), avail);
// 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);
// sample_copy_many(cpys, smps, avail);
#if 1
// write process data
for(int i=0; i<8; ++i) {
float aout_voltage = i * 1.0;
if (cnt < 1)
return 0;
in16_t aout_value = aout_voltage / 10.0 * INT16_MAX;
// Write process data
for (int i = 0; i < ETHERCAT_NUM_CHANNELS; ++i) {
float aout_voltage = smps[0]->data[i].f;
if (counter == 0)
printf("AnaOut(%d) : value=%f Volts\n", i, aout_voltage);
int16_t aout_value = aout_voltage / ETHERCAT_VOLTAGE_RANGE * INT16_MAX;
EC_WRITE_U16(domain1_pd + off_ana_out_values[i], 0x8000);
}
//EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#endif
EC_WRITE_U16(w->domain_pd + w->off_out_values[i], aout_value);
}
// send process data
//printf("queue\n");
ecrt_domain_queue(domain1);
//printf("send\n");
ecrt_master_send(master);
// send process data
ecrt_domain_queue(w->domain);
ecrt_master_send(master);
//sample_decref_many(cpys, avail);
return 1;
}
// int ethercat_poll_fds(struct node *n, int *fds)
// {
// struct ethercat *w = (struct ethercat *) n->_vd;
// fds[0] = queue_signalled_fd(&w->queue);
sample_decref_many(cpys, avail);
return cnt;
}
int ethercat_parse(struct node *n, json_t *cfg)
{
struct ethercat *w = (struct ethercat *) n->_vd;
int ret;
size_t i;
json_t *json_dests = NULL;
json_t *json_dest;
json_error_t err;
/* TODO: parse json */
return 0;
}
char * ethercat_print(struct node *n)
{
struct ethercat *w = (struct ethercat *) n->_vd;
char *buf = NULL;
/* TODO: maybe use asprintf to build string? */
return buf;
}
int ethercat_fd(struct node *n)
{
struct ethercat *w = (struct ethercat *) n->_vd;
return 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 = 0, /* unlimited */
.vectorize = 1, /* we only process a single sample per call */
.size = sizeof(struct ethercat),
.type.start = ethercat_type_start,
.start = ethercat_start,
.stop = ethercat_stop,
.destroy = ethercat_destroy,
.read = ethercat_read,
.write = ethercat_write,
.print = ethercat_print,
.parse = ethercat_parse,
.fd = ethercat_fd
// .poll_fds = ethercat_poll_fds
}
};