Adding the el5042 driver

src/CMakeLists.txt

@@ -23,6 +23,7 @@ add_library(jsd-lib STATIC
     jsd_el3318.c
     jsd_el3162.c
     jsd_el4102.c
+    jsd_el5042.c
     jsd_ild1900.c
     jsd_epd_nominal.c
     jsd_epd_sil.c

src/jsd.c

@@ -19,6 +19,7 @@
 #include "jsd/jsd_el3356.h"
 #include "jsd/jsd_el3602.h"
 #include "jsd/jsd_el4102.h"
+#include "jsd/jsd_el5042.h"
 #include "jsd/jsd_epd_nominal.h"
 #include "jsd/jsd_epd_sil.h"
 #include "jsd/jsd_ild1900.h"
@@ -389,6 +390,8 @@ const char* jsd_driver_type_to_string(jsd_driver_type_t driver_type) {
       return "JSD_DRIVER_TYPE_EL3602";
     case JSD_DRIVER_TYPE_EL4102:
       return "JSD_DRIVER_TYPE_EL4102";
+    case JSD_DRIVER_TYPE_EL5042:
+      return "JSD_DRIVER_TYPE_EL5042";
     case JSD_DRIVER_TYPE_EPD_NOMINAL:
       return "JSD_DRIVER_TYPE_EPD_NOMINAL";
     case JSD_DRIVER_TYPE_EPD_SIL:
@@ -509,6 +512,9 @@ bool jsd_driver_is_compatible_with_product_code(jsd_driver_type_t driver_type,
     case JSD_DRIVER_TYPE_EL4102:
       is_compatible = jsd_el4102_product_code_is_compatible(product_code);
       break;
+    case JSD_DRIVER_TYPE_EL5042:
+      is_compatible = jsd_el5042_product_code_is_compatible(product_code);
+      break;
     case JSD_DRIVER_TYPE_ILD1900:
       is_compatible = jsd_ild1900_product_code_is_compatible(product_code);
       break;
@@ -576,6 +582,9 @@ bool jsd_init_single_device(jsd_t* self, uint16_t slave_id) {
     case JSD_DRIVER_TYPE_EL4102:
       return jsd_el4102_init(self, slave_id);
       break;
+    case JSD_DRIVER_TYPE_EL5042:
+      return jsd_el5042_init(self, slave_id);
+      break;
     case JSD_DRIVER_TYPE_ILD1900:
       return jsd_ild1900_init(self, slave_id);
       break;

src/jsd_el5042.c

@@ -0,0 +1,129 @@
+#include "jsd/jsd_el5042.h"
+
+#include <assert.h>
+#include <string.h>
+
+#include "jsd/jsd_sdo.h"
+
+/****************************************************
+ * Public functions
+ ****************************************************/
+
+const jsd_el5042_state_t* jsd_el5042_get_state(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(jsd_el5042_product_code_is_compatible(
+      self->ecx_context.slavelist[slave_id].eep_id));
+
+  jsd_el5042_state_t* state = &self->slave_states[slave_id].el5042;
+  return state;
+}
+
+void jsd_el5042_read(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(jsd_el5042_product_code_is_compatible(
+      self->ecx_context.slavelist[slave_id].eep_id));
+
+  jsd_el5042_state_t* state = &self->slave_states[slave_id].el5042;
+
+  const jsd_el5042_txpdo_t* txpdo =
+      (jsd_el5042_txpdo_t*)self->ecx_context.slavelist[slave_id].inputs;
+
+  for (int ch = 0; ch < JSD_EL5042_NUM_CHANNELS; ++ch) {
+    state->position[ch] = txpdo->channel[ch].position;
+
+    state->warning[ch]             = (txpdo->channel[ch].status >> 0) & 0x01;
+    state->error[ch]               = (txpdo->channel[ch].status >> 1) & 0x01;
+    state->ready[ch]               = (txpdo->channel[ch].status >> 2) & 0x01;
+    state->diag[ch]                = (txpdo->channel[ch].status >> 4) & 0x01;
+    state->txpdo_state[ch]         = (txpdo->channel[ch].status >> 5) & 0x01;
+    state->input_cycle_counter[ch] = (txpdo->channel[ch].status >> 6) & 0x03;
+  }
+}
+
+/****************************************************
+ * Private functions
+ ****************************************************/
+
+bool jsd_el5042_init(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(jsd_el5042_product_code_is_compatible(
+      self->ecx_context.slavelist[slave_id].eep_id));
+  assert(self->ecx_context.slavelist[slave_id].eep_man ==
+         JSD_BECKHOFF_VENDOR_ID);
+
+  ec_slavet* slaves = self->ecx_context.slavelist;
+  ec_slavet* slave  = &slaves[slave_id];
+
+  slave->PO2SOconfigx = jsd_el5042_PO2SO_config;
+
+  return true;
+}
+
+int jsd_el5042_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id) {
+  assert(ecx_context);
+  assert(jsd_el5042_product_code_is_compatible(
+      ecx_context->slavelist[slave_id].eep_id));
+
+  // Since this function prototype is forced by SOEM, we have embedded a
+  // reference to jsd.slave_configs within the ecx_context and extract it here.
+  jsd_slave_config_t* slave_configs =
+      (jsd_slave_config_t*)ecx_context->userdata;
+
+  jsd_slave_config_t* config = &slave_configs[slave_id];
+
+  for (int ch = 0; ch < JSD_EL5042_NUM_CHANNELS; ++ch) {
+    // Index for settings is 0x80n8, where n is channel number (e.g. ch2 =
+    // 0x8018).
+    uint32_t sdo_channel_index = 0x8008 + (0x10 * ch);
+
+    // Set the encoder supply voltage, either 50 for 5V or 90 for 9V.
+    uint8_t supply_voltage = 50; // 5V (default)
+    if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
+                                    0x12, JSD_SDO_DATA_U8, &supply_voltage)) {
+      return 0;
+    }
+
+    // Set the CRC inversion
+    uint8_t CRC_invert = 1; // True correspond to CRC transmitted inverted
+    if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
+                                    0x03, JSD_SDO_DATA_U8, &CRC_invert)) {
+      return 0;
+    }
+
+    // Set the BiSS clock frequency.
+    // 0 -> 10 MHz
+    // 1 -> 5 MHz
+    // 2 -> 3.33 MHz
+    // 3 -> 2.5 MHz
+    // 4 -> 2 MHz
+    // 9 -> 1 MHz
+    // 17 -> 500 kHz
+    // 19 -> 250 kHz
+    uint8_t clock_frequency = 1; // 5 MHz
+    if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
+                                    0x13, JSD_SDO_DATA_U8, &clock_frequency)) {
+      return 0;
+    }
+
+    // Set the number of multiturn bits
+    uint8_t multiturn_bits = 0;
+    if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
+                                    0x15, JSD_SDO_DATA_U8, &multiturn_bits)) {
+      return 0;
+    }
+
+    // Set the number of singleturn bits
+    uint8_t singleturn_bits = 19;
+    if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
+                                    0x16, JSD_SDO_DATA_U8, &singleturn_bits)) {
+      return 0;
+    }
+  }
+
+  config->PO2SO_success = true;
+  return 1;
+}
+
+bool jsd_el5042_product_code_is_compatible(uint32_t product_code) {
+  return product_code == JSD_EL5042_PRODUCT_CODE;
+}

