Merge pull request #17 from nasa-jpl/jed-dev

Jed dev

Author: alex-brinkman

src/CMakeLists.txt

@@ -7,7 +7,8 @@ add_library(jsd-lib STATIC
     jsd_el2124.c
     jsd_egd.c
     jsd_el3356.c
-    jsd_jed.c
+    jsd_jed0101.c
+    jsd_jed0200.c
     jsd_timer.c
     jsd_ati_fts.c
     jsd_el3104.c

src/jsd.c

@@ -15,7 +15,8 @@
 #include "jsd/jsd_el3318.h"
 #include "jsd/jsd_el3356.h"
 #include "jsd/jsd_el3602.h"
-#include "jsd/jsd_jed.h"
+#include "jsd/jsd_jed0101.h"
+#include "jsd/jsd_jed0200.h"
 #include "jsd/jsd_print.h"
 #include "jsd/jsd_sdo.h"
 
@@ -464,8 +465,12 @@ bool jsd_init_single_device(jsd_t* self, uint16_t slave_id) {
       return jsd_el3356_init(self, slave_id);
       break;
     }
-    case JSD_JED_PRODUCT_CODE: {
-      return jsd_jed_init(self, slave_id);
+    case JSD_JED0101_PRODUCT_CODE: {
+      return jsd_jed0101_init(self, slave_id);
+      break;
+    }
+    case JSD_JED0200_PRODUCT_CODE: {
+      return jsd_jed0200_init(self, slave_id);
       break;
     }
     case JSD_ATI_FTS_PRODUCT_CODE: {

src/jsd_jed.c renamed to src/jsd_jed0101.c

@@ -1,4 +1,4 @@
-#include "jsd/jsd_jed.h"
+#include "jsd/jsd_jed0101.h"
 
 #include <assert.h>
 
@@ -9,29 +9,29 @@
  * Public functions
  ****************************************************/
 
-const jsd_jed_state_t* jsd_jed_get_state(jsd_t* self, uint16_t slave_id) {
+const jsd_jed0101_state_t* jsd_jed0101_get_state(jsd_t* self, uint16_t slave_id) {
   assert(self);
-  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED_PRODUCT_CODE);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0101_PRODUCT_CODE);
 
-  jsd_jed_state_t* state = &self->slave_states[slave_id].jed;
+  jsd_jed0101_state_t* state = &self->slave_states[slave_id].jed0101;
   return state;
 }
 
-void jsd_jed_set_cmd_value(jsd_t* self, uint16_t slave_id, uint16_t cmd) {
+void jsd_jed0101_set_cmd_value(jsd_t* self, uint16_t slave_id, uint16_t cmd) {
   assert(self);
-  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED_PRODUCT_CODE);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0101_PRODUCT_CODE);
 
-  jsd_jed_state_t* state = &self->slave_states[slave_id].jed;
+  jsd_jed0101_state_t* state = &self->slave_states[slave_id].jed0101;
   state->cmd             = cmd;
 }
 
-void jsd_jed_read(jsd_t* self, uint16_t slave_id) {
+void jsd_jed0101_read(jsd_t* self, uint16_t slave_id) {
   assert(self);
-  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED_PRODUCT_CODE);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0101_PRODUCT_CODE);
 
-  jsd_jed_state_t* state = &self->slave_states[slave_id].jed;
-  jsd_jed_txpdo_t* txpdo =
-      (jsd_jed_txpdo_t*)self->ecx_context.slavelist[slave_id].inputs;
+  jsd_jed0101_state_t* state = &self->slave_states[slave_id].jed0101;
+  jsd_jed0101_txpdo_t* txpdo =
+      (jsd_jed0101_txpdo_t*)self->ecx_context.slavelist[slave_id].inputs;
 
   state->status = txpdo->status;
   state->w_raw  = txpdo->w;
@@ -44,13 +44,13 @@ void jsd_jed_read(jsd_t* self, uint16_t slave_id) {
   state->z      = (double)txpdo->z / 1000.0;
 }
 
-void jsd_jed_process(jsd_t* self, uint16_t slave_id) {
+void jsd_jed0101_process(jsd_t* self, uint16_t slave_id) {
   assert(self);
-  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED_PRODUCT_CODE);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0101_PRODUCT_CODE);
 
-  jsd_jed_state_t* state = &self->slave_states[slave_id].jed;
-  jsd_jed_rxpdo_t* rxpdo =
-      (jsd_jed_rxpdo_t*)self->ecx_context.slavelist[slave_id].outputs;
+  jsd_jed0101_state_t* state = &self->slave_states[slave_id].jed0101;
+  jsd_jed0101_rxpdo_t* rxpdo =
+      (jsd_jed0101_rxpdo_t*)self->ecx_context.slavelist[slave_id].outputs;
 
   rxpdo->cmd = state->cmd;
 
@@ -61,26 +61,26 @@ void jsd_jed_process(jsd_t* self, uint16_t slave_id) {
  * Private functions
  ****************************************************/
 
-bool jsd_jed_init(jsd_t* self, uint16_t slave_id) {
+bool jsd_jed0101_init(jsd_t* self, uint16_t slave_id) {
   assert(self);
-  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED_PRODUCT_CODE);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0101_PRODUCT_CODE);
   assert(self->ecx_context.slavelist[slave_id].eep_man == JSD_JPL_VENDOR_ID);
 
   ec_slavet* slaves = self->ecx_context.slavelist;
   ec_slavet* slave  = &slaves[slave_id];
 
   jsd_slave_config_t* config = &self->slave_configs[slave_id];
 
-  slave->PO2SOconfigx   = jsd_jed_PO2SO_config;
+  slave->PO2SOconfigx   = jsd_jed0101_PO2SO_config;
   config->PO2SO_success = false;  // only set true in PO2SO callback
 
-  jsd_jed_state_t* state = &self->slave_states[slave_id].jed;
-  state->cmd             = config->jed.initial_cmd;
+  jsd_jed0101_state_t* state = &self->slave_states[slave_id].jed0101;
+  state->cmd             = config->jed0101.initial_cmd;
 
   return true;
 }
 
-int jsd_jed_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id) {
+int jsd_jed0101_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id) {
   jsd_slave_config_t* slave_configs =
       (jsd_slave_config_t*)ecx_context->userdata;
   jsd_slave_config_t* config = &slave_configs[slave_id];
@@ -100,7 +100,7 @@ int jsd_jed_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id) {
     config->PO2SO_success = false;
   } else {
     config->PO2SO_success = true;
-    SUCCESS("JED[%d] drive parameters successfully configured and verified",
+    SUCCESS("JED0101[%d] drive parameters successfully configured and verified",
             slave_id);
   }
   return 1;

src/jsd_jed.h renamed to src/jsd_jed0101.h

@@ -1,11 +1,11 @@
-#ifndef JSD_JED_H
-#define JSD_JED_H
+#ifndef JSD_JED0101_H
+#define JSD_JED0101_H
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-#include "jsd/jsd_jed_pub.h"
+#include "jsd/jsd_jed0101_pub.h"
 
 /**
  * @brief TxPDO struct used to read device data in SOEM IOmap
@@ -18,7 +18,7 @@ typedef struct __attribute__((__packed__)) {
   uint32_t x;
   uint32_t y;
   uint32_t z;
-} jsd_jed_txpdo_t;
+} jsd_jed0101_txpdo_t;
 
 /**
  * @brief RxPDO struct used to command the device via SOEM IOmap
@@ -27,25 +27,25 @@ typedef struct __attribute__((__packed__)) {
  */
 typedef struct __attribute__((__packed__)) {
   uint16_t cmd;
-} jsd_jed_rxpdo_t;
+} jsd_jed0101_rxpdo_t;
 
