diff --git a/src/Kbuild b/src/Kbuild index bd1ac29..e98f9ec 100644 --- a/src/Kbuild +++ b/src/Kbuild @@ -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 \ diff --git a/src/lcec_conf.c b/src/lcec_conf.c index f88c8a3..b0e9ad1 100644 --- a/src/lcec_conf.c +++ b/src/lcec_conf.c @@ -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 }, diff --git a/src/lcec_conf.h b/src/lcec_conf.h index efffc7e..5d9943c 100644 --- a/src/lcec_conf.h +++ b/src/lcec_conf.h @@ -143,6 +143,8 @@ typedef enum { lcecSlaveTypeEL4132, lcecSlaveTypeEL4104, lcecSlaveTypeEL4134, + lcecSlaveTypeEL5002, + lcecSlaveTypeEL5032, lcecSlaveTypeEL5101, lcecSlaveTypeEL5151, lcecSlaveTypeEL5152, diff --git a/src/lcec_el5002.c b/src/lcec_el5002.c new file mode 100644 index 0000000..4cc24ba --- /dev/null +++ b/src/lcec_el5002.c @@ -0,0 +1,318 @@ +// +// Copyright (C) 2023 Sascha Ittner +// +// 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; ichans[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; ichans[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; +} + diff --git a/src/lcec_el5002.h b/src/lcec_el5002.h new file mode 100644 index 0000000..20c00aa --- /dev/null +++ b/src/lcec_el5002.h @@ -0,0 +1,50 @@ +// +// Copyright (C) 2023 Sascha Ittner +// +// 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 + diff --git a/src/lcec_el5032.c b/src/lcec_el5032.c new file mode 100644 index 0000000..5a7b5a6 --- /dev/null +++ b/src/lcec_el5032.c @@ -0,0 +1,248 @@ +// +// Copyright (C) 2023 Sascha Ittner +// +// 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; ichans[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; ichans[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; +} + diff --git a/src/lcec_el5032.h b/src/lcec_el5032.h new file mode 100644 index 0000000..a7a0038 --- /dev/null +++ b/src/lcec_el5032.h @@ -0,0 +1,32 @@ +// +// Copyright (C) 2023 Sascha Ittner +// +// 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 + diff --git a/src/lcec_main.c b/src/lcec_main.c index f5b8768..9b849f6 100644 --- a/src/lcec_main.c +++ b/src/lcec_main.c @@ -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},