added EL1859

2020-06-04 19:24:43 +02:00
parent 23970090bd
commit a7d796fa9e
6 changed files with 167 additions and 0 deletions
--- a/src/lcec_el1859.c
+++ b/src/lcec_el1859.c
@@ -0,0 +1,125 @@
+//
+//    Copyright (C) 2011 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_el1859.h"
+
+typedef struct {
+  hal_bit_t *in;
+  hal_bit_t *in_not;
+  hal_bit_t *out;
+  hal_bit_t invert;
+  unsigned int pdo_in_os;
+  unsigned int pdo_in_bp;
+  unsigned int pdo_out_os;
+  unsigned int pdo_out_bp;
+
+} lcec_el1859_pin_t;
+
+static const lcec_pindesc_t slave_pins[] = {
+  { HAL_BIT, HAL_OUT, offsetof(lcec_el1859_pin_t, in), "%s.%s.%s.din-%d" },
+  { HAL_BIT, HAL_OUT, offsetof(lcec_el1859_pin_t, in_not), "%s.%s.%s.din-%d-not" },
+  { HAL_BIT, HAL_IN, offsetof(lcec_el1859_pin_t, out), "%s.%s.%s.dout-%d" },
+  { HAL_TYPE_UNSPECIFIED, HAL_DIR_UNSPECIFIED, -1, NULL }
+};
+
+static const lcec_pindesc_t slave_params[] = {
+  { HAL_BIT, HAL_RW, offsetof(lcec_el1859_pin_t, invert), "%s.%s.%s.dout-%d-invert" },
+  { HAL_TYPE_UNSPECIFIED, HAL_DIR_UNSPECIFIED, -1, NULL }
+};
+
+
+void lcec_el1859_read(struct lcec_slave *slave, long period);
+void lcec_el1859_write(struct lcec_slave *slave, long period);
+
+int lcec_el1859_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) {
+  lcec_master_t *master = slave->master;
+  lcec_el1859_pin_t *hal_data;
+  lcec_el1859_pin_t *pin;
+  int i;
+  int err;
+
+  // initialize callbacks
+  slave->proc_read = lcec_el1859_read;
+  slave->proc_write = lcec_el1859_write;
+
+  // alloc hal memory
+  if ((hal_data = hal_malloc(sizeof(lcec_el1859_pin_t) * LCEC_EL1859_PINS)) == 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_el1859_pin_t) * LCEC_EL1859_PINS);
+  slave->hal_data = hal_data;
+
+  // initialize pins
+  for (i=0, pin=hal_data; i<LCEC_EL1859_PINS; i++, pin++) {
+    // initialize POD entry
+    LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x01, &pin->pdo_in_os, &pin->pdo_in_bp);
+    LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7080 + (i << 4), 0x01, &pin->pdo_out_os, &pin->pdo_out_bp);
+
+    // export pins
+    if ((err = lcec_pin_newf_list(pin, slave_pins, LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
+      return err;
+    }
+
+    // export parameters
+    if ((err = lcec_param_newf_list(pin, slave_params, LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
+      return err;
+    }
+  }
+
+  return 0;
+}
+
+void lcec_el1859_read(struct lcec_slave *slave, long period) {
+  lcec_master_t *master = slave->master;
+  lcec_el1859_pin_t *hal_data = (lcec_el1859_pin_t *) slave->hal_data;
+  uint8_t *pd = master->process_data;
+  lcec_el1859_pin_t *pin;
+  int i, s;
+
+  // wait for slave to be operational
+  if (!slave->state.operational) {
+    return;
+  }
+
+  // check inputs
+  for (i=0, pin=hal_data; i<LCEC_EL1859_PINS; i++, pin++) {
+    s = EC_READ_BIT(&pd[pin->pdo_in_os], pin->pdo_in_bp);
+    *(pin->in) = s;
+    *(pin->in_not) = !s;
+  }
+}
+
+void lcec_el1859_write(struct lcec_slave *slave, long period) {
+  lcec_master_t *master = slave->master;
+  lcec_el1859_pin_t *hal_data = (lcec_el1859_pin_t *) slave->hal_data;
+  uint8_t *pd = master->process_data;
+  lcec_el1859_pin_t *pin;
+  int i, s;
+
+  // set outputs
+  for (i=0, pin=hal_data; i<LCEC_EL1859_PINS; i++, pin++) {
+    s = *(pin->out);
+    if (pin->invert) {
+      s = !s;
+    }
+    EC_WRITE_BIT(&pd[pin->pdo_out_os], pin->pdo_out_bp, s);
+  }
+}
+