Merge pull request #92 from nasa-jpl/feat-el1008-driver

Feat el1008 driver

Author: preston-rogers

README.md

@@ -61,7 +61,7 @@ most useful devices. The following table represents the latest state of the JSD
 | EL3356                    | 2    | 1.1.0                        |
 | JED (JPL EtherCat Device) | 2    | 1.2.0                        |
 | ATI Force-Torque Sensor   | 2    | 1.4.0                        |
-| EL1008                    | 2    | TBD                          |
+| EL1008                    | 2    | 2.3.4                        |
 | EL3202-0010               | 2    | 1.5.0                        |
 | EL3255                    | 2    | TBD                          |
 | EL3318                    | 2    | 1.5.0                        |

src/CMakeLists.txt

@@ -16,6 +16,7 @@ add_library(jsd-lib STATIC
     jsd_timer.c
     jsd_ati_fts.c
     jsd_el3104.c
+    jsd_el1008.c
     jsd_el3202.c
     jsd_el3318.c
     jsd_el3162.c

src/jsd.c

@@ -8,6 +8,7 @@
 
 #include "jsd/jsd_ati_fts.h"
 #include "jsd/jsd_egd.h"
+#include "jsd/jsd_el1008.h"
 #include "jsd/jsd_el2124.h"
 #include "jsd/jsd_el3104.h"
 #include "jsd/jsd_el3162.h"
@@ -439,6 +440,10 @@ bool jsd_init_single_device(jsd_t* self, uint16_t slave_id) {
       return jsd_egd_init(self, slave_id);
       break;
     }
+    case JSD_EL1008_PRODUCT_CODE: {
+      return jsd_el1008_init(self, slave_id);
+      break;
+    }
     case JSD_EL2124_PRODUCT_CODE: {
       return jsd_el2124_init(self, slave_id);
       break;

src/jsd_el1008.c

@@ -0,0 +1,72 @@
+#include "jsd/jsd_el1008.h"
+
+#include <assert.h>
+#include <string.h>
+
+#include "jsd/jsd_sdo.h"
+
+/****************************************************
+ * Public functions
+ ****************************************************/
+
+const jsd_el1008_state_t* jsd_el1008_get_state(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id ==
+         JSD_EL1008_PRODUCT_CODE);
+
+  jsd_el1008_state_t* state = &self->slave_states[slave_id].el1008;
+  return state;
+}
+
+void jsd_el1008_read(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id ==
+         JSD_EL1008_PRODUCT_CODE);
+
+  jsd_el1008_state_t* state = &self->slave_states[slave_id].el1008;
+
+  const jsd_el1008_txpdo_t* txpdo =
+      (jsd_el1008_txpdo_t*)self->ecx_context.slavelist[slave_id].inputs;
+
+  for (int ch = 0; ch < JSD_EL1008_NUM_CHANNELS; ++ch) {
+    state->values[ch] = (1 << ch) & txpdo->channels; // Bit shift the channel to get value
+  }
+}
+
+/****************************************************
+ * Private functions
+ ****************************************************/
+
+bool jsd_el1008_init(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id ==
+         JSD_EL1008_PRODUCT_CODE);
+  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_el1008_PO2SO_config;
+
+  return true;
+}
+
+int jsd_el1008_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id) {
+  assert(ecx_context);
+  assert(ecx_context->slavelist[slave_id].eep_id == JSD_EL1008_PRODUCT_CODE);
+
+  // 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];
+
+  MSG("Configuring slave no: %u, SII inferred name: %s", slave_id,
+      ecx_context->slavelist[slave_id].name);
+  MSG("\t Configured name: %s", config->name);
+
+  config->PO2SO_success = true;
+  return 1;
+}

src/jsd_el1008.h