src/jsd_el5042.h

@@ -0,0 +1,60 @@
+#ifndef JSD_EL5042_H
+#define JSD_EL5042_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd.h"
+
+/**
+ * @brief Single channel of TxPDO data struct
+ *
+ * Note: struct order matters and must be packed.
+ */
+typedef struct __attribute__((__packed__)) {
+  uint16_t status;
+  int64_t position;
+} jsd_el5042_txpdo_channel_t;
+
+/**
+ * @brief TxPDO struct used to read device data in SOEM IOmap
+ *
+ * Note: Struct order matters and must be packed.
+ */
+typedef struct __attribute__((__packed__)) {
+  jsd_el5042_txpdo_channel_t channel[JSD_EL5042_NUM_CHANNELS];
+} jsd_el5042_txpdo_t;
+
+/**
+ * @brief Intializes EL5042 and registers the PO2SO function
+ *
+ * @param self Pointer to JSD context
+ * @param slave_id Index of device on EtherCAT bus
+ * @return true on success, false on failure
+ */
+bool jsd_el5042_init(jsd_t* self, uint16_t slave_id);
+
+/**
+ * @brief Configuration function called by SOEM upon a PreOp to SafeOp state
+ * transition that (re)configures EL5042 device settings
+ *
+ * @param ecx_context SOEM context pointer
+ * @param slave_id Index of device on EtherCAT bus
+ * @return 1 on success, 0 on failure
+ */
+int jsd_el5042_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id);
+
+/**
+ * @brief Checks whether a product code is compatible with EL5042.
+ *
+ * @param product_code The product code to be checked
+ * @return True if the product code is compatible, false otherwise.
+ */
+bool jsd_el5042_product_code_is_compatible(uint32_t product_code);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_el5042_pub.h

