Commit 5d4b5b4a authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Lots of ethercat changes

Split esc (ethercat slave controller) code into seperate modules.
Add code to read SII eeprom of esc devices and use it for
device initialization.
Add per device thread for monitoring/recovery of devices.
parent 999e7277
......@@ -19,10 +19,9 @@
#include <string.h>
#include <stdio.h>
#include <malloc.h>
#include "controller_block.h"
#include "esc.h"
#include "ec_stoeber.h"
struct controller_block_private {
bool *ba1;
......
......@@ -16,6 +16,15 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
dt_ctr.c
Main loop for the DT controller.
Here the controller network is initialized.
(By loading a .ctrl file supplied on the command line).
After initialization the sample thread is started and this function's
work is done.
*/
#define _GNU_SOURCE
......
......@@ -20,10 +20,9 @@
#include <string.h>
#include <stdio.h>
#include <malloc.h>
#include "controller_block.h"
#include "esc.h"
#include "ec_stoeber.h"
#include "log.h"
struct controller_block_private {
......
......@@ -20,10 +20,9 @@
#include <string.h>
#include <stdio.h>
#include <malloc.h>
#include "controller_block.h"
#include "esc.h"
#include "ec_stoeber.h"
struct controller_block_private {
bool *ba1;
......
......@@ -11,8 +11,8 @@ endif
LIBSRCS= ec.c \
esc.c esc_coe.c esc_esi.c esc_mailbox.c \
canopen.c ec_stoeber.c block_ec.c
esc.c esc_coe.c esc_esi.c esc_mailbox.c esc_device.c esc_watchdog.c \
esc_dc.c canopen.c ec_stoeber.c block_ec.c
ifeq ($(OS), FreeBSD)
LIBSRCS+= eth_bsd.c
......@@ -47,7 +47,7 @@ libethercat.la_install: libethercat.la
ec_enum: libethercat.la_install
ec_enum_LDFLAGS=-lethercat -llog
ec_enum_LDFLAGS=-lethercat
ec_enum: ec_enum.o
ec_enum.o: esc_id.h
......
......@@ -26,6 +26,8 @@
#include "block_beckhoff_el1xxx.h"
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_id.h"
#include "log.h"
struct controller_block_private {
......@@ -182,12 +184,17 @@ struct controller_block * block_beckhoff_el1xxx_create(char *name, va_list ap)
name, sizeof(struct controller_block_private));
if (!sim) {
block->private->dev = esc_esi_device_create(&addr);
block->private->dev = esc_device_create(&addr, name);
esc_esi_device_fill(block->private->dev);
esc_device_initialize_pre_operational(block->private->dev);
block->private->rx_buffer = block->private->dev->rx_data;
if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
}
/* Inputs on the hardware are outputs on this software block */
......
......@@ -26,6 +26,9 @@
#include "block_beckhoff_el2502.h"
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_coe.h"
#include "esc_id.h"
#define EL2502_COE_RXPDO_ASSIGN 0x1c12
#define EL2502_COE_RXPDO_ASSIGN_O1_PWM 0x1600 /* only PWM */
......@@ -225,8 +228,8 @@ struct controller_block * block_beckhoff_el2502_create(char *name, va_list ap)
printf("Could not go to state safe operational\n");
if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state safe operational\n");
printf("state: %d\n", esc_al_state_get(&addr));
printf("statuscode: %d\n", esc_al_status_code_get(&addr));
printf("%s: state: %d\n", name, esc_al_state_get(&addr));
printf("%s: statuscode: %d\n", name, esc_al_status_code_get(&addr));
block->outputs = 0;
block->output = NULL;
......
......@@ -26,6 +26,8 @@
#include "block_beckhoff_el2xxx.h"
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_id.h"
#include "log.h"
struct controller_block_private {
......@@ -183,7 +185,9 @@ struct controller_block * block_beckhoff_el2xxx_create(char *name, va_list ap)
name, sizeof(struct controller_block_private));
if (!sim) {
block->private->dev = esc_esi_device_create(&addr);
block->private->dev = esc_device_create(&addr, name);
esc_esi_device_fill(block->private->dev);
esc_device_initialize_pre_operational(block->private->dev);
block->private->tx_buffer = block->private->dev->tx_data;
......@@ -191,8 +195,10 @@ struct controller_block * block_beckhoff_el2xxx_create(char *name, va_list ap)
log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
log_send(LOG_T_DEBUG, "state: %d", esc_al_state_get(&addr));
log_send(LOG_T_DEBUG, "statuscode: %d", esc_al_status_code_get(&addr));
log_send(LOG_T_DEBUG, "%s: state: %d",
name, esc_al_state_get(&addr));
log_send(LOG_T_DEBUG, "%s: statuscode: %d",
name, esc_al_status_code_get(&addr));
}
/* Outputs on the hardware are inputs on this software block */
......
......@@ -28,6 +28,9 @@
#include "block_beckhoff_el3xxx.h"
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_coe.h"
#include "esc_id.h"
#include "log.h"
......@@ -241,6 +244,7 @@ static struct controller_block_outterm_list outterms_8[] = {
{ "error", CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, have_errors) },
{ NULL },
};
static struct beckhoff_el3xxx_type devlist[] = {
/*
* name, productcode, inputs, gain, offset, sign, pdo_bits_per_ch, driver_verified
......@@ -334,6 +338,7 @@ struct controller_block * block_beckhoff_el3xxx_create(char *name, va_list ap)
char *eltype;
int devno;
struct timespec timeout = { 1, 0 };
uint32_t vendor, product;
eltype = va_arg(ap, char *);
......@@ -348,8 +353,6 @@ struct controller_block * block_beckhoff_el3xxx_create(char *name, va_list ap)
}
if (dev == NULL)
return NULL;
uint32_t vendor, product;
ec_addr_set_auto_inc_nr(&addr, devno);
vendor = esc_esi_vendorid_get(&addr);
......@@ -372,16 +375,20 @@ struct controller_block * block_beckhoff_el3xxx_create(char *name, va_list ap)
if (!block)
return NULL;
block->private->dev = esc_esi_device_create(&addr);
block->private->dev = esc_device_create(&addr, name);
esc_esi_device_fill(block->private->dev);
esc_device_initialize_pre_operational(block->private->dev);
block->private->rx_buffer = block->private->dev->rx_data;
esc_coe_create(block->private->dev->mailbox);
if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state safe operational\n");
log_send(LOG_T_ERROR,
"%s: Could not go to state safe operational", name);
if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state operational\n");
log_send(LOG_T_ERROR,
"%s: Could not go to state operational", name);
/* Inputs on the hardware are outputs on this software block */
......
This diff is collapsed.
......@@ -26,6 +26,9 @@
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_coe.h"
#include "esc_id.h"
#include "log.h"
struct controller_block_private {
......@@ -36,6 +39,8 @@ struct controller_block_private {
double offset;
unsigned char *rx_buffer;
struct esc_device *dev;
};
static void calculate(struct controller_block *block)
......@@ -44,7 +49,7 @@ static void calculate(struct controller_block *block)
struct {
uint8_t status;
uint32_t value;
} __attribute__((packed)) *rxdata;
} __packed *rxdata;
uint32_t value;
rxdata = (void*)priv->rx_buffer;
......@@ -56,41 +61,27 @@ static void calculate(struct controller_block *block)
priv->converted_value = (value + priv->gain) / priv->offset;
}
static struct controller_block_outterm_list outterms[] = {
{ "converted_value", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, converted_value) },
{ "value", CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, value) },
{ NULL },
};
struct controller_block * block_beckhoff_el5001_create(char *name, va_list ap)
{
struct controller_block *block;
struct ec_dgram_addr addr;
struct esc_syncmanager_info pdo_rx, pdo_tx;
int slaves;
int i;
bool found;
int devno;
struct timespec timeout = { 1, 0 };
struct esc_mailbox *mb;
struct esc_syncmanager_info mb_rd, mb_wr;
uint32_t vendor, product;
devno = va_arg(ap, int);
slaves = ec_slave_count();
found = false;
for (i = 0; i < slaves; i++) {
struct ec_dgram_addr devaddr;
uint32_t vendor, product;
ec_addr_set_auto_inc_nr(&devaddr, i);
vendor = esc_esi_vendorid_get(&devaddr);
product = esc_esi_productcode_get(&devaddr);
if (vendor == ESC_ESI_VENDORID_BECKHOFF &&
product == ESC_ESI_PRODUCTCODE_BECKHOFF_EL5001) {
if (!devno || devno == i) {
found = true;
addr = devaddr;
}
}
}
if (!found) {
ec_addr_set_auto_inc_nr(&addr, devno);
vendor = esc_esi_vendorid_get(&addr);
product = esc_esi_productcode_get(&addr);
if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
product != ESC_ESI_PRODUCTCODE_BECKHOFF_EL5001) {
log_send(LOG_T_ERROR, "%s: Device not found\n", name);
return NULL;
}
......@@ -100,69 +91,30 @@ struct controller_block * block_beckhoff_el5001_create(char *name, va_list ap)
if (!block)
return NULL;
esc_init(&addr);
pdo_rx.start = 0x1100;
pdo_rx.len = 5;
pdo_rx.sm = 3;
block->private->dev = esc_device_create(&addr, name);
esc_esi_device_fill(block->private->dev);
esc_device_initialize_pre_operational(block->private->dev);
pdo_tx.start = 0x1000;
pdo_tx.len = 0;
pdo_tx.sm = 2;
esc_pdo_tx_set(&addr, &pdo_tx, NULL, true);
esc_pdo_rx_set(&addr, &pdo_rx, &block->private->rx_buffer);
if (esc_al_state_set(&addr, ESC_AL_STATE_INIT, &timeout) < 0)
printf("Could not go to state init\n");
block->private->rx_buffer = block->private->dev->rx_data;
/* mailbox read (slave->master) @0x18f6 */
mb_rd.start = 0x18f6;
mb_rd.len = 246;
mb_rd.sm = 1;
/* mailbox write (master->slave) @0x1800 */
mb_wr.start = 0x1800;
mb_wr.len = 246;
mb_wr.sm = 0;
mb = esc_mailbox_create(&addr, &mb_rd, &mb_wr);
esc_coe_create(mb);
esc_coe_create(block->private->dev->mailbox);
if (esc_al_state_set(&addr, ESC_AL_STATE_PRE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state pre operational\n");
if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state safe operational\n");
log_send(LOG_T_ERROR,
"%s: Could not go to state safe operational", name);
if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state operational\n");
log_send(LOG_T_ERROR,
"%s: Could not go to state operational", name);
block->inputs = 0;
block->input = NULL;
/* Inputs on the hardware are outputs on this software block */
block->outputs = 2;
block->output = malloc(sizeof(struct controller_block_outterm) * 8);
if (!block->output)
if (controller_block_outterm_list_init(block, outterms))
return NULL;
block->output[0].name = "converted_value";
block->output[0].type = CONTROLLER_BLOCK_TERM_FLOAT;
block->output[0].value.f = &block->private->converted_value;
block->output[0].source = block;
block->output[1].name = "value";
block->output[1].type = CONTROLLER_BLOCK_TERM_UINT32;
block->output[1].value.v = &block->private->value;
block->output[1].source = block;
block->calculate = calculate;
block->private->gain = 1.0;
block->private->offset = 0.0;
block->params = 0;
block->param = NULL;
block->param_get = NULL;
block->param_set = NULL;
controller_block_add(block);
return block;
}
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009, 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2009
Copyright (c) 2010, 2012,
Daan Vreeken <Daan @ Vitsch . nl> - Vitsch Electronics
......@@ -31,8 +31,17 @@
#include "block_beckhoff_el5101.h"
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_coe.h"
#include "esc_id.h"
#include "log.h"
#define EL5101_OBJ_SM_OUTPUT 0x1c32
#define EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE 1
#define EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE_SYNC0 2
struct controller_block_private {
float converted_value;
int16_t enc;
......@@ -43,6 +52,9 @@ struct controller_block_private {
unsigned char *rx_buffer;
unsigned char *tx_buffer;
struct esc_device *dev;
struct canopen_dev *can_dev;
};
static void calculate(struct controller_block *block)
......@@ -52,11 +64,11 @@ static void calculate(struct controller_block *block)
uint8_t status;
uint16_t value;
uint16_t latch;
} __attribute__((packed)) *rxdata;
} __packed *rxdata;
struct {
uint8_t ctrl;
uint16_t value;
} __attribute__((packed)) *txdata;
} __packed *txdata;
uint16_t enc;
int32_t value = priv->value;
......@@ -90,36 +102,20 @@ struct controller_block * block_beckhoff_el5101_create(char *name, va_list ap)
{
struct controller_block *block;
struct ec_dgram_addr addr;
struct esc_syncmanager_info pdo_rx, pdo_tx;
int slaves;
int i;
bool found;
struct canopen_dev *can_dev;
int devno;
struct timespec timeout = { 1, 0 };
struct esc_mailbox *mb;
struct esc_syncmanager_info mb_rd, mb_wr;
uint32_t vendor, product, leval32;
uint16_t leval16;
devno = va_arg(ap, int);
slaves = ec_slave_count();
found = false;
for (i = 0; i < slaves; i++) {
struct ec_dgram_addr devaddr;
uint32_t vendor, product;
ec_addr_set_auto_inc_nr(&devaddr, i);
vendor = esc_esi_vendorid_get(&devaddr);
product = esc_esi_productcode_get(&devaddr);
if (vendor == ESC_ESI_VENDORID_BECKHOFF &&
product == ESC_ESI_PRODUCTCODE_BECKHOFF_EL5101) {
if (!devno || devno == i) {
found = true;
addr = devaddr;
}
}
}
if (!found) {
ec_addr_set_auto_inc_nr(&addr, devno);
vendor = esc_esi_vendorid_get(&addr);
product = esc_esi_productcode_get(&addr);
if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
product != ESC_ESI_PRODUCTCODE_BECKHOFF_EL5101) {
log_send(LOG_T_ERROR, "%s: Device not found\n", name);
return NULL;
}
......@@ -129,41 +125,64 @@ struct controller_block * block_beckhoff_el5101_create(char *name, va_list ap)
if (!block)
return NULL;
esc_init(&addr);
block->private->dev = esc_device_create(&addr, name);
esc_esi_device_fill(block->private->dev);
pdo_tx.start = 0x1000;
pdo_tx.len = 3;
pdo_tx.sm = 2;
block->private->dev->dc_sync.sync0_active = false;
pdo_rx.start = 0x1100;
pdo_rx.len = 5;
pdo_rx.sm = 3;
esc_pdo_tx_set(&addr, &pdo_tx, &block->private->tx_buffer, false);
esc_pdo_rx_set(&addr, &pdo_rx, &block->private->rx_buffer);
esc_device_initialize_pre_operational(block->private->dev);
if (esc_al_state_set(&addr, ESC_AL_STATE_INIT, &timeout) < 0)
printf("Could not go to state init\n");
/* mailbox read (slave->master) @0x1880 */
mb_rd.start = 0x1880;
mb_rd.len = 48;
mb_rd.sm = 1;
/* mailbox write (master->slave) @0x1800 */
mb_wr.start = 0x1800;
mb_wr.len = 48;
mb_wr.sm = 0;
mb = esc_mailbox_create(&addr, &mb_rd, &mb_wr);
esc_coe_create(mb);
if (block->private->dev->sm[block->private->dev->tx_pdo_sm].len != 3) {
log_send(LOG_T_ERROR, "TX PDO length mismatch");
}
if (block->private->dev->sm[block->private->dev->rx_pdo_sm].len != 5) {
log_send(LOG_T_ERROR, "RX PDO length mismatch");
}
block->private->tx_buffer = block->private->dev->tx_data;
block->private->rx_buffer = block->private->dev->rx_data;
can_dev = esc_coe_create(block->private->dev->mailbox);
block->private->can_dev = can_dev;
canopen_read_param(can_dev,
EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE,
&leval16, sizeof(leval16));
log_send(LOG_T_DEBUG, "mode: %d", le16toh(leval16));
canopen_read_param(can_dev,
EL5101_OBJ_SM_OUTPUT, 3,
&leval32, sizeof(leval32));
log_send(LOG_T_DEBUG, "cycle time: %d ns", le32toh(leval32));
leval32 = htole32(block->private->dev->dc_sync.sync0_cycle);
log_send(LOG_T_DEBUG, "cycle time: %d ns", le32toh(leval32));
canopen_write_param(can_dev,
EL5101_OBJ_SM_OUTPUT, 3,
&leval32, sizeof(leval32));
canopen_read_param(can_dev,
EL5101_OBJ_SM_OUTPUT, 4,
&leval16, sizeof(leval16));
log_send(LOG_T_DEBUG, "available modes: 0x%02x", le16toh(leval16));
/* switch device to dc synchronised mode */
leval16 = htole16(EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE_SYNC0);
canopen_write_param(can_dev,
EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE,
&leval16, sizeof(leval16));
canopen_read_param(can_dev,
EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE,
&leval16, sizeof(leval16));
log_send(LOG_T_DEBUG, "mode: %d", le16toh(leval16));
if (esc_al_state_set(&addr, ESC_AL_STATE_PRE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state pre operational\n");
if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state safe operational\n");
log_send(LOG_T_ERROR,
"%s: Could not go to state safe operational", name);
if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
printf("Could not go to state operational\n");
log_send(LOG_T_ERROR,
"%s: Could not go to state operational", name);
/* Inputs on the hardware are outputs on this software block */
if (controller_block_outterm_list_init(block, outterms))
......
......@@ -34,6 +34,9 @@
#include "ec.h"
#include "esc.h"
#include "esc_esi.h"
#include "esc_coe.h"
#include "esc_id.h"
#include "log.h"
struct controller_block_private {
......
/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007 - 2012
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007 - 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007 - 2012
This program is free software: you can redistribute it and/or modify
......@@ -83,7 +83,7 @@ static int ec_rx_pdo_add_time(void)
struct ec_dgram_hdr *prev;
struct unaligned_uint16_t *wkc;
// rx_pdo.rx_users = ec_slave_count();
rx_pdo.rx_users = ec_slave_dc_count();
dgram = (struct ec_dgram_hdr *)(rx_pdo.tx_frame + rx_pdo.flen);
rx_pdo.time = (struct unaligned_uint64_t *)
......@@ -250,10 +250,10 @@ int ec_tx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len)
static ssize_t ec_dgram(void *tx_frame, void *rx_frame, size_t len)
{
int ret;
ssize_t ret;
struct ec_dgram_hdr *rx_dgram1, *tx_dgram1;
static uint8_t dgram_idx = 0;
ssize_t minlen = sizeof(struct ec_frame_hdr)+sizeof(struct ec_dgram_hdr);
dgram_idx++;
tx_dgram1 = (struct ec_dgram_hdr *)
......@@ -271,10 +271,10 @@ static ssize_t ec_dgram(void *tx_frame, void *rx_frame, size_t len)
do {
ret = eth_recv(ec_sock, rx_frame, len);
if (ret < sizeof(struct ec_frame_hdr)+sizeof(struct ec_dgram_hdr))
if (ret < minlen)
return 1;