From 7647fd8cba9f4671474b3c41a67463d784691489 Mon Sep 17 00:00:00 2001 From: Joseph Bowkett Date: Sat, 11 Dec 2021 20:13:30 -0800 Subject: [PATCH 1/7] Rename jed->jed0101 --- src/CMakeLists.txt | 2 +- src/jsd.c | 6 +-- src/{jsd_jed.c => jsd_jed0101.c} | 48 ++++++++++---------- src/{jsd_jed.h => jsd_jed0101.h} | 18 ++++---- src/jsd_jed0101_pub.h | 48 ++++++++++++++++++++ src/{jsd_jed_types.h => jsd_jed0101_types.h} | 16 +++---- src/jsd_jed_pub.h | 48 -------------------- src/jsd_types.h | 6 +-- 8 files changed, 96 insertions(+), 96 deletions(-) rename src/{jsd_jed.c => jsd_jed0101.c} (65%) rename src/{jsd_jed.h => jsd_jed0101.h} (69%) create mode 100644 src/jsd_jed0101_pub.h rename src/{jsd_jed_types.h => jsd_jed0101_types.h} (70%) delete mode 100644 src/jsd_jed_pub.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 156a7ff..f896a28 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -7,7 +7,7 @@ add_library(jsd-lib STATIC jsd_el2124.c jsd_egd.c jsd_el3356.c - jsd_jed.c + jsd_jed0101.c jsd_timer.c jsd_ati_fts.c jsd_el3104.c diff --git a/src/jsd.c b/src/jsd.c index e753d92..79e206f 100644 --- a/src/jsd.c +++ b/src/jsd.c @@ -15,7 +15,7 @@ #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_print.h" #include "jsd/jsd_sdo.h" @@ -464,8 +464,8 @@ 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_ATI_FTS_PRODUCT_CODE: { diff --git a/src/jsd_jed.c b/src/jsd_jed0101.c similarity index 65% rename from src/jsd_jed.c rename to src/jsd_jed0101.c index 6fc96d3..b625109 100644 --- a/src/jsd_jed.c +++ b/src/jsd_jed0101.c @@ -1,4 +1,4 @@ -#include "jsd/jsd_jed.h" +#include "jsd/jsd_jed0101.h" #include @@ -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,9 +61,9 @@ 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; @@ -71,16 +71,16 @@ bool jsd_jed_init(jsd_t* self, uint16_t 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; diff --git a/src/jsd_jed.h b/src/jsd_jed0101.h similarity index 69% rename from src/jsd_jed.h rename to src/jsd_jed0101.h index 8565e34..1d4a127 100644 --- a/src/jsd_jed.h +++ b/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 } diff --git a/src/jsd_jed0101_pub.h b/src/jsd_jed0101_pub.h new file mode 100644 index 0000000..48f2d10 --- /dev/null +++ b/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 diff --git a/src/jsd_jed_types.h b/src/jsd_jed0101_types.h similarity index 70% rename from src/jsd_jed_types.h rename to src/jsd_jed0101_types.h index d378db7..418f714 100644 --- a/src/jsd_jed_types.h +++ b/src/jsd_jed0101_types.h @@ -1,5 +1,5 @@ -#ifndef JSD_JED_TYPES_H -#define JSD_JED_TYPES_H +#ifndef JSD_JED0101_TYPES_H +#define JSD_JED0101_TYPES_H #ifdef __cplusplus extern "C" { @@ -7,17 +7,17 @@ extern "C" { #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 } diff --git a/src/jsd_jed_pub.h b/src/jsd_jed_pub.h deleted file mode 100644 index c6986f2..0000000 --- a/src/jsd_jed_pub.h +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef JSD_JED_PUB_H -#define JSD_JED_PUB_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "jsd/jsd_pub.h" - -/** - * @brief Read the JED state - * - * @param self pointer to JSD context - * @param slave_id slave id of JED - * @return Pointer to JED state - */ -const jsd_jed_state_t* jsd_jed_get_state(jsd_t* self, uint16_t slave_id); - -/** - * @brief Sets the cmd PDO register on the JED board - * - * @param self pointer to JSD context - * @param slave_id id of JED - * @param cmd Command integer value - */ -void jsd_jed_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 JED - */ -void jsd_jed_read(jsd_t* self, uint16_t slave_id); - -/** - * @brief Processes Async SDO responses - * - * @param self pointer to JSD context - * @param slave_id id of JED - */ -void jsd_jed_process(jsd_t* self, uint16_t slave_id); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/src/jsd_types.h b/src/jsd_types.h index 6b711a1..28bc29c 100644 --- a/src/jsd_types.h +++ b/src/jsd_types.h @@ -18,7 +18,7 @@ extern "C" { #include "jsd/jsd_el3318_types.h" #include "jsd/jsd_el3356_types.h" #include "jsd/jsd_el3602_types.h" -#include "jsd/jsd_jed_types.h" +#include "jsd/jsd_jed0101_types.h" typedef struct { bool configuration_active; @@ -30,7 +30,7 @@ typedef struct { jsd_el2124_config_t el2124; jsd_egd_config_t egd; jsd_el3356_config_t el3356; - jsd_jed_config_t jed; + jsd_jed0101_config_t jed0101; jsd_ati_fts_config_t ati_fts; jsd_el3104_config_t el3104; jsd_el3202_config_t el3202; @@ -47,7 +47,7 @@ typedef struct { jsd_el2124_state_t el2124; jsd_egd_private_state_t egd; jsd_el3356_state_t el3356; - jsd_jed_state_t jed; + jsd_jed0101_state_t jed0101; jsd_ati_fts_state_t ati_fts; jsd_el3104_state_t el3104; jsd_el3202_state_t el3202; From fb92e500f8435262b0e42d1df7cc59ed13406cbc Mon Sep 17 00:00:00 2001 From: Joseph Bowkett Date: Sat, 11 Dec 2021 21:25:13 -0800 Subject: [PATCH 2/7] Cleaning up jed0101 tests --- test/CMakeLists.txt | 6 ++--- .../{jsd_jed_test.c => jsd_jed0101_test.c} | 22 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) rename test/device/{jsd_jed_test.c => jsd_jed0101_test.c} (78%) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 09f2b40..e26622e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -68,10 +68,10 @@ if(BUILD_JSD_TESTS) jsd_test_utils.c) target_link_libraries(jsd_el3356_test ${jsd_test_libs}) - add_executable(jsd_jed_test - device/jsd_jed_test.c + add_executable(jsd_jed0101_test + device/jsd_jed0101_test.c jsd_test_utils.c) - target_link_libraries(jsd_jed_test ${jsd_test_libs}) + target_link_libraries(jsd_jed0101_test ${jsd_test_libs}) add_executable(jsd_ati_fts_test device/jsd_ati_fts_test.c diff --git a/test/device/jsd_jed_test.c b/test/device/jsd_jed0101_test.c similarity index 78% rename from test/device/jsd_jed_test.c rename to test/device/jsd_jed0101_test.c index 11aee76..3d6c354 100644 --- a/test/device/jsd_jed_test.c +++ b/test/device/jsd_jed0101_test.c @@ -1,7 +1,7 @@ #include #include -#include "jsd/jsd_jed_pub.h" +#include "jsd/jsd_jed0101_pub.h" #include "jsd_test_utils.h" extern bool quit; @@ -28,7 +28,7 @@ void telemetry_data(void* self) { } single_device_server_t* sds = (single_device_server_t*)self; - const jsd_jed_state_t* state = jsd_jed_get_state(sds->jsd, slave_id); + const jsd_jed0101_state_t* state = jsd_jed0101_get_state(sds->jsd, slave_id); fprintf(file, "%u,", state->status); fprintf(file, "%u,", state->w_raw); @@ -49,7 +49,7 @@ void print_info(void* self) { assert(self); single_device_server_t* sds = (single_device_server_t*)self; - const jsd_jed_state_t* state = jsd_jed_get_state(sds->jsd, slave_id); + const jsd_jed0101_state_t* state = jsd_jed0101_get_state(sds->jsd, slave_id); MSG("Status: %u (Qw: %lf, Qx: %lf, Qy: %lf, Qz: %lf) Cmd: %u", state->status, state->w, state->x, state->y, state->z, state->cmd); @@ -59,22 +59,22 @@ void extract_data(void* self) { assert(self); single_device_server_t* sds = (single_device_server_t*)self; - jsd_jed_read(sds->jsd, slave_id); - jsd_jed_process(sds->jsd, slave_id); + jsd_jed0101_read(sds->jsd, slave_id); + jsd_jed0101_process(sds->jsd, slave_id); } void command(void* self) { assert(self); single_device_server_t* sds = (single_device_server_t*)self; - jsd_jed_set_cmd_value(sds->jsd, slave_id, cmd++ % 255); + jsd_jed0101_set_cmd_value(sds->jsd, slave_id, cmd++ % 255); } int main(int argc, char const* argv[]) { if (argc != 4) { ERROR("Expecting exactly 3 arguments"); - MSG("Usage: jsd_jed_test "); - MSG("Example: $ jsd_jed_test eth0 5 500"); + MSG("Usage: jsd_jed0101_test "); + MSG("Example: $ jsd_jed0101_test eth0 5 500"); return 0; } @@ -99,10 +99,10 @@ int main(int argc, char const* argv[]) { snprintf(my_config.name, JSD_NAME_LEN, "bigfoot"); my_config.configuration_active = true; - my_config.product_code = JSD_JED_PRODUCT_CODE; - my_config.jed.initial_cmd = 0; + my_config.product_code = JSD_JED0101_PRODUCT_CODE; + my_config.jed0101.initial_cmd = 0; jsd_set_slave_config(sds.jsd, slave_id, my_config); - sds_run(&sds, ifname, "/tmp/jsd_jed.csv"); + sds_run(&sds, ifname, "/tmp/jsd_jed0101.csv"); return 0; } From 78a18f8c6d894030ce1c398c6277636fc43cdedb Mon Sep 17 00:00:00 2001 From: Joseph Bowkett Date: Sat, 11 Dec 2021 21:39:06 -0800 Subject: [PATCH 3/7] Created jed0200 as copy of jed0101 --- src/CMakeLists.txt | 1 + src/jsd.c | 5 ++ src/jsd_jed0200.c | 107 ++++++++++++++++++++++++++++++++++++++++ src/jsd_jed0200.h | 54 ++++++++++++++++++++ src/jsd_jed0200_pub.h | 48 ++++++++++++++++++ src/jsd_jed0200_types.h | 40 +++++++++++++++ src/jsd_types.h | 3 ++ 7 files changed, 258 insertions(+) create mode 100644 src/jsd_jed0200.c create mode 100644 src/jsd_jed0200.h create mode 100644 src/jsd_jed0200_pub.h create mode 100644 src/jsd_jed0200_types.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f896a28..f2ea7ae 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -8,6 +8,7 @@ add_library(jsd-lib STATIC jsd_egd.c jsd_el3356.c jsd_jed0101.c + jsd_jed0200.c jsd_timer.c jsd_ati_fts.c jsd_el3104.c diff --git a/src/jsd.c b/src/jsd.c index 79e206f..5c8252f 100644 --- a/src/jsd.c +++ b/src/jsd.c @@ -16,6 +16,7 @@ #include "jsd/jsd_el3356.h" #include "jsd/jsd_el3602.h" #include "jsd/jsd_jed0101.h" +#include "jsd/jsd_jed0200.h" #include "jsd/jsd_print.h" #include "jsd/jsd_sdo.h" @@ -468,6 +469,10 @@ bool jsd_init_single_device(jsd_t* self, uint16_t slave_id) { 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: { return jsd_ati_fts_init(self, slave_id); break; diff --git a/src/jsd_jed0200.c b/src/jsd_jed0200.c new file mode 100644 index 0000000..3432b93 --- /dev/null +++ b/src/jsd_jed0200.c @@ -0,0 +1,107 @@ +#include "jsd/jsd_jed0200.h" + +#include + +#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->w_raw = txpdo->w; + state->x_raw = txpdo->x; + state->y_raw = txpdo->y; + state->z_raw = txpdo->z; + state->w = (double)txpdo->w / 1000.0; + state->x = (double)txpdo->x / 1000.0; + state->y = (double)txpdo->y / 1000.0; + state->z = (double)txpdo->z / 1000.0; +} + +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; +} diff --git a/src/jsd_jed0200.h b/src/jsd_jed0200.h new file mode 100644 index 0000000..c0f2a7b --- /dev/null +++ b/src/jsd_jed0200.h @@ -0,0 +1,54 @@ +#ifndef JSD_JED0200_H +#define JSD_JED0200_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "jsd/jsd_jed0200_pub.h" + +/** + * @brief TxPDO struct used to read device data in SOEM IOmap + * + * Note: Struct order matters and must be packed. + */ +typedef struct __attribute__((__packed__)) { + uint16_t status; + uint32_t w; + uint32_t x; + uint32_t y; + uint32_t z; +} jsd_jed0200_txpdo_t; + +/** + * @brief RxPDO struct used to command the device via SOEM IOmap + * + * Note: Struct order matters and must be packed. + */ +typedef struct __attribute__((__packed__)) { + uint16_t cmd; +} jsd_jed0200_rxpdo_t; + +/** @brief Initializes jed0200 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_jed0200_init(jsd_t* self, uint16_t slave_id); + +/** + * @brief Configuration function called by SOEM upon a PreOp to SafeOp state + * transition that (re)configures JED0200 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_jed0200_PO2SO_config(ecx_contextt* ecx_context, uint16_t slave_id); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/jsd_jed0200_pub.h b/src/jsd_jed0200_pub.h new file mode 100644 index 0000000..d3d34fd --- /dev/null +++ b/src/jsd_jed0200_pub.h @@ -0,0 +1,48 @@ +#ifndef JSD_JED0200_PUB_H +#define JSD_JED0200_PUB_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "jsd/jsd_pub.h" + +/** + * @brief Read the JED0200 state + * + * @param self pointer to JSD context + * @param slave_id slave id of JED0200 + * @return Pointer to JED0200 state + */ +const jsd_jed0200_state_t* jsd_jed0200_get_state(jsd_t* self, uint16_t slave_id); + +/** + * @brief Sets the cmd PDO register on the JED0200 board + * + * @param self pointer to JSD context + * @param slave_id id of JED0200 + * @param cmd Command integer value + */ +void jsd_jed0200_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 JED0200 + */ +void jsd_jed0200_read(jsd_t* self, uint16_t slave_id); + +/** + * @brief Processes Async SDO responses + * + * @param self pointer to JSD context + * @param slave_id id of JED0200 + */ +void jsd_jed0200_process(jsd_t* self, uint16_t slave_id); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/jsd_jed0200_types.h b/src/jsd_jed0200_types.h new file mode 100644 index 0000000..f1571d3 --- /dev/null +++ b/src/jsd_jed0200_types.h @@ -0,0 +1,40 @@ +#ifndef JSD_JED0200_TYPES_H +#define JSD_JED0200_TYPES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "jsd/jsd_common_device_types.h" + +#define JSD_JED0200_PRODUCT_CODE (uint32_t)0x00009252 + +/** + * @brief configuration struct for JED0200 device initialization + */ +typedef struct { + uint16_t initial_cmd; ///< Initial cmd value +} jsd_jed0200_config_t; + +/** + * @brief state data for JED0200 module + */ + +typedef struct { + uint16_t status; ///< Status value placeholder + uint32_t w_raw; ///< raw w component of IMU quaternion + uint32_t x_raw; ///< raw x component of IMU quaternion + uint32_t y_raw; ///< raw y component of IMU quaternion + uint32_t z_raw; ///< raw z component of IMU quaternion + double w; ///< converted w component of IMU quaternion + 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 JED0200 +} jsd_jed0200_state_t; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/jsd_types.h b/src/jsd_types.h index 28bc29c..e6e343f 100644 --- a/src/jsd_types.h +++ b/src/jsd_types.h @@ -19,6 +19,7 @@ extern "C" { #include "jsd/jsd_el3356_types.h" #include "jsd/jsd_el3602_types.h" #include "jsd/jsd_jed0101_types.h" +#include "jsd/jsd_jed0200_types.h" typedef struct { bool configuration_active; @@ -31,6 +32,7 @@ typedef struct { jsd_egd_config_t egd; jsd_el3356_config_t el3356; jsd_jed0101_config_t jed0101; + jsd_jed0200_config_t jed0200; jsd_ati_fts_config_t ati_fts; jsd_el3104_config_t el3104; jsd_el3202_config_t el3202; @@ -48,6 +50,7 @@ typedef struct { jsd_egd_private_state_t egd; jsd_el3356_state_t el3356; jsd_jed0101_state_t jed0101; + jsd_jed0200_state_t jed0200; jsd_ati_fts_state_t ati_fts; jsd_el3104_state_t el3104; jsd_el3202_state_t el3202; From 48c37bb7e9929b2d9249bb8c4f364f8a1a9433a1 Mon Sep 17 00:00:00 2001 From: Joseph Bowkett Date: Sat, 11 Dec 2021 21:54:30 -0800 Subject: [PATCH 4/7] Updated jed0200 fields --- src/jsd_jed0200.c | 18 ++++++++++-------- src/jsd_jed0200.h | 16 +++++++++++----- src/jsd_jed0200_types.h | 23 ++++++++++++----------- 3 files changed, 33 insertions(+), 24 deletions(-) diff --git a/src/jsd_jed0200.c b/src/jsd_jed0200.c index 3432b93..b36d227 100644 --- a/src/jsd_jed0200.c +++ b/src/jsd_jed0200.c @@ -34,14 +34,16 @@ void jsd_jed0200_read(jsd_t* self, uint16_t slave_id) { (jsd_jed0200_txpdo_t*)self->ecx_context.slavelist[slave_id].inputs; state->status = txpdo->status; - state->w_raw = txpdo->w; - state->x_raw = txpdo->x; - state->y_raw = txpdo->y; - state->z_raw = txpdo->z; - state->w = (double)txpdo->w / 1000.0; - state->x = (double)txpdo->x / 1000.0; - state->y = (double)txpdo->y / 1000.0; - state->z = (double)txpdo->z / 1000.0; + 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) { diff --git a/src/jsd_jed0200.h b/src/jsd_jed0200.h index c0f2a7b..04692ba 100644 --- a/src/jsd_jed0200.h +++ b/src/jsd_jed0200.h @@ -13,11 +13,17 @@ extern "C" { * Note: Struct order matters and must be packed. */ typedef struct __attribute__((__packed__)) { - uint16_t status; - uint32_t w; - uint32_t x; - uint32_t y; - uint32_t z; + uint16_t status; ///< Status value placeholder + uint32_t ticks; ///< Ticks + float voltage_hv; ///< High voltage + float voltage_lv; ///< Low voltage + float voltage_12v; ///< 12v voltage + float temp_ambient; ///< Ambient temperature + float temp_actuator; ///< Actuator temperature + float humidity; ///< Humidity + float pressure; ///< Pressure + uint16_t brake_current; ///< Brake current + uint16_t brake_cc_val; ///< Brake CC val } jsd_jed0200_txpdo_t; /** diff --git a/src/jsd_jed0200_types.h b/src/jsd_jed0200_types.h index f1571d3..5eab15c 100644 --- a/src/jsd_jed0200_types.h +++ b/src/jsd_jed0200_types.h @@ -7,7 +7,7 @@ extern "C" { #include "jsd/jsd_common_device_types.h" -#define JSD_JED0200_PRODUCT_CODE (uint32_t)0x00009252 +#define JSD_JED0200_PRODUCT_CODE (uint32_t)0x00001001 /** * @brief configuration struct for JED0200 device initialization @@ -21,16 +21,17 @@ typedef struct { */ typedef struct { - uint16_t status; ///< Status value placeholder - uint32_t w_raw; ///< raw w component of IMU quaternion - uint32_t x_raw; ///< raw x component of IMU quaternion - uint32_t y_raw; ///< raw y component of IMU quaternion - uint32_t z_raw; ///< raw z component of IMU quaternion - double w; ///< converted w component of IMU quaternion - 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 JED0200 + uint16_t status; ///< Status value placeholder + uint32_t ticks; ///< Ticks + float voltage_hv; ///< High voltage + float voltage_lv; ///< Low voltage + float voltage_12v; ///< 12v voltage + float temp_ambient; ///< Ambient temperature + float temp_actuator; ///< Actuator temperature + float humidity; ///< Humidity + float pressure; ///< Pressure + uint16_t brake_current; ///< Brake current + uint16_t brake_cc_val; ///< Brake CC val } jsd_jed0200_state_t; #ifdef __cplusplus From 825b04c0106b5b4cd381504cf2c94324abd166b7 Mon Sep 17 00:00:00 2001 From: Joseph Bowkett Date: Sat, 11 Dec 2021 21:56:28 -0800 Subject: [PATCH 5/7] Add cmd back into jed0200 status --- src/jsd_jed0200_types.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/jsd_jed0200_types.h b/src/jsd_jed0200_types.h index 5eab15c..69d6a1d 100644 --- a/src/jsd_jed0200_types.h +++ b/src/jsd_jed0200_types.h @@ -32,6 +32,7 @@ typedef struct { float pressure; ///< Pressure uint16_t brake_current; ///< Brake current uint16_t brake_cc_val; ///< Brake CC val + uint16_t cmd; ///< User specified cmd mode sent to JED0200 } jsd_jed0200_state_t; #ifdef __cplusplus From b52bf658880f1a36cb6830626b327c761d5e736f Mon Sep 17 00:00:00 2001 From: Joseph Bowkett Date: Sun, 12 Dec 2021 00:28:07 -0800 Subject: [PATCH 6/7] Added test for jed0200 --- test/CMakeLists.txt | 5 ++ test/device/jsd_jed0200_test.c | 110 +++++++++++++++++++++++++++++++++ 2 files changed, 115 insertions(+) create mode 100644 test/device/jsd_jed0200_test.c diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e26622e..6a7f90b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -73,6 +73,11 @@ if(BUILD_JSD_TESTS) jsd_test_utils.c) target_link_libraries(jsd_jed0101_test ${jsd_test_libs}) + add_executable(jsd_jed0200_test + device/jsd_jed0200_test.c + jsd_test_utils.c) + target_link_libraries(jsd_jed0200_test ${jsd_test_libs}) + add_executable(jsd_ati_fts_test device/jsd_ati_fts_test.c jsd_test_utils.c) diff --git a/test/device/jsd_jed0200_test.c b/test/device/jsd_jed0200_test.c new file mode 100644 index 0000000..aeffba8 --- /dev/null +++ b/test/device/jsd_jed0200_test.c @@ -0,0 +1,110 @@ +#include +#include + +#include "jsd/jsd_jed0200_pub.h" +#include "jsd_test_utils.h" + +extern bool quit; +extern FILE* file; +uint8_t slave_id; +uint16_t cmd = 0; + +void telemetry_header() { + if (!file) { + return; + } + + fprintf(file, "status, "); + fprintf(file, "w_raw, x_raw, y_raw, z_raw, "); + fprintf(file, "w, x, y, z, "); + fprintf(file, "cmd, "); + 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_jed0200_state_t* state = jsd_jed0200_get_state(sds->jsd, slave_id); + + fprintf(file, "%u, ", state->status); + fprintf(file, "%u, ", state->ticks); + fprintf(file, "%u, ", state->voltage_hv); + fprintf(file, "%u, ", state->voltage_lv); + fprintf(file, "%u, ", state->voltage_12v); + fprintf(file, "%u, ", state->temp_ambient); + fprintf(file, "%u, ", state->temp_actuator); + fprintf(file, "%u, ", state->humidity); + fprintf(file, "%u, ", state->pressure); + fprintf(file, "%u, ", state->brake_current); + fprintf(file, "%u, ", state->brake_cc_val); + fprintf(file, "%u, ", state->cmd); + + fprintf(file, "\n"); + fflush(file); +} + +void print_info(void* self) { + assert(self); + + single_device_server_t* sds = (single_device_server_t*)self; + const jsd_jed0200_state_t* state = jsd_jed0200_get_state(sds->jsd, slave_id); + + MSG("Status: %u (Qw: %lf, Qx: %lf, Qy: %lf, Qz: %lf) Cmd: %u", state->status, + state->w, state->x, state->y, state->z, state->cmd); +} + +void extract_data(void* self) { + assert(self); + single_device_server_t* sds = (single_device_server_t*)self; + + jsd_jed0200_read(sds->jsd, slave_id); + jsd_jed0200_process(sds->jsd, slave_id); +} + +void command(void* self) { + assert(self); + single_device_server_t* sds = (single_device_server_t*)self; + + jsd_jed0200_set_cmd_value(sds->jsd, slave_id, cmd++ % 255); +} + +int main(int argc, char const* argv[]) { + if (argc != 4) { + ERROR("Expecting exactly 3 arguments"); + MSG("Usage: jsd_jed0200_test "); + MSG("Example: $ jsd_jed0200_test 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_JED0200_PRODUCT_CODE; + my_config.jed0200.initial_cmd = 0; + jsd_set_slave_config(sds.jsd, slave_id, my_config); + sds_run(&sds, ifname, "/tmp/jsd_jed0200.csv"); + + return 0; +} From e06bd09c2d21428d60a183b38b37d6845fb20597 Mon Sep 17 00:00:00 2001 From: Joseph Bowkett Date: Sun, 12 Dec 2021 00:51:49 -0800 Subject: [PATCH 7/7] Update jed0200 test fields --- test/device/jsd_jed0200_test.c | 41 ++++++++++++++++++++++++---------- 1 file changed, 29 insertions(+), 12 deletions(-) diff --git a/test/device/jsd_jed0200_test.c b/test/device/jsd_jed0200_test.c index aeffba8..952a8a3 100644 --- a/test/device/jsd_jed0200_test.c +++ b/test/device/jsd_jed0200_test.c @@ -15,8 +15,16 @@ void telemetry_header() { } fprintf(file, "status, "); - fprintf(file, "w_raw, x_raw, y_raw, z_raw, "); - fprintf(file, "w, x, y, z, "); + fprintf(file, "ticks, "); + fprintf(file, "voltage_hv, "); + fprintf(file, "voltage_lv, "); + fprintf(file, "voltage_12v, "); + fprintf(file, "temp_ambient, "); + fprintf(file, "temp_actuator, "); + fprintf(file, "humidity, "); + fprintf(file, "pressure, "); + fprintf(file, "brake_current, "); + fprintf(file, "brake_cc_vel, "); fprintf(file, "cmd, "); fprintf(file, "\n"); } @@ -32,13 +40,13 @@ void telemetry_data(void* self) { fprintf(file, "%u, ", state->status); fprintf(file, "%u, ", state->ticks); - fprintf(file, "%u, ", state->voltage_hv); - fprintf(file, "%u, ", state->voltage_lv); - fprintf(file, "%u, ", state->voltage_12v); - fprintf(file, "%u, ", state->temp_ambient); - fprintf(file, "%u, ", state->temp_actuator); - fprintf(file, "%u, ", state->humidity); - fprintf(file, "%u, ", state->pressure); + fprintf(file, "%f, ", state->voltage_hv); + fprintf(file, "%f, ", state->voltage_lv); + fprintf(file, "%f, ", state->voltage_12v); + fprintf(file, "%f, ", state->temp_ambient); + fprintf(file, "%f, ", state->temp_actuator); + fprintf(file, "%f, ", state->humidity); + fprintf(file, "%f, ", state->pressure); fprintf(file, "%u, ", state->brake_current); fprintf(file, "%u, ", state->brake_cc_val); fprintf(file, "%u, ", state->cmd); @@ -52,9 +60,18 @@ void print_info(void* self) { single_device_server_t* sds = (single_device_server_t*)self; const jsd_jed0200_state_t* state = jsd_jed0200_get_state(sds->jsd, slave_id); - - MSG("Status: %u (Qw: %lf, Qx: %lf, Qy: %lf, Qz: %lf) Cmd: %u", state->status, - state->w, state->x, state->y, state->z, state->cmd); + MSG("status: %u, ", state->status); + MSG("ticks: %u, ", state->ticks); + MSG("voltage_hv: %f, ", state->voltage_hv); + MSG("voltage_lv: %f, ", state->voltage_lv); + MSG("voltage_12v: %f, ", state->voltage_12v); + MSG("temp_ambient: %f, ", state->temp_ambient); + MSG("temp_actuator: %f, ", state->temp_actuator); + MSG("humidity: %f, ", state->humidity); + MSG("pressure: %f, ", state->pressure); + MSG("brake_current: %u, ", state->brake_current); + MSG("brake_cc_vel: %u, ", state->brake_cc_val); + MSG("cmd: %u, ", state->cmd); } void extract_data(void* self) {