@@ -0,0 +1,41 @@
+#ifndef JSD_EL1008_H
+#define JSD_EL1008_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd.h"
+
+/**
+ * @brief TxPDO struct used to read device data in SOEM IOmap
+ *
+ * Note: Struct order matters and must be packed.
+ */
+typedef struct __attribute__((__packed__)) {
+  uint8_t channels; // Each bit corresponds to a channel.
+} jsd_el1008_txpdo_t;
+
+/** @brief Initializes el1008 and registers the PO2SO function
+ *
+ * @param self pointer JSD context
+ * @param slave_id index of device on EtherCAT bus
+ * @return true on success, false on failure
+ */
+bool jsd_el1008_init(jsd_t* self, uint16_t slave_id);
+
+/**
+ * @brief Configuration function called by SOEM upon a PreOp to SafeOp state
+ * transition that (re)configures EL1008 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_el1008_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_el1008_pub.h

@@ -0,0 +1,31 @@
+#ifndef JSD_EL1008_PUB_H
+#define JSD_EL1008_PUB_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_pub.h"
+
+/**
+ * @brief Read the EL1008 device state
+ *
+ * @param self Pointer to JSD context
+ * @param slave_id Slave ID of EL1008 device
+ * @return Pointer to EL1008 device state
+ */
+const jsd_el1008_state_t* jsd_el1008_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 EL1008 device
+ */
+void jsd_el1008_read(jsd_t* self, uint16_t slave_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_el1008_types.h

@@ -0,0 +1,30 @@
+#ifndef JSD_EL1008_TYPES_H
+#define JSD_EL1008_TYPES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_common_device_types.h"
+
+#define JSD_EL1008_PRODUCT_CODE (uint32_t)0x03f03052
+#define JSD_EL1008_NUM_CHANNELS 8 // Each of the 8 values is 1 bit long.
+
+/**
+ * @brief configuration struct for EL1008 device initialization
+ */
+typedef struct {
+} jsd_el1008_config_t;
+
+/**
+ * @brief Read struct for el1008 module
+ */
+typedef struct {
+  uint8_t values[JSD_EL1008_NUM_CHANNELS]; 
+} jsd_el1008_state_t;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_types.h

@@ -12,6 +12,7 @@ extern "C" {
 #include "jsd/jsd_ati_fts_types.h"
 #include "jsd/jsd_egd_types.h"
 #include "jsd/jsd_el2124_types.h"
+#include "jsd/jsd_el1008_types.h"
 #include "jsd/jsd_el3104_types.h"
 #include "jsd/jsd_el3162_types.h"
 #include "jsd/jsd_el3202_types.h"
@@ -32,6 +33,7 @@ typedef struct {
   uint32_t product_code;
   char     name[JSD_NAME_LEN];
   union {
+    jsd_el1008_config_t  el1008;
     jsd_el3602_config_t  el3602;
     jsd_el3208_config_t  el3208;
     jsd_el2124_config_t  el2124;
@@ -54,6 +56,7 @@ typedef struct {
 
 typedef struct {
   union {
+    jsd_el1008_state_t      el1008;
     jsd_el3602_state_t      el3602;
     jsd_el3208_state_t      el3208;
     jsd_el2124_state_t      el2124;

test/CMakeLists.txt

@@ -36,6 +36,11 @@ if(BUILD_JSD_TESTS)
         jsd_test_utils.c)
     target_link_libraries(jsd_device_test_template ${jsd_test_libs})
 
+    add_executable(jsd_el1008_test
+        device/jsd_el1008_test.c
+        jsd_test_utils.c)
+    target_link_libraries(jsd_el1008_test ${jsd_test_libs})
+
     add_executable(jsd_el2124_test
         device/jsd_el2124_test.c
         jsd_test_utils.c)

test/device/jsd_el1008_test.c

@@ -0,0 +1,96 @@
+#include <assert.h>
+#include <string.h>
+
+#include "jsd/jsd_el1008_pub.h"
+#include "jsd/jsd_el1008_types.h"
+#include "jsd_test_utils.h"
+
+extern bool  quit;
+extern FILE* file;
+uint8_t      slave_id;
+
+void telemetry_header() {
+  if (!file) {
+    return;
+  }
+  for (int i = 0; i < JSD_EL1008_NUM_CHANNELS; ++i) {
+    fprintf(file, "EL1008_ch%d_level, ", i);
+  }
+  fprintf(file, "\n");
+}
+
+void telemetry_data(void* self) {
+  assert(self);
+
+  if (!file) {
+    return;
+  }
+
+  single_device_server_t*   sds   = (single_device_server_t*)self;
+  const jsd_el1008_state_t* state = jsd_el1008_get_state(sds->jsd, slave_id);
+
+  for (int i = 0; i < JSD_EL1008_NUM_CHANNELS; ++i) {
+    fprintf(file, "%d,", state->values[i]);
+  }
+  fprintf(file, "\n");
+  fflush(file);
+}
+
+void print_info(void* self) {
+  assert(self);
+
+  single_device_server_t*   sds   = (single_device_server_t*)self;
+  const jsd_el1008_state_t* state = jsd_el1008_get_state(sds->jsd, slave_id);
+  MSG("Ch0: %d, Ch1: %d, Ch2: %d, Ch3: %d, Ch4: %d, Ch5: %d, Ch6: %d, Ch7: %d, ", 
+      state->values[0], state->values[1], state->values[2], state->values[3], 
+      state->values[4], state->values[5], state->values[6], state->values[7]);
+}
+
+void extract_data(void* self) {
+  assert(self);
+
+  single_device_server_t* sds = (single_device_server_t*)self;
+  jsd_el1008_read(sds->jsd, slave_id);
+}
+
+void command(void* self) { (void)self; };
+
+int main(int argc, char* argv[]) {
+  if (argc != 4) {
+    ERROR("Expecting exactly 3 arguments");
+    MSG("Usage: jsd_el1008_test <ifname> <el1008_slave_index> <loop_freq_hz>");
+    MSG("Example: $ jsd_el1008_test eth0 2 1000");
+    return 0;
+  }
+
+  char* ifname          = strdup(argv[1]);
+  slave_id              = atoi(argv[2]);
+  uint32_t loop_freq_hz = atoi(argv[3]);
+  MSG("Configuring device %s, using slave %d", ifname, slave_id);
+  MSG("Using frequency of %i hz", loop_freq_hz);
+
+  single_device_server_t sds;
+
+  sds_set_telemetry_header_callback(&sds, telemetry_header);
+  sds_set_telemetry_data_callback(&sds, telemetry_data);
+  sds_set_print_info_callback(&sds, print_info);
+  sds_set_extract_data_callback(&sds, extract_data);
+  sds_set_command_callback(&sds, command);
+
+  sds_setup(&sds, loop_freq_hz);
+
+  // Set device configuration here.
+  jsd_slave_config_t my_config = {0};
+
+  snprintf(my_config.name, JSD_NAME_LEN, "unicorn");
+  my_config.configuration_active = true;
+  my_config.product_code         = JSD_EL1008_PRODUCT_CODE;
+
+  jsd_set_slave_config(sds.jsd, slave_id, my_config);
+
+  sds_run(&sds, ifname, "/tmp/jsd_el1008.csv");
+
+  free(ifname);
+
+  return 0;
+}