Skip to content

Commit eb8ebaa

Browse files
kwehagekwehage
authored
Merge pull request #121 from nasa-jpl/kwehage-el3314
El3314 Driver
2 parents be365f3 + e93c297 commit eb8ebaa

8 files changed

+372
-14
lines changed

‎.gitignore

+3-2
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,8 @@
44
# swap files
55
*.swp
66
*.swo
7-
7+
*.swn
8+
*.swb
89

910
# Ignore folders
1011
build
@@ -17,4 +18,4 @@ bin
1718
.doxyfile
1819

1920
# clang-format
20-
.clang-format
21+
.clang-format

‎src/CMakeLists.txt

+17-12
Original file line numberDiff line numberDiff line change
@@ -5,27 +5,32 @@ add_library(jsd-lib STATIC
55
jsd_common_device_types.c
66
jsd_elmo_common.c
77
jsd_epd_common.c
8+
jsd_timer.c
89

910
# Devices
10-
jsd_el3602.c
11-
jsd_el3208.c
12-
jsd_el2124.c
13-
jsd_el2809.c
1411
jsd_egd.c
15-
jsd_el3356.c
16-
jsd_jed0101.c
17-
jsd_jed0200.c
18-
jsd_timer.c
12+
jsd_epd_nominal.c
13+
jsd_epd_sil.c
14+
1915
jsd_ati_fts.c
20-
jsd_el3104.c
16+
2117
jsd_el1008.c
18+
jsd_el2124.c
19+
jsd_el2809.c
20+
jsd_el3104.c
21+
jsd_el3162.c
2222
jsd_el3202.c
23+
jsd_el3208.c
24+
jsd_el3314.c
2325
jsd_el3318.c
24-
jsd_el3162.c
26+
jsd_el3356.c
27+
jsd_el3602.c
2528
jsd_el4102.c
29+
2630
jsd_ild1900.c
27-
jsd_epd_nominal.c
28-
jsd_epd_sil.c
31+
32+
jsd_jed0101.c
33+
jsd_jed0200.c
2934
)
3035

3136
message(STATUS "SOEM INCLUDE DIRS: ${SOEM_INCLUDE_DIRS}")

‎src/jsd.c

