From 880e9bc71ae55218d920b22c4a7289b30c6935e0 Mon Sep 17 00:00:00 2001 From: prestonr Date: Thu, 24 Oct 2024 11:07:33 -0700 Subject: [PATCH] Adding the el5042 driver --- src/CMakeLists.txt | 1 + src/jsd.c | 9 +++ src/jsd_el5042.c | 129 +++++++++++++++++++++++++++++++++++++++++ src/jsd_el5042.h | 60 +++++++++++++++++++ src/jsd_el5042_pub.h | 31 ++++++++++ src/jsd_el5042_types.h | 36 ++++++++++++ src/jsd_types.h | 4 ++ 7 files changed, 270 insertions(+) create mode 100644 src/jsd_el5042.c create mode 100644 src/jsd_el5042.h create mode 100644 src/jsd_el5042_pub.h create mode 100644 src/jsd_el5042_types.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 617e466..cd72fe7 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/jsd.c b/src/jsd.c index 3b0e9b4..ce621b3 100644 --- a/src/jsd.c +++ b/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; diff --git a/src/jsd_el5042.c b/src/jsd_el5042.c new file mode 100644 index 0000000..307acf6 --- /dev/null +++ b/src/jsd_el5042.c @@ -0,0 +1,129 @@ +#include "jsd/jsd_el5042.h" + +#include +#include + +#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; +} diff --git a/src/jsd_el5042.h b/src/jsd_el5042.h new file mode 100644 index 0000000..62f52ef --- /dev/null +++ b/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 diff --git a/src/jsd_el5042_pub.h b/src/jsd_el5042_pub.h new file mode 100644 index 0000000..701f425 --- /dev/null +++ b/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 diff --git a/src/jsd_el5042_types.h b/src/jsd_el5042_types.h new file mode 100644 index 0000000..a70e0bc --- /dev/null +++ b/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 diff --git a/src/jsd_types.h b/src/jsd_types.h index 0b94d6f..d70beef 100644 --- a/src/jsd_types.h +++ b/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;