Merge pull request #99 from nasa-jpl/feat-el2809-driver

Feat el2809 driver

src/CMakeLists.txt

@@ -9,6 +9,7 @@ add_library(jsd-lib STATIC
     jsd_el3602.c
     jsd_el3208.c
     jsd_el2124.c
+    jsd_el2809.c
     jsd_egd.c
     jsd_el3356.c
     jsd_jed0101.c

src/jsd.c

@@ -10,6 +10,7 @@
 #include "jsd/jsd_egd.h"
 #include "jsd/jsd_el1008.h"
 #include "jsd/jsd_el2124.h"
+#include "jsd/jsd_el2809.h"
 #include "jsd/jsd_el3104.h"
 #include "jsd/jsd_el3162.h"
 #include "jsd/jsd_el3202.h"
@@ -448,6 +449,10 @@ bool jsd_init_single_device(jsd_t* self, uint16_t slave_id) {
       return jsd_el2124_init(self, slave_id);
       break;
     }
+    case JSD_EL2809_PRODUCT_CODE: {
+      return jsd_el2809_init(self, slave_id);
+      break;
+    }
     case JSD_EL3356_PRODUCT_CODE: {
       return jsd_el3356_init(self, slave_id);
       break;

src/jsd_el2809.c

@@ -0,0 +1,80 @@
+#include "jsd/jsd_el2809.h"
+
+#include <assert.h>
+
+#include "jsd/jsd_sdo.h"
+
+/****************************************************
+ * Public functions
+ ****************************************************/
+
+/**
+ * @brief RxPDO struct used to set device command data in SOEM IOmap
+ */
+typedef struct __attribute__((__packed__)) {
+  uint16_t flags;
+} jsd_el2809_rxpdo_t;
+
+const jsd_el2809_state_t* jsd_el2809_get_state(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id ==
+         JSD_EL2809_PRODUCT_CODE);
+
+  return &self->slave_states[slave_id].el2809;
+}
+
+void jsd_el2809_process(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id ==
+         JSD_EL2809_PRODUCT_CODE);
+
+  jsd_el2809_rxpdo_t* rxpdo =
+      (jsd_el2809_rxpdo_t*)self->ecx_context.slavelist[slave_id].outputs;
+
+  int ch;
+  for (ch = 0; ch < JSD_EL2809_NUM_CHANNELS; ch++) {
+    uint8_t output = self->slave_states[slave_id].el2809.output[ch];
+
+    if (output > 0) {
+      rxpdo->flags |= 0x01 << ch;
+    } else {
+      rxpdo->flags &= ~(0x01 << ch);
+    }
+  }
+}
+
+void jsd_el2809_write_single_channel(jsd_t* self, uint16_t slave_id,
+                                     uint8_t channel, uint8_t output) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id ==
+         JSD_EL2809_PRODUCT_CODE);
+
+  self->slave_states[slave_id].el2809.output[channel] = output;
+}
+
+void jsd_el2809_write_all_channels(jsd_t* self, uint16_t slave_id,
+                                   uint8_t output[JSD_EL2809_NUM_CHANNELS]) {
+  int ch;
+  for (ch = 0; ch < JSD_EL2809_NUM_CHANNELS; ch++) {
+    jsd_el2809_write_single_channel(self, slave_id, ch, output[ch]);
+  }
+}
+
+/****************************************************
+ * Private functions
+ ****************************************************/
+
+bool jsd_el2809_init(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id ==
+         JSD_EL2809_PRODUCT_CODE);
+  assert(self->ecx_context.slavelist[slave_id].eep_man ==
+         JSD_BECKHOFF_VENDOR_ID);
+
+  jsd_slave_config_t* config = &self->slave_configs[slave_id];
+
+  // no PO2SO callback for 2809 devices, so set the success flag now
+  config->PO2SO_success = true;
+
+  return true;
+}

src/jsd_el2809.h