@@ -0,0 +1,31 @@
+#ifndef JSD_EL5042_PUB_H
+#define JSD_EL5042_PUB_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_pub.h"
+
+/**
+ * @brief Read the EL5042 device state
+ *
+ * @param self Pointer to JSD context
+ * @param slave_id Slave ID of EL5042 device
+ * @return Pointer to EL5042 device state
+ */
+const jsd_el5042_state_t* jsd_el5042_get_state(jsd_t* self, uint16_t slave_id);
+
+/**
+ * @brief Converts raw PDO data to state data
+ *
+ * @param self pointer to JSD context
+ * @param slave_id ID of EL5042 device
+ */
+void jsd_el5042_read(jsd_t* self, uint16_t slave_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_el5042_types.h

@@ -0,0 +1,36 @@
+#ifndef JSD_EL5042_TYPES_H
+#define JSD_EL5042_TYPES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_common_device_types.h"
+
+#define JSD_EL5042_PRODUCT_CODE (uint32_t)0x13b23052
+#define JSD_EL5042_NUM_CHANNELS 2
+
+/**
+ * @brief Configuration struct for EL5042 device initialization
+ */
+typedef struct {
+} jsd_el5042_config_t;
+
+/**
+ * @brief Read struct for EL5042 device
+ */
+typedef struct {
+  int64_t position[JSD_EL5042_NUM_CHANNELS];  ///< Position in counts
+  uint8_t warning[JSD_EL5042_NUM_CHANNELS];
+  uint8_t error[JSD_EL5042_NUM_CHANNELS];
+  uint8_t ready[JSD_EL5042_NUM_CHANNELS];
+  uint8_t diag[JSD_EL5042_NUM_CHANNELS];
+  uint8_t txpdo_state[JSD_EL5042_NUM_CHANNELS];
+  uint8_t input_cycle_counter[JSD_EL5042_NUM_CHANNELS];
+} jsd_el5042_state_t;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_types.h

@@ -22,6 +22,7 @@ extern "C" {
 #include "jsd/jsd_el3356_types.h"
 #include "jsd/jsd_el3602_types.h"
 #include "jsd/jsd_el4102_types.h"
+#include "jsd/jsd_el5042_types.h"
 #include "jsd/jsd_epd_nominal_types.h"
 #include "jsd/jsd_epd_sil_types.h"
 #include "jsd/jsd_error_cirq.h"
@@ -43,6 +44,7 @@ typedef enum {
   JSD_DRIVER_TYPE_EL3356,
   JSD_DRIVER_TYPE_EL3602,
   JSD_DRIVER_TYPE_EL4102,
+  JSD_DRIVER_TYPE_EL5042,
   JSD_DRIVER_TYPE_EPD_NOMINAL,
   JSD_DRIVER_TYPE_EPD_SIL,
   JSD_DRIVER_TYPE_ILD1900,
@@ -70,6 +72,7 @@ typedef struct {
     jsd_el3318_config_t  el3318;
     jsd_el3162_config_t  el3162;
     jsd_el4102_config_t  el4102;
+    jsd_el5042_config_t  el5042;
     jsd_ild1900_config_t ild1900;
     jsd_epd_nominal_config_t epd_nominal;
     jsd_epd_sil_config_t epd_sil;
@@ -95,6 +98,7 @@ typedef struct {
     jsd_el3318_state_t      el3318;
     jsd_el3162_state_t      el3162;
     jsd_el4102_state_t      el4102;
+    jsd_el5042_state_t      el5042;
     jsd_ild1900_state_t     ild1900;
     jsd_epd_nominal_private_state_t epd_nominal;
     jsd_epd_sil_private_state_t epd_sil;