+9
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include "jsd/jsd_el3162.h"
1616
#include "jsd/jsd_el3202.h"
1717
#include "jsd/jsd_el3208.h"
18+
#include "jsd/jsd_el3314.h"
1819
#include "jsd/jsd_el3318.h"
1920
#include "jsd/jsd_el3356.h"
2021
#include "jsd/jsd_el3602.h"
@@ -468,6 +469,8 @@ const char* jsd_driver_type_to_string(jsd_driver_type_t driver_type) {
468469
return "JSD_DRIVER_TYPE_EL3202";
469470
case JSD_DRIVER_TYPE_EL3208:
470471
return "JSD_DRIVER_TYPE_EL3208";
472+
case JSD_DRIVER_TYPE_EL3314:
473+
return "JSD_DRIVER_TYPE_EL3314";
471474
case JSD_DRIVER_TYPE_EL3318:
472475
return "JSD_DRIVER_TYPE_EL3318";
473476
case JSD_DRIVER_TYPE_EL3356:
@@ -587,6 +590,9 @@ bool jsd_driver_is_compatible_with_product_code(jsd_driver_type_t driver_type,
587590
case JSD_DRIVER_TYPE_EL3202:
588591
is_compatible = jsd_el3202_product_code_is_compatible(product_code);
589592
break;
593+
case JSD_DRIVER_TYPE_EL3314:
594+
is_compatible = jsd_el3314_product_code_is_compatible(product_code);
595+
break;
590596
case JSD_DRIVER_TYPE_EL3318:
591597
is_compatible = jsd_el3318_product_code_is_compatible(product_code);
592598
break;
@@ -654,6 +660,9 @@ bool jsd_init_single_device(jsd_t* self, uint16_t slave_id) {
654660
case JSD_DRIVER_TYPE_EL3104:
655661
return jsd_el3104_init(self, slave_id);
656662
break;
663+
case JSD_DRIVER_TYPE_EL3314:
664+
return jsd_el3314_init(self, slave_id);
665+
break;
657666
case JSD_DRIVER_TYPE_EL3318:
658667
return jsd_el3318_init(self, slave_id);
659668
break;

‎src/jsd_el3314.c

+167
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
1+
#include "jsd/jsd_el3314.h"
2+
3+
#include <assert.h>
4+
#include <string.h>
5+
6+
#include "jsd/jsd_sdo.h"
7+
8+
const char* jsd_el3314_element_strings[] = {
9+
[JSD_EL3314_ELEMENT_TYPE_K] = "TYPE_K",
10+
[JSD_EL3314_ELEMENT_TYPE_J] = "TYPE_J",
11+
[JSD_EL3314_ELEMENT_TYPE_L] = "TYPE_L",
12+
[JSD_EL3314_ELEMENT_TYPE_E] = "TYPE_E",
13+
[JSD_EL3314_ELEMENT_TYPE_T] = "TYPE_T",
14+
[JSD_EL3314_ELEMENT_TYPE_N] = "TYPE_N",
15+
[JSD_EL3314_ELEMENT_TYPE_U] = "TYPE_U",
16+
[JSD_EL3314_ELEMENT_TYPE_B] = "TYPE_B",
17+
[JSD_EL3314_ELEMENT_TYPE_R] = "TYPE_R",
18+
[JSD_EL3314_ELEMENT_TYPE_S] = "TYPE_S",
19+
[JSD_EL3314_ELEMENT_TYPE_C] = "TYPE_C",
20+
};
21+
22+
const char* jsd_el3314_presentation_strings[] = {
23+
[JSD_EL3314_PRESENTATION_SIGNED] = "Signed",
24+
[1] = "unused presentation",
25+
[JSD_EL3314_PRESENTATION_HIGH_RES] = "High Res",
26+
};
27+
28+
/****************************************************
29+
* Public functions
30+
****************************************************/
31+
const jsd_el3314_state_t* jsd_el3314_get_state(jsd_t* self, uint16_t slave_id) {
32+
assert(self);
33+
assert(jsd_el3314_product_code_is_compatible(
34+
self->ecx_context.slavelist[slave_id].eep_id));
35+
36+
return &self->slave_states[slave_id].el3314;
37+
}
38+
39+
void jsd_el3314_read(jsd_t* self, uint16_t slave_id) {
40+
assert(self);
41+
assert(jsd_el3314_product_code_is_compatible(
42+
self->ecx_context.slavelist[slave_id].eep_id));
43+
44+
jsd_el3314_state_t* state = &self->slave_states[slave_id].el3314;
45+
jsd_el3314_config_t* config = &self->slave_configs[slave_id].el3314;
46+
47+
jsd_el3314_txpdo_t* txpdo =
48+
(jsd_el3314_txpdo_t*)self->ecx_context.slavelist[slave_id].inputs;
49+
50+
int ch;
51+
for (ch = 0; ch < JSD_EL3314_NUM_CHANNELS; ch++) {
52+
state->adc_value[ch] = txpdo->channel[ch].value;
53+
state->output_eu[ch] =
54+
jsd_el3314_output_from_config(txpdo->channel[ch].value, config, ch);
55+
56+
state->underrange[ch] = (txpdo->channel[ch].flags >> 0) & 0x01;
57+
state->overrange[ch] = (txpdo->channel[ch].flags >> 1) & 0x01;
58+
state->error[ch] = (txpdo->channel[ch].flags >> 6) & 0x01;
59+
state->txPDO_state[ch] = (txpdo->channel[ch].flags >> 14) & 0x01;
60+
state->txPDO_toggle[ch] = (txpdo->channel[ch].flags >> 15) & 0x01;
61+
}
62+
}
63+
64+
/****************************************************
65+
* Private functions
66+
****************************************************/
67+
68+
bool jsd_el3314_init(jsd_t* self, uint16_t slave_id) {
69+
assert(self);
70+
assert(jsd_el3314_product_code_is_compatible(
71+
self->ecx_context.slavelist[slave_id].eep_id));
72+
assert(self->ecx_context.slavelist[slave_id].eep_man ==
73+
JSD_BECKHOFF_VENDOR_ID);
74+
75+
ec_slavet* slaves = self->ecx_context.slavelist;
76+
ec_slavet* slave = &slaves[slave_id];
77+
78+
slave->PO2SOconfigx = jsd_el3314_PO2SO_config;
79+
80+
return true;
81+
}
82+
83+
int jsd_el3314_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id) {
84+
assert(ecx_context);
85+
assert(jsd_el3314_product_code_is_compatible(
86+
ecx_context->slavelist[slave_id].eep_id));
87+
88+
// cast the void* to slave_config
89+
jsd_slave_config_t* slave_configs =
90+
(jsd_slave_config_t*)ecx_context->userdata;
91+
92+
jsd_slave_config_t* config = &slave_configs[slave_id];
93+
94+
// Reset to factory default
95+
uint32_t reset_word = JSD_BECKHOFF_RESET_WORD;
96+
if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, JSD_BECKHOFF_RESET_SDO,
97+
JSD_BECKHOFF_RESET_SUBIND, JSD_SDO_DATA_U32,
98+
&reset_word)) {
99+
return 0;
100+
}
101+
102+
MSG("Configuring slave no: %u, SII inferred name: %s", slave_id,
103+
ecx_context->slavelist[slave_id].name);
104+
MSG("\t Configured name: %s", config->name);
105+
106+
int ch;
107+
for (ch = 0; ch < JSD_EL3314_NUM_CHANNELS; ch++) {
108+
MSG("\t Ch%d Config:", ch);
109+
MSG("\t\tElement: %s",
110+
jsd_el3314_element_strings[config->el3314.element[ch]]);
111+
MSG("\t\tFilter: %s",
112+
jsd_beckhoff_filter_strings[config->el3314.filter[ch]]);
113+
MSG("\t\tPresentation: %s",
114+
jsd_el3314_presentation_strings[config->el3314.presentation[ch]]);
115+
116+
// register is 0x80n0, where n is channel number (e.g. ch4 = 0x8040)
117+
uint32_t sdo_channel_index = 0x8000 + (0x10 * ch);
118+
119+
uint16_t element = config->el3314.element[ch];
120+
if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
121+
0x19, JSD_SDO_DATA_U16, &element)) {
122+
return 0;
123+
}
124+
125+
uint16_t filter = config->el3314.filter[ch];
126+
if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
127+
0x15, JSD_SDO_DATA_U16, &filter)) {
128+
return 0;
129+
}
130+
131+
uint8_t presentation = config->el3314.presentation[ch];
132+
if (!jsd_sdo_set_param_blocking(ecx_context, slave_id, sdo_channel_index,
133+
0x02, JSD_SDO_DATA_U8, &presentation)) {
134+
return 0;
135+
}
136+
}
137+
138+
config->PO2SO_success = true;
139+
return 1;
140+
}
141+
142+
double jsd_el3314_output_from_config(int16_t adc_value,
143+
jsd_el3314_config_t* config,
144+
uint16_t channel) {
145+
assert(config->presentation[channel] < JSD_EL3314_NUM_PRESENTATIONS);
146+
assert(config->element[channel] < JSD_EL3314_NUM_ELEMENTS);
147+
148+
double temp_scale_factor = 1;
149+
150+
switch (config->presentation[channel]) {
151+
case JSD_EL3314_PRESENTATION_SIGNED:
152+
temp_scale_factor = 0.1;
153+
break;
154+
case JSD_EL3314_PRESENTATION_HIGH_RES:
155+
temp_scale_factor = 0.01;
156+
break;
157+
default:
158+
WARNING("Bad/Unsupported presentation setting");
159+
break;
160+
}
161+
162+
return (double)adc_value * temp_scale_factor;
163+
}
164+
165+
bool jsd_el3314_product_code_is_compatible(uint32_t product_code) {
166+
return product_code == JSD_EL3314_PRODUCT_CODE;
167+
}