@@ -0,0 +1,22 @@
+#ifndef JSD_EL2809_H
+#define JSD_EL2809_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_el2809_pub.h"
+
+/** @brief Initializes el2809
+ *
+ * @param self pointer JSD context
+ * @param slave_id index of device on EtherCAT bus
+ * @return true on success, false on failure
+ */
+bool jsd_el2809_init(jsd_t* self, uint16_t slave_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_el2809_pub.h

@@ -0,0 +1,56 @@
+#ifndef JSD_EL2809_PUB_H
+#define JSD_EL2809_PUB_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_el2809_types.h"
+#include "jsd/jsd_pub.h"
+
+/**
+ * @brief Read the EL2809 State
+ *
+ * Note: this device does not actually provide PDO feedback on state,
+ * This function reads back the cmd sent to the EL2809 device
+ *
+ * @param self pointer to JSD context
+ * @param slave_id id of EL2809 device
+ * @return Pointer to EL2809 device state
+ */
+const jsd_el2809_state_t* jsd_el2809_get_state(jsd_t* self, uint16_t slave_id);
+
+/**
+ * @brief process loop required for proper device function
+ *
+ * @param self pointer to JSD context
+ * @param slave_id id of EL2809 device
+ */
+void jsd_el2809_process(jsd_t* self, uint16_t slave_id);
+
+/**
+ * @brief Sets a specified channel level
+ *
+ * @param self pointer to JSD context
+ * @param slave_id id of EL2809 device
+ * @param channel specified device channel to command
+ * @param output command level (0 or 1)
+ */
+void jsd_el2809_write_single_channel(jsd_t* self, uint16_t slave_id,
+                                     uint8_t channel, uint8_t output);
+
+/**
+ * @brief Sets all channel levels
+ *
+ * @param self pointer to JSD context
+ * @param slave_id id of EL2809 device
+ * @param output command level (0 or 1)
+ */
+void jsd_el2809_write_all_channels(jsd_t* self, uint16_t slave_id,
+                                   uint8_t output[JSD_EL2809_NUM_CHANNELS]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_el2809_types.h

@@ -0,0 +1,31 @@
+#ifndef JSD_EL2809_TYPES_H
+#define JSD_EL2809_TYPES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_common_device_types.h"
+
+#define JSD_EL2809_PRODUCT_CODE (uint32_t)0x0af93052
+
+#define JSD_EL2809_NUM_CHANNELS 16
+
+/**
+ * @brief EL2809 State Data
+ */
+typedef struct {
+  uint8_t output[JSD_EL2809_NUM_CHANNELS];  ///< digital output level (0 or 1)
+} jsd_el2809_state_t;
+
+/**
+ * @brief EL2809 device configuration
+ */
+typedef struct {
+} jsd_el2809_config_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_el2809_types.h"
 #include "jsd/jsd_el1008_types.h"
 #include "jsd/jsd_el3104_types.h"
 #include "jsd/jsd_el3162_types.h"
@@ -37,6 +38,7 @@ typedef struct {
     jsd_el3602_config_t  el3602;
     jsd_el3208_config_t  el3208;
     jsd_el2124_config_t  el2124;
+    jsd_el2809_config_t  el2809;
     jsd_egd_config_t     egd;
     jsd_el3356_config_t  el3356;
     jsd_jed0101_config_t jed0101;
@@ -60,6 +62,7 @@ typedef struct {
     jsd_el3602_state_t      el3602;
     jsd_el3208_state_t      el3208;
     jsd_el2124_state_t      el2124;
+    jsd_el2809_state_t      el2809;
     jsd_egd_private_state_t egd;
     jsd_el3356_state_t      el3356;
     jsd_jed0101_state_t     jed0101;

test/CMakeLists.txt

@@ -46,6 +46,11 @@ if(BUILD_JSD_TESTS)
         jsd_test_utils.c)
     target_link_libraries(jsd_el2124_test ${jsd_test_libs})
 
+    add_executable(jsd_el2809_test
+        device/jsd_el2809_test.c
+        jsd_test_utils.c)
+    target_link_libraries(jsd_el2809_test ${jsd_test_libs})
+
     add_executable(jsd_el3602_test
         device/jsd_el3602_test.c
         jsd_test_utils.c)

test/device/jsd_el2809_test.c

@@ -0,0 +1,109 @@
+#include <assert.h>
+#include <string.h>
+
+#include "jsd/jsd_el2809_pub.h"
+#include "jsd_test_utils.h"
+
+extern bool  quit;
+extern FILE* file;
+uint8_t      slave_id;
+uint8_t      cmd_output[JSD_EL2809_NUM_CHANNELS] = {1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+
+void telemetry_header() {
+  int i;
+  if (!file) {
+    return;
+  }
+  for (i = 0; i < JSD_EL2809_NUM_CHANNELS; ++i) {
+    fprintf(file, "EL2809_output_ch%d, ", 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_el2809_state_t* state = jsd_el2809_get_state(sds->jsd, slave_id);
+
+  int i;
+  for (i = 0; i < JSD_EL2809_NUM_CHANNELS; ++i) {
+    fprintf(file, "%u,", state->output[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_el2809_state_t* state = jsd_el2809_get_state(sds->jsd, slave_id);
+
+  MSG("Ch0: %u, Ch1: %u, Ch2: %u, Ch3: %u, Ch4: %u, Ch5: %u, Ch6: %u, Ch7: %u, Ch8: %u, "
+      "Ch9: %u, Ch10: %u, Ch11: %u, Ch12: %u, Ch13: %u, Ch14: %u, Ch15: %u, ", 
+      state->output[0], state->output[1], state->output[2], state->output[3], 
+      state->output[4], state->output[5], state->output[6], state->output[7], 
+      state->output[8], state->output[9], state->output[10], state->output[11], 
+      state->output[12], state->output[13], state->output[14], state->output[15]);
+}
+
+void extract_data(void* self) {
+  assert(self);
+  single_device_server_t* sds = (single_device_server_t*)self;
+
+  jsd_el2809_process(sds->jsd, slave_id);
+}
+
+void command(void* self) {
+  assert(self);
+  single_device_server_t* sds = (single_device_server_t*)self;
+
+  cmd_output[0] ^= 0x01;
+  cmd_output[1] ^= 0x01;
+  cmd_output[2] ^= 0x01;
+  cmd_output[3] ^= 0x01;
+
+  jsd_el2809_write_all_channels(sds->jsd, slave_id, cmd_output);
+}
+
+int main(int argc, char const* argv[]) {
+  if (argc != 4) {
+    ERROR("Expecting exactly 3 arguments");
+    MSG("Usage: jsd_el2809_test <ifname> <el2809_slave_index> <loop_freq_hz>");
+    MSG("Example: $ jsd_el2809 eth0 5 500");
+    return 0;
+  }
+
+  char* ifname          = strdup(argv[1]);
+  slave_id              = atoi(argv[2]);
+  uint16_t loop_freq_hz = atoi(argv[3]);
+  MSG("Configuring device %s, using slave %d", ifname, slave_id);
+  MSG("Using frequency of %u 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, "bigfoot");
+  my_config.configuration_active = true;
+  my_config.product_code         = JSD_EL2809_PRODUCT_CODE;
+
+  jsd_set_slave_config(sds.jsd, slave_id, my_config);
+
+  sds_run(&sds, ifname, "/tmp/jsd_el2809.csv");
+
+  return 0;
+}