-/** @brief Initializes jed and registers the PO2SO function
+/** @brief Initializes jed0101 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_jed_init(jsd_t* self, uint16_t slave_id);
+bool jsd_jed0101_init(jsd_t* self, uint16_t slave_id);
 
 /**
  * @brief Configuration function called by SOEM upon a PreOp to SafeOp state
- * transition that (re)configures JED device settings
+ * transition that (re)configures JED0101 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_jed_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id);
+int jsd_jed0101_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id);
 
 #ifdef __cplusplus
 }

src/jsd_jed0101_pub.h

@@ -0,0 +1,48 @@
+#ifndef JSD_JED0101_PUB_H
+#define JSD_JED0101_PUB_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "jsd/jsd_pub.h"
+
+/**
+ * @brief Read the JED0101 state
+ *
+ * @param self pointer to JSD context
+ * @param slave_id slave id of JED0101
+ * @return Pointer to JED0101 state
+ */
+const jsd_jed0101_state_t* jsd_jed0101_get_state(jsd_t* self, uint16_t slave_id);
+
+/**
+ * @brief Sets the cmd PDO register on the JED0101 board
+ *
+ * @param self pointer to JSD context
+ * @param slave_id id of JED0101
+ * @param cmd Command integer value
+ */
+void jsd_jed0101_set_cmd_value(jsd_t* self, uint16_t slave_id, uint16_t cmd);
+
+/**
+ * @brief Converts raw PDO data to state data
+ *
+ * @param self pointer to JSD context
+ * @param slave_id id of JED0101
+ */
+void jsd_jed0101_read(jsd_t* self, uint16_t slave_id);
+
+/**
+ * @brief Processes Async SDO responses
+ *
+ * @param self pointer to JSD context
+ * @param slave_id id of JED0101
+ */
+void jsd_jed0101_process(jsd_t* self, uint16_t slave_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/jsd_jed_types.h renamed to src/jsd_jed0101_types.h

@@ -1,23 +1,23 @@
-#ifndef JSD_JED_TYPES_H
-#define JSD_JED_TYPES_H
+#ifndef JSD_JED0101_TYPES_H
+#define JSD_JED0101_TYPES_H
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
 #include "jsd/jsd_common_device_types.h"
 
-#define JSD_JED_PRODUCT_CODE (uint32_t)0x00009252
+#define JSD_JED0101_PRODUCT_CODE (uint32_t)0x00009252
 
 /**
- * @brief configuration struct for JED device initialization
+ * @brief configuration struct for JED0101 device initialization
  */
 typedef struct {
   uint16_t initial_cmd;  ///< Initial cmd value
-} jsd_jed_config_t;
+} jsd_jed0101_config_t;
 
 /**
- * @brief state data for JED module
+ * @brief state data for JED0101 module
  */
 
 typedef struct {
@@ -30,8 +30,8 @@ typedef struct {
   double   x;       ///< converted x component of IMU quaternion
   double   y;       ///< converted y component of IMU quaternion
   double   z;       ///< converted z component of IMU quaternion
-  uint16_t cmd;     ///< User specified cmd mode sent to JED
-} jsd_jed_state_t;
+  uint16_t cmd;     ///< User specified cmd mode sent to JED0101
+} jsd_jed0101_state_t;
 
 #ifdef __cplusplus
 }