‎src/jsd_el3314.h

+63
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
#ifndef JSD_EL3314_H
2+
#define JSD_EL3314_H
3+
4+
#ifdef __cplusplus
5+
extern "C" {
6+
#endif
7+
8+
#include "jsd/jsd_el3314_pub.h"
9+
10+
/**
11+
* @brief Private IOmap channel struct used to retreive input data from SOEM
12+
* IOmap
13+
*/
14+
typedef struct __attribute__((__packed__)) {
15+
uint16_t flags;
16+
int16_t value;
17+
} jsd_el3314_channel_txpdo_t;
18+
19+
/**
20+
* @brief Private IOmap struct used to retreive input data from SOEM IOmap
21+
*/
22+
typedef struct __attribute__((__packed__)) {
23+
jsd_el3314_channel_txpdo_t channel[JSD_EL3314_NUM_CHANNELS];
24+
} jsd_el3314_txpdo_t;
25+
26+
/** @brief Initializes el3314 and registers the PO2SO function
27+
*
28+
* @param self pointer JSD context
29+
* @param slave_id index of device on EtherCAT bus
30+
* @return true on success, false on failure
31+
*/
32+
bool jsd_el3314_init(jsd_t* self, uint16_t slave_id);
33+
34+
/**
35+
* @brief Configuration function called by SOEM upon a PreOp to SafeOp state
36+
* transition that (re)configures el3314 device settings
37+
*
38+
* @param ecx_context SOEM context pointer
39+
* @param slave_id index of device on EtherCAT bus
40+
* @return 1 on success, 0 on failure
41+
*/
42+
int jsd_el3314_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id);
43+
44+
/**
45+
* @brief Computes the state output_eu field using device config
46+
*/
47+
double jsd_el3314_output_from_config(int16_t adc_value,
48+
jsd_el3314_config_t* config,
49+
uint16_t channel);
50+
51+
/**
52+
* @brief Checks whether a product code is compatible with EL3314.
53+
*
54+
* @param product_code The product code to be checked
55+
* @return True if the product code is compatible, false otherwise.
56+
*/
57+
bool jsd_el3314_product_code_is_compatible(uint32_t product_code);
58+
59+
#ifdef __cplusplus
60+
}
61+
#endif
62+
63+
#endif

‎src/jsd_el3314_pub.h

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
#ifndef JSD_EL3314_PUB_H
2+
#define JSD_EL3314_PUB_H
3+
4+
#ifdef __cplusplus
5+
extern "C" {
6+
#endif
7+
8+
#include "jsd/jsd_pub.h"
9+
10+
/**
11+
* @brief Reads EL3314 state
12+
*
13+
* @param self Pointer to JSD context
14+
* @param slave_id id of EL3314 device
15+
* @return Pointer to EL3314 state
16+
*/
17+
const jsd_el3314_state_t* jsd_el3314_get_state(jsd_t* self, uint16_t slave_id);
18+
19+
/**
20+
* @brief Converts raw PDO data to state data
21+
*
22+
* @param self pointer to JSD context
23+
* @param slave_id id of EL3314 device
24+
*/
25+
void jsd_el3314_read(jsd_t* self, uint16_t slave_id);
26+
27+
#ifdef __cplusplus
28+
}
29+
#endif
30+
31+
#endif

0 commit comments

Comments
 (0)