nic/linuxcnc-ethercat
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added el5002 and el5032

Browse Source
This commit is contained in:
Sascha Ittner
2023-03-31 20:32:13 +02:00
parent a22b7f92a3
commit ed06a41f6a
8 changed files with 685 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/Kbuild
View File

@@ -20,6 +20,8 @@ lcec-objs := \
lcec_el40x8.o \
lcec_el41x2.o \
lcec_el41x4.o \
lcec_el5002.o \
lcec_el5032.o \
lcec_el5101.o \
lcec_el5151.o \
lcec_el5152.o \

29
src/lcec_conf.c
View File

@@ -34,6 +34,7 @@
#include "lcec_stmds5k.h"
#include "lcec_el6900.h"
#include "lcec_el1918_logic.h"
#include "lcec_el5002.h"
#include "lcec_el70x1.h"
#include "lcec_el7411.h"
#include "lcec_class_ax5.h"
@@ -121,6 +122,32 @@ static const LCEC_CONF_MODPARAM_DESC_T slaveAX5Params[] = {
{ NULL }
};
static const LCEC_CONF_MODPARAM_DESC_T slaveEL5002Params[] = {
{ "ch0DisFrameErr", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_DIS_FRAME_ERR, MODPARAM_TYPE_BIT } ,
{ "ch0EnPwrFailChk", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_EN_PWR_FAIL_CHK, MODPARAM_TYPE_BIT } ,
{ "ch0EnInhibitTime", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_EN_INHIBIT_TIME, MODPARAM_TYPE_BIT } ,
{ "ch0Coding", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_CODING, MODPARAM_TYPE_U32 } ,
{ "ch0Baudrate", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_BAUDRATE, MODPARAM_TYPE_U32 } ,
{ "ch0ClkJitComp", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_CLK_JIT_COMP, MODPARAM_TYPE_U32 } ,
{ "ch0FrameType", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_FRAME_TYPE, MODPARAM_TYPE_U32 } ,
{ "ch0FrameSize", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_FRAME_SIZE, MODPARAM_TYPE_U32 } ,
{ "ch0DataLen", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_DATA_LEN, MODPARAM_TYPE_U32 } ,
{ "ch0MinInhibitTime", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_MIN_INHIBIT_TIME, MODPARAM_TYPE_U32 } ,
{ "ch0NoClkBursts", LCEC_EL5002_PARAM_CH_0 || LCEC_EL5002_PARAM_NO_CLK_BURSTS, MODPARAM_TYPE_U32 } ,
{ "ch1DisFrameErr", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_DIS_FRAME_ERR, MODPARAM_TYPE_BIT } ,
{ "ch1EnPwrFailChk", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_EN_PWR_FAIL_CHK, MODPARAM_TYPE_BIT } ,
{ "ch1EnInhibitTime", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_EN_INHIBIT_TIME, MODPARAM_TYPE_BIT } ,
{ "ch1Coding", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_CODING, MODPARAM_TYPE_U32 } ,
{ "ch1Baudrate", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_BAUDRATE, MODPARAM_TYPE_U32 } ,
{ "ch1ClkJitComp", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_CLK_JIT_COMP, MODPARAM_TYPE_U32 } ,
{ "ch1FrameType", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_FRAME_TYPE, MODPARAM_TYPE_U32 } ,
{ "ch1FrameSize", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_FRAME_SIZE, MODPARAM_TYPE_U32 } ,
{ "ch1DataLen", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_DATA_LEN, MODPARAM_TYPE_U32 } ,
{ "ch1MinInhibitTime", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_MIN_INHIBIT_TIME, MODPARAM_TYPE_U32 } ,
{ "ch1NoClkBursts", LCEC_EL5002_PARAM_CH_1 || LCEC_EL5002_PARAM_NO_CLK_BURSTS, MODPARAM_TYPE_U32 } ,
{ NULL }
};
static const LCEC_CONF_TYPELIST_T slaveTypes[] = {
// bus coupler
{ "EK1100", lcecSlaveTypeEK1100, NULL },
@@ -234,6 +261,8 @@ static const LCEC_CONF_TYPELIST_T slaveTypes[] = {
{ "EL4038", lcecSlaveTypeEL4038, NULL },
// encoder inputs
{ "EL5002", lcecSlaveTypeEL5002, slaveEL5002Params },
{ "EL5032", lcecSlaveTypeEL5032, NULL },
{ "EL5101", lcecSlaveTypeEL5101, NULL },
{ "EL5151", lcecSlaveTypeEL5151, NULL },
{ "EL5152", lcecSlaveTypeEL5152, NULL },

2
src/lcec_conf.h
View File

@@ -143,6 +143,8 @@ typedef enum {
lcecSlaveTypeEL4132,
lcecSlaveTypeEL4104,
lcecSlaveTypeEL4134,
lcecSlaveTypeEL5002,
lcecSlaveTypeEL5032,
lcecSlaveTypeEL5101,
lcecSlaveTypeEL5151,
lcecSlaveTypeEL5152,

318
src/lcec_el5002.c Normal file
View File

@@ -0,0 +1,318 @@
//
// Copyright (C) 2023 Sascha Ittner <sascha.ittner@modusoft.de>
//
// 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 2 of the License, or
// (at your option) 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, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
#include "lcec.h"
#include "lcec_el5002.h"
typedef struct {
hal_bit_t *reset;
hal_bit_t *abs_mode;
hal_bit_t *err_data;
hal_bit_t *err_frame;
hal_bit_t *err_power;
hal_bit_t *err_sync;
hal_bit_t *tx_state;
hal_bit_t *tx_toggle;
hal_s32_t *raw_count;
hal_s32_t *count;
hal_float_t *pos;
hal_float_t *pos_scale;
unsigned int err_data_os;
unsigned int err_data_bp;
unsigned int err_frame_os;
unsigned int err_frame_bp;
unsigned int err_power_os;
unsigned int err_power_bp;
unsigned int err_sync_os;
unsigned int err_sync_bp;
unsigned int tx_state_os;
unsigned int tx_state_bp;
unsigned int tx_toggle_os;
unsigned int tx_toggle_bp;
unsigned int count_pdo_os;
int do_init;
int32_t last_count;
double old_scale;
double scale;
} lcec_el5002_chan_t;
typedef struct {
lcec_el5002_chan_t chans[LCEC_EL5002_CHANS];
int last_operational;
} lcec_el5002_data_t;
static const lcec_pindesc_t slave_pins[] = {
{ HAL_BIT, HAL_IN, offsetof(lcec_el5002_chan_t, reset), "%s.%s.%s.enc-%d-reset" },
{ HAL_BIT, HAL_IN, offsetof(lcec_el5002_chan_t, abs_mode), "%s.%s.%s.enc-%d-abs-mode" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5002_chan_t, err_data), "%s.%s.%s.enc-%d-err-data" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5002_chan_t, err_frame), "%s.%s.%s.enc-%d-err-frame" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5002_chan_t, err_power), "%s.%s.%s.enc-%d-err-power" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5002_chan_t, err_sync), "%s.%s.%s.enc-%d-err-sync" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5002_chan_t, tx_state), "%s.%s.%s.enc-%d-tx-state" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5002_chan_t, tx_toggle), "%s.%s.%s.enc-%d-tx-toggle" },
{ HAL_S32, HAL_OUT, offsetof(lcec_el5002_chan_t, raw_count), "%s.%s.%s.enc-%d-raw-count" },
{ HAL_S32, HAL_OUT, offsetof(lcec_el5002_chan_t, count), "%s.%s.%s.enc-%d-count" },
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_el5002_chan_t, pos), "%s.%s.%s.enc-%d-pos" },
{ HAL_FLOAT, HAL_IO, offsetof(lcec_el5002_chan_t, pos_scale), "%s.%s.%s.enc-%d-pos-scale" },
{ HAL_TYPE_UNSPECIFIED, HAL_DIR_UNSPECIFIED, -1, NULL }
};
static ec_pdo_entry_info_t lcec_el5002_channel1_in[] = {
{0x6000, 0x01, 1}, // Data error
{0x6000, 0x02, 1}, // Frame error
{0x6000, 0x03, 1}, // Power fail
{0x6000, 0x04, 1}, // ?
{0x0000, 0x00, 9}, // Gap
{0x6000, 0x0e, 1}, // Sync error
{0x6000, 0x0f, 1}, // TxPDO state
{0x6000, 0x10, 1}, // TxPDO toggle
{0x6000, 0x11, 32}, // counter
};
static ec_pdo_entry_info_t lcec_el5002_channel2_in[] = {
{0x6010, 0x01, 1}, // Data error
{0x6010, 0x02, 1}, // Frame error
{0x6010, 0x03, 1}, // Power fail
{0x6010, 0x04, 1}, // ?
{0x0010, 0x00, 9}, // Gap
{0x6010, 0x0e, 1}, // Sync error
{0x6010, 0x0f, 1}, // TxPDO state
{0x6010, 0x10, 1}, // TxPDO toggle
{0x6010, 0x11, 32}, // counter
};
static ec_pdo_info_t lcec_el5002_pdos_in[] = {
{0x1A00, 9, lcec_el5002_channel1_in},
{0x1A01, 9, lcec_el5002_channel2_in}
};
static ec_sync_info_t lcec_el5002_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL},
{1, EC_DIR_INPUT, 0, NULL},
{2, EC_DIR_OUTPUT, 0, NULL},
{3, EC_DIR_INPUT, 2, lcec_el5002_pdos_in},
{0xff}
};
void lcec_el5002_read(struct lcec_slave *slave, long period);
int lcec_el5002_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) {
lcec_master_t *master = slave->master;
lcec_slave_modparam_t *p;
lcec_el5002_data_t *hal_data;
int i;
lcec_el5002_chan_t *chan;
int err;
// set config patameters
for (p = slave->modparams; p != NULL && p->id >= 0; p++) {
// get channel offset
i = (p->id & LCEC_EL5002_PARAM_CH_MASK) << 4;
switch(p->id & LCEC_EL5002_PARAM_FNK_MASK) {
case LCEC_EL5002_PARAM_DIS_FRAME_ERR:
if (ecrt_slave_config_sdo8(slave->config, 0x8000 + i, 0x01, p->value.bit) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo DisFrameErr\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_EN_PWR_FAIL_CHK:
if (ecrt_slave_config_sdo8(slave->config, 0x8000 + i, 0x02, p->value.bit) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo EnPwrFailChk\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_EN_INHIBIT_TIME:
if (ecrt_slave_config_sdo8(slave->config, 0x8000 + i, 0x03, p->value.bit) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo EnInhibitTime\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_CODING:
if (ecrt_slave_config_sdo8(slave->config, 0x8000 + i, 0x06, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo Coding\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_BAUDRATE:
if (ecrt_slave_config_sdo8(slave->config, 0x8000 + i, 0x09, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo Baudrate\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_CLK_JIT_COMP:
if (ecrt_slave_config_sdo8(slave->config, 0x8000 + i, 0x0c, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo ClkJitComp\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_FRAME_TYPE:
if (ecrt_slave_config_sdo8(slave->config, 0x8000 + i, 0x0f, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo FrameType\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_FRAME_SIZE:
if (ecrt_slave_config_sdo16(slave->config, 0x8000 + i, 0x11, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo FrameSize\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_DATA_LEN:
if (ecrt_slave_config_sdo16(slave->config, 0x8000 + i, 0x12, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo DataLen\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_MIN_INHIBIT_TIME:
if (ecrt_slave_config_sdo16(slave->config, 0x8000 + i, 0x13, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo MinInhibitTime\n", master->name, slave->name);
return -1;
}
break;
case LCEC_EL5002_PARAM_NO_CLK_BURSTS:
if (ecrt_slave_config_sdo16(slave->config, 0x8000 + i, 0x14, p->value.u32) != 0) {
rtapi_print_msg (RTAPI_MSG_ERR, LCEC_MSG_PFX "fail to configure slave %s.%s sdo NoClkBursts\n", master->name, slave->name);
return -1;
}
break;
}
}
// initialize callbacks
slave->proc_read = lcec_el5002_read;
// alloc hal memory
if ((hal_data = hal_malloc(sizeof(lcec_el5002_data_t))) == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name);
return -EIO;
}
memset(hal_data, 0, sizeof(lcec_el5002_data_t));
slave->hal_data = hal_data;
// initialize sync info
slave->sync_info = lcec_el5002_syncs;
// initialize global data
hal_data->last_operational = 0;
// initialize pins
for (i=0; i<LCEC_EL5002_CHANS; i++) {
chan = &hal_data->chans[i];
// initialize POD entries
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x01, &chan->err_data_os, &chan->err_data_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x02, &chan->err_frame_os, &chan->err_frame_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x03, &chan->err_power_os, &chan->err_power_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0e, &chan->err_sync_os, &chan->err_sync_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0f, &chan->tx_state_os, &chan->tx_state_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x10, &chan->tx_toggle_os, &chan->tx_toggle_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x11, &chan->count_pdo_os, NULL);
// export pins
if ((err = lcec_pin_newf_list(chan, slave_pins, LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
return err;
}
// initialize pins
*(chan->pos_scale) = 1.0;
// initialize variables
chan->do_init = 1;
chan->last_count = 0;
chan->old_scale = *(chan->pos_scale) + 1.0;
chan->scale = 1.0;
}
return 0;
}
void lcec_el5002_read(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el5002_data_t *hal_data = (lcec_el5002_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int i;
lcec_el5002_chan_t *chan;
int32_t raw_count, raw_delta;
// wait for slave to be operational
if (!slave->state.operational) {
hal_data->last_operational = 0;
return;
}
// check inputs
for (i=0; i<LCEC_EL5002_CHANS; i++) {
chan = &hal_data->chans[i];
// check for change in scale value
if (*(chan->pos_scale) != chan->old_scale) {
// scale value has changed, test and update it
if ((*(chan->pos_scale) < 1e-20) && (*(chan->pos_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(chan->pos_scale) = 1.0;
}
// save new scale to detect future changes
chan->old_scale = *(chan->pos_scale);
// we actually want the reciprocal
chan->scale = 1.0 / *(chan->pos_scale);
}
// get bit states
*(chan->err_data) = EC_READ_BIT(&pd[chan->err_data_os], chan->err_data_bp);
*(chan->err_frame) = EC_READ_BIT(&pd[chan->err_frame_os], chan->err_frame_bp);
*(chan->err_power) = EC_READ_BIT(&pd[chan->err_power_os], chan->err_power_bp);
*(chan->err_sync) = EC_READ_BIT(&pd[chan->err_sync_os], chan->err_sync_bp);
*(chan->tx_state) = EC_READ_BIT(&pd[chan->tx_state_os], chan->tx_state_bp);
*(chan->tx_toggle) = EC_READ_BIT(&pd[chan->tx_toggle_os], chan->tx_toggle_bp);
// read raw values
raw_count = EC_READ_S32(&pd[chan->count_pdo_os]);
// check for operational change of slave
if (!hal_data->last_operational) {
chan->last_count = raw_count;
}
// update raw values
*(chan->raw_count) = raw_count;
// handle initialization
if (chan->do_init || *(chan->reset)) {
chan->do_init = 0;
chan->last_count = raw_count;
*(chan->count) = 0;
}
// compute net counts
raw_delta = raw_count - chan->last_count;
chan->last_count = raw_count;
*(chan->count) += raw_delta;
// scale count to make floating point position
if (*(chan->abs_mode)) {
*(chan->pos) = *(chan->raw_count) * chan->scale;
} else {
*(chan->pos) = *(chan->count) * chan->scale;
}
}
hal_data->last_operational = 1;
}

50
src/lcec_el5002.h Normal file
View File

@@ -0,0 +1,50 @@
//
// Copyright (C) 2023 Sascha Ittner <sascha.ittner@modusoft.de>
//
// 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 2 of the License, or
// (at your option) 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, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
#ifndef _LCEC_EL5002_H_
#define _LCEC_EL5002_H_
#include "lcec.h"
#define LCEC_EL5002_VID LCEC_BECKHOFF_VID
#define LCEC_EL5002_PID 0x138a3052
#define LCEC_EL5002_CHANS 2
#define LCEC_EL5002_PDOS (7 * LCEC_EL5002_CHANS)
#define LCEC_EL5002_PARAM_CH_MASK 0x000f
#define LCEC_EL5002_PARAM_FNK_MASK 0xfff0
#define LCEC_EL5002_PARAM_CH_0 0x0000
#define LCEC_EL5002_PARAM_CH_1 0x0001
#define LCEC_EL5002_PARAM_DIS_FRAME_ERR 0x0010
#define LCEC_EL5002_PARAM_EN_PWR_FAIL_CHK 0x0020
#define LCEC_EL5002_PARAM_EN_INHIBIT_TIME 0x0030
#define LCEC_EL5002_PARAM_CODING 0x0040
#define LCEC_EL5002_PARAM_BAUDRATE 0x0050
#define LCEC_EL5002_PARAM_CLK_JIT_COMP 0x0060
#define LCEC_EL5002_PARAM_FRAME_TYPE 0x0070
#define LCEC_EL5002_PARAM_FRAME_SIZE 0x0080
#define LCEC_EL5002_PARAM_DATA_LEN 0x0090
#define LCEC_EL5002_PARAM_MIN_INHIBIT_TIME 0x00a0
#define LCEC_EL5002_PARAM_NO_CLK_BURSTS 0x00b0
int lcec_el5002_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs);
#endif

248
src/lcec_el5032.c Normal file
View File

@@ -0,0 +1,248 @@
//
// Copyright (C) 2023 Sascha Ittner <sascha.ittner@modusoft.de>
//
// 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 2 of the License, or
// (at your option) 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, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
#include "lcec.h"
#include "lcec_el5032.h"
typedef struct {
hal_bit_t *reset;
hal_bit_t *abs_mode;
hal_bit_t *warn;
hal_bit_t *error;
hal_bit_t *ready;
hal_bit_t *diag;
hal_bit_t *tx_state;
hal_u32_t *cyc_cnt;
hal_u32_t *raw_count_lo;
hal_u32_t *raw_count_hi;
hal_s32_t *count;
hal_float_t *pos;
hal_float_t *pos_scale;
unsigned int warn_os;
unsigned int warn_bp;
unsigned int error_os;
unsigned int error_bp;
unsigned int ready_os;
unsigned int ready_bp;
unsigned int diag_os;
unsigned int diag_bp;
unsigned int tx_state_os;
unsigned int tx_state_bp;
unsigned int cyc_cnt_os;
unsigned int cyc_cnt_bp;
unsigned int count_pdo_os;
int do_init;
int64_t last_count;
double old_scale;
double scale;
} lcec_el5032_chan_t;
typedef struct {
lcec_el5032_chan_t chans[LCEC_EL5032_CHANS];
int last_operational;
} lcec_el5032_data_t;
static const lcec_pindesc_t slave_pins[] = {
{ HAL_BIT, HAL_IN, offsetof(lcec_el5032_chan_t, reset), "%s.%s.%s.enc-%d-reset" },
{ HAL_BIT, HAL_IN, offsetof(lcec_el5032_chan_t, abs_mode), "%s.%s.%s.enc-%d-abs-mode" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5032_chan_t, warn), "%s.%s.%s.enc-%d-warn" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5032_chan_t, error), "%s.%s.%s.enc-%d-error" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5032_chan_t, ready), "%s.%s.%s.enc-%d-ready" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5032_chan_t, diag), "%s.%s.%s.enc-%d-diag" },
{ HAL_BIT, HAL_OUT, offsetof(lcec_el5032_chan_t, tx_state), "%s.%s.%s.enc-%d-tx-state" },
{ HAL_S32, HAL_OUT, offsetof(lcec_el5032_chan_t, cyc_cnt), "%s.%s.%s.enc-%d-cyc-cnt" },
{ HAL_S32, HAL_OUT, offsetof(lcec_el5032_chan_t, raw_count_lo), "%s.%s.%s.enc-%d-raw-count-lo" },
{ HAL_S32, HAL_OUT, offsetof(lcec_el5032_chan_t, raw_count_hi), "%s.%s.%s.enc-%d-raw-count-hi" },
{ HAL_S32, HAL_OUT, offsetof(lcec_el5032_chan_t, count), "%s.%s.%s.enc-%d-count" },
{ HAL_FLOAT, HAL_OUT, offsetof(lcec_el5032_chan_t, pos), "%s.%s.%s.enc-%d-pos" },
{ HAL_FLOAT, HAL_IO, offsetof(lcec_el5032_chan_t, pos_scale), "%s.%s.%s.enc-%d-pos-scale" },
{ HAL_TYPE_UNSPECIFIED, HAL_DIR_UNSPECIFIED, -1, NULL }
};
static ec_pdo_entry_info_t lcec_el5032_channel1_in[] = {
{0x6000, 0x01, 1}, // warning
{0x6000, 0x02, 1}, // error
{0x6000, 0x03, 1}, // ready
{0x0000, 0x00, 5}, // Gap
{0x0000, 0x00, 4}, // Gap
{0x6000, 0x0d, 1}, // diag
{0x6000, 0x0e, 1}, // TxPDO state
{0x6000, 0x0f, 2}, // cycle counter
{0x6000, 0x11, 64}, // counter
};
static ec_pdo_entry_info_t lcec_el5032_channel2_in[] = {
{0x6010, 0x01, 1}, // warning
{0x6010, 0x02, 1}, // error
{0x6010, 0x03, 1}, // ready
{0x0000, 0x00, 5}, // Gap
{0x0000, 0x00, 4}, // Gap
{0x6010, 0x0d, 1}, // diag
{0x6010, 0x0e, 1}, // TxPDO state
{0x6010, 0x0f, 2}, // cycle counter
{0x6010, 0x11, 64}, // counter
};
static ec_pdo_info_t lcec_el5032_pdos_in[] = {
{0x1A00, 9, lcec_el5032_channel1_in},
{0x1A01, 9, lcec_el5032_channel2_in}
};
static ec_sync_info_t lcec_el5032_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL},
{1, EC_DIR_INPUT, 0, NULL},
{2, EC_DIR_OUTPUT, 0, NULL},
{3, EC_DIR_INPUT, 2, lcec_el5032_pdos_in},
{0xff}
};
void lcec_el5032_read(struct lcec_slave *slave, long period);
int lcec_el5032_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) {
lcec_master_t *master = slave->master;
lcec_el5032_data_t *hal_data;
int i;
lcec_el5032_chan_t *chan;
int err;
// initialize callbacks
slave->proc_read = lcec_el5032_read;
// alloc hal memory
if ((hal_data = hal_malloc(sizeof(lcec_el5032_data_t))) == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name);
return -EIO;
}
memset(hal_data, 0, sizeof(lcec_el5032_data_t));
slave->hal_data = hal_data;
// initialize sync info
slave->sync_info = lcec_el5032_syncs;
// initialize global data
hal_data->last_operational = 0;
// initialize pins
for (i=0; i<LCEC_EL5032_CHANS; i++) {
chan = &hal_data->chans[i];
// initialize POD entries
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x01, &chan->warn_os, &chan->warn_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x02, &chan->error_os, &chan->error_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x03, &chan->ready_os, &chan->ready_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0d, &chan->diag_os, &chan->diag_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0e, &chan->tx_state_os, &chan->tx_state_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0f, &chan->cyc_cnt_os, &chan->cyc_cnt_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x11, &chan->count_pdo_os, NULL);
// export pins
if ((err = lcec_pin_newf_list(chan, slave_pins, LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
return err;
}
// initialize pins
*(chan->pos_scale) = 1.0;
// initialize variables
chan->do_init = 1;
chan->last_count = 0;
chan->old_scale = *(chan->pos_scale) + 1.0;
chan->scale = 1.0;
}
return 0;
}
void lcec_el5032_read(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el5032_data_t *hal_data = (lcec_el5032_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int i;
lcec_el5032_chan_t *chan;
int64_t raw_count, raw_delta;
// wait for slave to be operational
if (!slave->state.operational) {
hal_data->last_operational = 0;
return;
}
// check inputs
for (i=0; i<LCEC_EL5032_CHANS; i++) {
chan = &hal_data->chans[i];
// check for change in scale value
if (*(chan->pos_scale) != chan->old_scale) {
// scale value has changed, test and update it
if ((*(chan->pos_scale) < 1e-20) && (*(chan->pos_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(chan->pos_scale) = 1.0;
}
// save new scale to detect future changes
chan->old_scale = *(chan->pos_scale);
// we actually want the reciprocal
chan->scale = 1.0 / *(chan->pos_scale);
}
// get bit states
*(chan->warn) = EC_READ_BIT(&pd[chan->warn_os], chan->warn_bp);
*(chan->error) = EC_READ_BIT(&pd[chan->error_os], chan->error_bp);
*(chan->ready) = EC_READ_BIT(&pd[chan->ready_os], chan->ready_bp);
*(chan->diag) = EC_READ_BIT(&pd[chan->diag_os], chan->diag_bp);
*(chan->tx_state) = EC_READ_BIT(&pd[chan->tx_state_os], chan->tx_state_bp);
// get cycle counter
*(chan->cyc_cnt) = (EC_READ_U8(&pd[chan->cyc_cnt_os]) >> chan->cyc_cnt_bp) & 0x03;
// read raw values
raw_count = EC_READ_S64(&pd[chan->count_pdo_os]);
// check for operational change of slave
if (!hal_data->last_operational) {
chan->last_count = raw_count;
}
// update raw values
*(chan->raw_count_lo) = raw_count;
*(chan->raw_count_hi) = raw_count >> 32;
// handle initialization
if (chan->do_init || *(chan->reset)) {
chan->do_init = 0;
chan->last_count = raw_count;
*(chan->count) = 0;
}
// compute net counts
raw_delta = raw_count - chan->last_count;
chan->last_count = raw_count;
*(chan->count) += raw_delta;
// scale count to make floating point position
if (*(chan->abs_mode)) {
*(chan->pos) = raw_count * chan->scale;
} else {
*(chan->pos) = *(chan->count) * chan->scale;
}
}
hal_data->last_operational = 1;
}

32
src/lcec_el5032.h Normal file
View File

@@ -0,0 +1,32 @@
//
// Copyright (C) 2023 Sascha Ittner <sascha.ittner@modusoft.de>
//
// 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 2 of the License, or
// (at your option) 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, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
#ifndef _LCEC_EL5032_H_
#define _LCEC_EL5032_H_
#include "lcec.h"
#define LCEC_EL5032_VID LCEC_BECKHOFF_VID
#define LCEC_EL5032_PID 0x13a83052
#define LCEC_EL5032_CHANS 2
#define LCEC_EL5032_PDOS (7 * LCEC_EL5032_CHANS)
int lcec_el5032_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs);
#endif

4
src/lcec_main.c
View File

@@ -34,6 +34,8 @@
#include "lcec_el40x8.h"
#include "lcec_el41x2.h"
#include "lcec_el41x4.h"
#include "lcec_el5002.h"
#include "lcec_el5032.h"
#include "lcec_el5101.h"
#include "lcec_el5151.h"
#include "lcec_el5152.h"
@@ -182,6 +184,8 @@ static const lcec_typelist_t types[] = {
{ lcecSlaveTypeEL4038, LCEC_EL40x8_VID, LCEC_EL4038_PID, LCEC_EL40x8_PDOS, 0, NULL, lcec_el40x8_init},
// encoder inputs
{ lcecSlaveTypeEL5002, LCEC_EL5002_VID, LCEC_EL5002_PID, LCEC_EL5002_PDOS, 0, NULL, lcec_el5002_init},
{ lcecSlaveTypeEL5032, LCEC_EL5032_VID, LCEC_EL5032_PID, LCEC_EL5032_PDOS, 0, NULL, lcec_el5032_init},
{ lcecSlaveTypeEL5101, LCEC_EL5101_VID, LCEC_EL5101_PID, LCEC_EL5101_PDOS, 0, NULL, lcec_el5101_init},
{ lcecSlaveTypeEL5151, LCEC_EL5151_VID, LCEC_EL5151_PID, LCEC_EL5151_PDOS, 0, NULL, lcec_el5151_init},
{ lcecSlaveTypeEL5152, LCEC_EL5152_VID, LCEC_EL5152_PID, LCEC_EL5152_PDOS, 0, NULL, lcec_el5152_init},