src/jsd_jed0200.c

@@ -0,0 +1,109 @@
+#include "jsd/jsd_jed0200.h"
+
+#include <assert.h>
+
+#include "jsd/jsd.h"
+#include "jsd/jsd_sdo.h"
+
+/****************************************************
+ * Public functions
+ ****************************************************/
+
+const jsd_jed0200_state_t* jsd_jed0200_get_state(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0200_PRODUCT_CODE);
+
+  jsd_jed0200_state_t* state = &self->slave_states[slave_id].jed0200;
+  return state;
+}
+
+void jsd_jed0200_set_cmd_value(jsd_t* self, uint16_t slave_id, uint16_t cmd) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0200_PRODUCT_CODE);
+
+  jsd_jed0200_state_t* state = &self->slave_states[slave_id].jed0200;
+  state->cmd             = cmd;
+}
+
+void jsd_jed0200_read(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0200_PRODUCT_CODE);
+
+  jsd_jed0200_state_t* state = &self->slave_states[slave_id].jed0200;
+  jsd_jed0200_txpdo_t* txpdo =
+      (jsd_jed0200_txpdo_t*)self->ecx_context.slavelist[slave_id].inputs;
+
+  state->status = txpdo->status;
+  state->ticks = txpdo->ticks;
+  state->voltage_hv = txpdo->voltage_hv;
+  state->voltage_lv = txpdo->voltage_lv;
+  state->voltage_12v = txpdo->voltage_12v;
+  state->temp_ambient = txpdo->temp_ambient;
+  state->temp_actuator = txpdo->temp_actuator;
+  state->humidity = txpdo->humidity;
+  state->pressure = txpdo->pressure;
+  state->brake_current = txpdo->brake_current;
+  state->brake_cc_val = txpdo->brake_cc_val;
+}
+
+void jsd_jed0200_process(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0200_PRODUCT_CODE);
+
+  jsd_jed0200_state_t* state = &self->slave_states[slave_id].jed0200;
+  jsd_jed0200_rxpdo_t* rxpdo =
+      (jsd_jed0200_rxpdo_t*)self->ecx_context.slavelist[slave_id].outputs;
+
+  rxpdo->cmd = state->cmd;
+
+  jsd_async_sdo_process_response(self, slave_id);
+}
+
+/****************************************************
+ * Private functions
+ ****************************************************/
+
+bool jsd_jed0200_init(jsd_t* self, uint16_t slave_id) {
+  assert(self);
+  assert(self->ecx_context.slavelist[slave_id].eep_id == JSD_JED0200_PRODUCT_CODE);
+  assert(self->ecx_context.slavelist[slave_id].eep_man == JSD_JPL_VENDOR_ID);
+
+  ec_slavet* slaves = self->ecx_context.slavelist;
+  ec_slavet* slave  = &slaves[slave_id];
+
+  jsd_slave_config_t* config = &self->slave_configs[slave_id];
+
+  slave->PO2SOconfigx   = jsd_jed0200_PO2SO_config;
+  config->PO2SO_success = false;  // only set true in PO2SO callback
+
+  jsd_jed0200_state_t* state = &self->slave_states[slave_id].jed0200;
+  state->cmd             = config->jed0200.initial_cmd;
+
+  return true;
+}
+
+int jsd_jed0200_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id) {
+  jsd_slave_config_t* slave_configs =
+      (jsd_slave_config_t*)ecx_context->userdata;
+  jsd_slave_config_t* config = &slave_configs[slave_id];
+
+  // There were initial issues with the Sync Managers having the wrong address
+  uint8_t SM0, SM1, SM2, SM3;
+  jsd_sdo_get_param_blocking(ecx_context, slave_id, 0x1C00, 1, JSD_SDO_DATA_U8,
+                             &SM0);
+  jsd_sdo_get_param_blocking(ecx_context, slave_id, 0x1C00, 2, JSD_SDO_DATA_U8,
+                             &SM1);
+  jsd_sdo_get_param_blocking(ecx_context, slave_id, 0x1C00, 3, JSD_SDO_DATA_U8,
+                             &SM2);
+  jsd_sdo_get_param_blocking(ecx_context, slave_id, 0x1C00, 4, JSD_SDO_DATA_U8,
+                             &SM3);
+  if (SM0 != 1 || SM1 != 2 || SM2 != 3 || SM3 != 4) {
+    ERROR("Sync Manager are configured incorrectly, check the 0x1C00 register");
+    config->PO2SO_success = false;
+  } else {
+    config->PO2SO_success = true;
+    SUCCESS("JED0200[%d] drive parameters successfully configured and verified",
+            slave_id);
+  }
+  return 1;
+}