diff --git a/CMakeLists.txt b/CMakeLists.txt index 494da40..b9ae2ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -158,12 +158,10 @@ install(FILES ) if(SOEM_BUILD_SAMPLES) - add_subdirectory(samples/coetest) + add_subdirectory(samples/ec_sample) add_subdirectory(samples/eepromtool) add_subdirectory(samples/firm_update) - add_subdirectory(samples/red_test) add_subdirectory(samples/simple_ng) - add_subdirectory(samples/simple_test) add_subdirectory(samples/slaveinfo) if (${CMAKE_SYSTEM_NAME} STREQUAL Linux) diff --git a/cmake/Linux.cmake b/cmake/Linux.cmake index ec2bc51..2254068 100644 --- a/cmake/Linux.cmake +++ b/cmake/Linux.cmake @@ -18,14 +18,12 @@ target_include_directories(soem PUBLIC foreach(target IN ITEMS soem - coetest + ec_sample eepromtool eni_test eoe_test firm_update - red_test simple_ng - simple_test slaveinfo) if (TARGET ${target}) target_compile_options(${target} PRIVATE diff --git a/cmake/Windows.cmake b/cmake/Windows.cmake index 874424d..cfd4d73 100644 --- a/cmake/Windows.cmake +++ b/cmake/Windows.cmake @@ -31,14 +31,12 @@ target_link_libraries(soem PUBLIC foreach(target IN ITEMS soem - coetest + ec_sample eepromtool eni_test eoe_test firm_update - red_test simple_ng - simple_test slaveinfo) if (TARGET ${target}) target_compile_options(${target} PRIVATE diff --git a/cmake/rt-kernel.cmake b/cmake/rt-kernel.cmake index 77f5503..6a5aeb4 100644 --- a/cmake/rt-kernel.cmake +++ b/cmake/rt-kernel.cmake @@ -28,14 +28,12 @@ target_include_directories(soem PUBLIC foreach(target IN ITEMS soem - coetest + ec_sample eepromtool eni_test eoe_test firm_update - red_test simple_ng - simple_test slaveinfo) if (TARGET ${target}) target_compile_options(${target} PRIVATE diff --git a/samples/coetest/CMakeLists.txt b/samples/coetest/CMakeLists.txt deleted file mode 100644 index b3d5b6f..0000000 --- a/samples/coetest/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -add_executable(coetest coetest.c) -target_link_libraries(coetest soem) -install(TARGETS coetest DESTINATION bin) diff --git a/samples/ec_sample/CMakeLists.txt b/samples/ec_sample/CMakeLists.txt new file mode 100644 index 0000000..0c72def --- /dev/null +++ b/samples/ec_sample/CMakeLists.txt @@ -0,0 +1,3 @@ +add_executable(ec_sample ec_sample.c) +target_link_libraries(ec_sample soem) +install(TARGETS ec_sample DESTINATION bin) diff --git a/samples/coetest/coetest.c b/samples/ec_sample/ec_sample.c similarity index 66% rename from samples/coetest/coetest.c rename to samples/ec_sample/ec_sample.c index d91e8d5..1761661 100644 --- a/samples/coetest/coetest.c +++ b/samples/ec_sample/ec_sample.c @@ -1,26 +1,27 @@ -/** \file - * \brief CoE example code for Simple Open EtherCAT master - * - * Usage : coetest [ifname1] - * ifname is NIC interface, f.e. eth0 - * - * (c)Arthur Ketels 2010 - 2024 +/* + * This software is dual-licensed under GPLv3 and a commercial + * license. See the file LICENSE.md distributed with this software for + * full license information. */ #include +#include #include #include #include "soem/soem.h" #define EC_TIMEOUTMON 500 + #define NSEC_PER_SEC 1000000000 static uint8 IOmap[4096]; static OSAL_THREAD_HANDLE threadrt, thread1; static int expectedWKC; +static int wkc; static int mappingdone, dorun, inOP, dowkccheck; -static int adapterisbound, conf_io_size, currentgroup; +static int currentgroup = 0; +static int cycle = 0; static int64_t cycletime = 1000000; static ecx_contextt ctx; @@ -39,7 +40,7 @@ static float pgain = 0.01f; static float igain = 0.00002f; /* set linux sync point 500us later than DC sync, just as example */ static int64 syncoffset = 500000; -int64 timeerror; +static int64 timeerror; /* PI calculation to get linux time synced to DC time */ void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime) @@ -56,11 +57,11 @@ void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime) *offsettime = (int64)((timeerror * pgain) + (integral * igain)); } -/* RT EtherCAT thread */ +/* Cyclic RT EtherCAT thread */ OSAL_THREAD_FUNC_RT ecatthread(void) { ec_timet ts; - int ht, wkc; + int ht; static int64_t toff = 0; dorun = 0; @@ -80,11 +81,13 @@ OSAL_THREAD_FUNC_RT ecatthread(void) osal_monotonic_sleep(&ts); if (dorun > 0) { + cycle++; wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); if (wkc != expectedWKC) dowkccheck++; else dowkccheck = 0; + if (ctx.slavelist[0].hasdc && (wkc > 0)) { /* calculate toff to get linux time and DC synced */ @@ -96,6 +99,7 @@ OSAL_THREAD_FUNC_RT ecatthread(void) } } +/* Slave error handler */ OSAL_THREAD_FUNC ecatcheck(void) { int slaveix; @@ -147,7 +151,6 @@ OSAL_THREAD_FUNC ecatcheck(void) if (slave->Ibytes) { memset(slave->inputs, 0x00, slave->Ibytes); - printf("zero inputs %p %d\n\r", slave->inputs, slave->Ibytes); } printf("ERROR : slave %d lost\n", slaveix); } @@ -178,59 +181,127 @@ OSAL_THREAD_FUNC ecatcheck(void) } } -void ethercatstartup(char *ifname) +/* Transition network to operational state */ +void ecatbringup(char *ifname) { printf("EtherCAT Startup\n"); int rv = ecx_init(&ctx, ifname); if (rv) { - adapterisbound = 1; ecx_config_init(&ctx); if (ctx.slavecount > 0) { ec_groupt *group = &ctx.grouplist[0]; - conf_io_size = ecx_config_map_group(&ctx, IOmap, 0); + ecx_config_map_group(&ctx, IOmap, 0); expectedWKC = (group->outputsWKC * 2) + group->inputsWKC; + printf("%d slaves found and configured.\n", ctx.slavecount); + + printf("segments : %d : %d %d %d %d\n", + group->nsegments, + group->IOsegment[0], + group->IOsegment[1], + group->IOsegment[2], + group->IOsegment[3]); + + /* Configure distributed clocks */ mappingdone = 1; ecx_configdc(&ctx); + + /* Add all CoE slaves to cyclic mailbox handler */ int sdoslave = -1; for (int si = 1; si <= ctx.slavecount; si++) { ec_slavet *slave = &ctx.slavelist[si]; - printf("Slave %d name:%s man:%8.8x id:%8.8x rev:%d ser:%d\n", si, slave->name, slave->eep_man, slave->eep_id, slave->eep_rev, slave->eep_ser); if (slave->CoEdetails > 0) { ecx_slavembxcyclic(&ctx, si); sdoslave = si; - printf(" Slave added to cyclic mailbox handler\n"); + printf(" Slave %d added to cyclic mailbox handler\n", si); } } + + /* Let network sync to clocks */ dorun = 1; osal_usleep(1000000); + + /* Go to operational state */ ctx.slavelist[0].state = EC_STATE_OPERATIONAL; ecx_writestate(&ctx, 0); ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); - inOP = 1; - printf("EtherCAT OP\n"); - uint32_t aval = 0; - int avals = sizeof(aval); - for (int i = 0; i < 100; i++) + + if (ctx.slavelist[0].state != EC_STATE_OPERATIONAL) { - printf("Cycle %d timeerror %" PRId64 "\n", i, timeerror); - if (sdoslave > 0) + ecx_readstate(&ctx); + for (int si = 1; si <= ctx.slavecount; si++) { - aval = 0; - int rval = ecx_SDOread(&ctx, sdoslave, 0x1018, 0x02, FALSE, &avals, &aval, EC_TIMEOUTRXM); - printf(" Slave %d Rval %d Prodcode %8.8x\n", sdoslave, rval, aval); + ec_slavet *slave = &ctx.slavelist[si]; + if (slave->state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + si, + slave->state, + slave->ALstatuscode, + ec_ALstatuscode2string(slave->ALstatuscode)); + } } - osal_usleep(100000); } - inOP = 0; - printf("EtherCAT to safe-OP\n"); + else + { + int size; + + printf("EtherCAT OP\n"); + inOP = TRUE; + + /* acyclic loop 5000 x 20ms = 100s */ + for (int i = 0; i < 5000; i++) + { + printf("Processdata cycle %5d , Wck %3d, DCtime %12" PRId64 ", dt %8" PRId64 ", O:", + cycle, + wkc, + ctx.DCtime, + timeerror); + + size = group->Obytes < 8 ? group->Obytes : 8; + for (int j = 0; j < size; j++) + { + printf(" %2.2x", *(ctx.slavelist[0].outputs + j)); + } + + printf(" I:"); + size = group->Ibytes < 8 ? group->Ibytes : 8; + for (int j = 0; j < size; j++) + { + printf(" %2.2x", *(ctx.slavelist[0].inputs + j)); + } + + printf("\r"); + fflush(stdout); + + /* Demonstrate SDO access from other threads */ + uint32_t value = 0; + int size = sizeof(value); + if (sdoslave > 0) + { + value = 0; + int sdo_wkc = ecx_SDOread(&ctx, sdoslave, 0x1018, 0x02, FALSE, &size, &value, EC_TIMEOUTRXM); + (void)sdo_wkc; + } + + osal_usleep(20000); + } + printf("\n"); + dorun = 0; + inOP = FALSE; + } + + /* Go to SAFE_OP */ + printf("EtherCAT to SAFE_OP\n"); ctx.slavelist[0].state = EC_STATE_SAFE_OP; ecx_writestate(&ctx, 0); ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* Go to INIT state */ printf("EtherCAT to INIT\n"); ctx.slavelist[0].state = EC_STATE_INIT; ecx_writestate(&ctx, 0); @@ -241,22 +312,27 @@ void ethercatstartup(char *ifname) int main(int argc, char *argv[]) { - printf("SOEM (Simple Open EtherCAT Master)\nCoEtest\n"); + printf("SOEM (Simple Open EtherCAT Master)\nec_sample\n"); + + if (argc > 2) + cycletime = atoi(argv[2]) * 1000; if (argc > 1) { - /* create thread to handle slave error handling in OP */ + /* create process data thread */ osal_thread_create_rt(&threadrt, 128000, &ecatthread, NULL); /* create thread to handle slave error handling in OP */ osal_thread_create(&thread1, 128000, &ecatcheck, NULL); - /* start cyclic part */ - ethercatstartup(argv[1]); + /* bringup network */ + ecatbringup(argv[1]); } else { ec_adaptert *adapter = NULL; ec_adaptert *head = NULL; - printf("Usage: coetest ifname1\nifname = eth0 for example\n"); + printf("Usage: ec_sample ifname1 [cycletime]\n"); + printf("ifname = eth0 for example\n"); + printf("cycletime in us\n"); printf("\nAvailable adapters:\n"); head = adapter = ec_find_adapters(); diff --git a/samples/red_test/CMakeLists.txt b/samples/red_test/CMakeLists.txt deleted file mode 100644 index 6257622..0000000 --- a/samples/red_test/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -add_executable(red_test red_test.c) -target_link_libraries(red_test soem) -install(TARGETS red_test DESTINATION bin) diff --git a/samples/red_test/red_test.c b/samples/red_test/red_test.c deleted file mode 100644 index b2de8d0..0000000 --- a/samples/red_test/red_test.c +++ /dev/null @@ -1,356 +0,0 @@ -/** \file - * \brief Example code for Simple Open EtherCAT master - * - * Usage : red_test [ifname1] [ifname2] [cycletime] - * ifname is NIC interface, f.e. eth0 - * cycletime in us, f.e. 500 - * - * This is a redundancy test. - * - * (c)Arthur Ketels 2008 - */ - -#include -#include -#include -#include - -#include "soem/soem.h" - -#define EC_TIMEOUTMON 500 -#define NSEC_PER_SEC 1000000000 - -static uint8 IOmap[4096]; -static OSAL_THREAD_HANDLE thread1; -static OSAL_THREAD_HANDLE thread2; -static int expectedWKC; -static boolean needlf; -static volatile int wkc; -static boolean inOP; -static uint8 currentgroup = 0; -static uint8 *digout = NULL; -static int dorun = 0; -static int64 toff, gl_delta; - -static ecx_contextt ctx; - -void redtest(char *ifname, char *ifname2) -{ - int cnt, i, j, oloop, iloop; - - printf("Starting Redundant test\n"); - - /* initialise SOEM, bind socket to ifname */ - (void)ifname2; - // if (ecx_init_redundant(&ctx, ifname, ifname2)) - if (ecx_init(&ctx, ifname)) - { - printf("ecx_init on %s succeeded.\n", ifname); - - /* find and auto-config slaves */ - if (ecx_config_init(&ctx) > 0) - { - ec_groupt *group = &ctx.grouplist[0]; - - ecx_config_map_group(&ctx, IOmap, 0); - - printf("%d slaves found and configured.\n", ctx.slavecount); - - /* wait for all slaves to reach SAFE_OP state */ - ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - - ecx_configdc(&ctx); - - /* read indevidual slave state and store in ctx.slavelist[] */ - ecx_readstate(&ctx); - for (cnt = 1; cnt <= ctx.slavecount; cnt++) - { - printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", - cnt, ctx.slavelist[cnt].name, ctx.slavelist[cnt].Obits, ctx.slavelist[cnt].Ibits, - ctx.slavelist[cnt].state, (int)ctx.slavelist[cnt].pdelay, ctx.slavelist[cnt].hasdc); - printf(" Out:%p,%4d In:%p,%4d\n", - ctx.slavelist[cnt].outputs, ctx.slavelist[cnt].Obytes, ctx.slavelist[cnt].inputs, ctx.slavelist[cnt].Ibytes); - /* check for EL2004 or EL2008 */ - if (!digout && ((ctx.slavelist[cnt].eep_id == 0x0af83052) || (ctx.slavelist[cnt].eep_id == 0x07d83052))) - { - digout = ctx.slavelist[cnt].outputs; - } - } - expectedWKC = (group->outputsWKC * 2) + group->inputsWKC; - printf("Calculated workcounter %d\n", expectedWKC); - - printf("Request operational state for all slaves\n"); - ctx.slavelist[0].state = EC_STATE_OPERATIONAL; - /* request OP state for all slaves */ - ecx_writestate(&ctx, 0); - /* activate cyclic process data */ - dorun = 1; - /* wait for all slaves to reach OP state */ - ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); - - oloop = group->Obytes; - if (oloop > 8) oloop = 8; - iloop = group->Ibytes; - if (iloop > 8) iloop = 8; - - if (ctx.slavelist[0].state == EC_STATE_OPERATIONAL) - { - printf("Operational state reached for all slaves.\n"); - inOP = TRUE; - /* acyclic loop 5000 x 20ms = 10s */ - for (i = 1; i <= 5000; i++) - { - printf("Processdata cycle %5d , Wck %3d, DCtime %12" PRId64 ", dt %12" PRId64 ", O:", - dorun, wkc, ctx.DCtime, gl_delta); - for (j = 0; j < oloop; j++) - { - printf(" %2.2x", *(ctx.slavelist[0].outputs + j)); - } - printf(" I:"); - for (j = 0; j < iloop; j++) - { - printf(" %2.2x", *(ctx.slavelist[0].inputs + j)); - } - printf("\r"); - fflush(stdout); - osal_usleep(20000); - } - dorun = 0; - inOP = FALSE; - } - else - { - printf("Not all slaves reached operational state.\n"); - ecx_readstate(&ctx); - for (i = 1; i <= ctx.slavecount; i++) - { - ec_slavet *slave = &ctx.slavelist[i]; - if (slave->state != EC_STATE_OPERATIONAL) - { - printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", - i, slave->state, slave->ALstatuscode, ec_ALstatuscode2string(slave->ALstatuscode)); - } - } - } - printf("Request safe operational state for all slaves\n"); - ctx.slavelist[0].state = EC_STATE_SAFE_OP; - /* request SAFE_OP state for all slaves */ - ecx_writestate(&ctx, 0); - } - else - { - printf("No slaves found!\n"); - } - printf("End redundant test, close socket\n"); - /* stop SOEM, close socket */ - ecx_close(&ctx); - } - else - { - printf("No socket connection on %s\nExcecute as root\n", ifname); - } -} - -/* add ns to ec_timet */ -void add_time_ns(ec_timet *ts, int64 addtime) -{ - ec_timet addts; - - addts.tv_nsec = addtime % NSEC_PER_SEC; - addts.tv_sec = (addtime - addts.tv_nsec) / NSEC_PER_SEC; - osal_timespecadd(ts, &addts, ts); -} - -/* PI calculation to get linux time synced to DC time */ -void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime) -{ - static int64 integral = 0; - int64 delta; - /* set linux sync point 50us later than DC sync, just as example */ - delta = (reftime - 50000) % cycletime; - if (delta > (cycletime / 2)) - { - delta = delta - cycletime; - } - if (delta > 0) - { - integral++; - } - if (delta < 0) - { - integral--; - } - *offsettime = -(delta / 100) - (integral / 20); - gl_delta = delta; -} - -/* RT EtherCAT thread */ -OSAL_THREAD_FUNC_RT ecatthread(void *ptr) -{ - ec_timet ts; - int ht; - int64 cycletime; - - osal_get_monotonic_time(&ts); - ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ - ts.tv_nsec = ht * 1000000; - if (ts.tv_nsec >= NSEC_PER_SEC) - { - ts.tv_sec++; - ts.tv_nsec -= NSEC_PER_SEC; - } - cycletime = *(int *)ptr * 1000; /* cycletime in ns */ - toff = 0; - dorun = 0; - ecx_send_processdata(&ctx); - while (1) - { - /* calculate next cycle start */ - add_time_ns(&ts, cycletime + toff); - /* wait to cycle start */ - osal_monotonic_sleep(&ts); - if (dorun > 0) - { - wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - - dorun++; - /* if we have some digital output, cycle */ - if (digout) *digout = (uint8)((dorun / 16) & 0xff); - - if (ctx.slavelist[0].hasdc) - { - /* calculate toff to get linux time and DC synced */ - ec_sync(ctx.DCtime, cycletime, &toff); - } - ecx_send_processdata(&ctx); - } - } -} - -OSAL_THREAD_FUNC ecatcheck(void *ptr) -{ - int slaveix; - (void)ptr; /* Not used */ - - while (1) - { - if (inOP && ((wkc < expectedWKC) || ctx.grouplist[currentgroup].docheckstate)) - { - if (needlf) - { - needlf = FALSE; - printf("\n"); - } - /* one or more slaves are not responding */ - ctx.grouplist[currentgroup].docheckstate = FALSE; - ecx_readstate(&ctx); - for (slaveix = 1; slaveix <= ctx.slavecount; slaveix++) - { - ec_slavet *slave = &ctx.slavelist[slaveix]; - - if ((slave->group == currentgroup) && (slave->state != EC_STATE_OPERATIONAL)) - { - ctx.grouplist[currentgroup].docheckstate = TRUE; - if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) - { - printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slaveix); - slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK); - ecx_writestate(&ctx, slaveix); - } - else if (slave->state == EC_STATE_SAFE_OP) - { - printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slaveix); - slave->state = EC_STATE_OPERATIONAL; - if (slave->mbxhandlerstate == ECT_MBXH_LOST) slave->mbxhandlerstate = ECT_MBXH_CYCLIC; - ecx_writestate(&ctx, slaveix); - } - else if (slave->state > EC_STATE_NONE) - { - if (ecx_reconfig_slave(&ctx, slaveix, EC_TIMEOUTMON) >= EC_STATE_PRE_OP) - { - slave->islost = FALSE; - printf("MESSAGE : slave %d reconfigured\n", slaveix); - } - } - else if (!slave->islost) - { - /* re-check state */ - ecx_statecheck(&ctx, slaveix, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); - if (slave->state == EC_STATE_NONE) - { - slave->islost = TRUE; - slave->mbxhandlerstate = ECT_MBXH_LOST; - /* zero input data for this slave */ - if (slave->Ibytes) - { - memset(slave->inputs, 0x00, slave->Ibytes); - printf("zero inputs %p %d\n\r", slave->inputs, slave->Ibytes); - } - printf("ERROR : slave %d lost\n", slaveix); - } - } - } - if (slave->islost) - { - if (slave->state <= EC_STATE_INIT) - { - if (ecx_recover_slave(&ctx, slaveix, EC_TIMEOUTMON)) - { - slave->islost = FALSE; - printf("MESSAGE : slave %d recovered\n", slaveix); - } - } - else - { - slave->islost = FALSE; - printf("MESSAGE : slave %d found\n", slaveix); - } - } - } - if (!ctx.grouplist[currentgroup].docheckstate) - printf("OK : all slaves resumed OPERATIONAL.\n"); - } - osal_usleep(10000); - } -} - -int main(int argc, char *argv[]) -{ - int ctime; - - printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); - - if (argc > 3) - { - dorun = 0; - ctime = atoi(argv[3]); - - /* create RT thread */ - osal_thread_create_rt(&thread1, 128000, &ecatthread, (void *)&ctime); - - /* create thread to handle slave error handling in OP */ - osal_thread_create(&thread2, 128000, &ecatcheck, NULL); - - /* start acyclic part */ - redtest(argv[1], argv[2]); - } - else - { - ec_adaptert *adapter = NULL; - ec_adaptert *head = NULL; - printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); - - printf("\nAvailable adapters:\n"); - head = adapter = ec_find_adapters(); - while (adapter != NULL) - { - printf(" - %s (%s)\n", adapter->name, adapter->desc); - adapter = adapter->next; - } - ec_free_adapters(head); - } - - printf("End program\n"); - - return (0); -} diff --git a/samples/simple_test/CMakeLists.txt b/samples/simple_test/CMakeLists.txt deleted file mode 100644 index 5ef7327..0000000 --- a/samples/simple_test/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -add_executable(simple_test simple_test.c) -target_link_libraries(simple_test soem) -install(TARGETS simple_test DESTINATION bin) diff --git a/samples/simple_test/simple_test.c b/samples/simple_test/simple_test.c deleted file mode 100644 index b3c7ccf..0000000 --- a/samples/simple_test/simple_test.c +++ /dev/null @@ -1,262 +0,0 @@ -/** \file - * \brief Example code for Simple Open EtherCAT master - * - * Usage : simple_test [ifname1] - * ifname is NIC interface, f.e. eth0 - * - * This is a minimal test. - * - * (c)Arthur Ketels 2010 - 2011 - */ - -#include -#include -#include - -#include "soem/soem.h" - -#define EC_TIMEOUTMON 500 - -static uint8 IOmap[4096]; -static OSAL_THREAD_HANDLE thread1; -static int expectedWKC; -static boolean needlf; -static volatile int wkc; -static boolean inOP; -static uint8 currentgroup = 0; - -static ecx_contextt ctx; - -void simpletest(char *ifname) -{ - int i, j, oloop, iloop, chk; - needlf = FALSE; - inOP = FALSE; - - printf("Starting simple test\n"); - - /* initialise SOEM, bind socket to ifname */ - if (ecx_init(&ctx, ifname)) - { - printf("ecx_init on %s succeeded.\n", ifname); - - /* find and auto-config slaves */ - if (ecx_config_init(&ctx) > 0) - { - ec_groupt *group = &ctx.grouplist[0]; - - ecx_config_map_group(&ctx, IOmap, 0); - - ecx_configdc(&ctx); - - printf("%d slaves found and configured.\n", ctx.slavecount); - - /* wait for all slaves to reach SAFE_OP state */ - ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); - - oloop = group->Obytes; - if (oloop > 8) oloop = 8; - iloop = group->Ibytes; - if (iloop > 8) iloop = 8; - - printf("segments : %d : %d %d %d %d\n", group->nsegments, group->IOsegment[0], group->IOsegment[1], group->IOsegment[2], group->IOsegment[3]); - - printf("Request operational state for all slaves\n"); - expectedWKC = (group->outputsWKC * 2) + group->inputsWKC; - printf("Calculated workcounter %d\n", expectedWKC); - ctx.slavelist[0].state = EC_STATE_OPERATIONAL; - /* send one valid process data to make outputs in slaves happy*/ - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - /* request OP state for all slaves */ - ecx_writestate(&ctx, 0); - chk = 200; - /* wait for all slaves to reach OP state */ - do - { - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, 50000); - } while (chk-- && (ctx.slavelist[0].state != EC_STATE_OPERATIONAL)); - if (ctx.slavelist[0].state == EC_STATE_OPERATIONAL) - { - printf("Operational state reached for all slaves.\n"); - inOP = TRUE; - /* cyclic loop */ - for (i = 1; i <= 10000; i++) - { - ecx_send_processdata(&ctx); - wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - - if (wkc >= expectedWKC) - { - printf("Processdata cycle %4d, WKC %d , O:", i, wkc); - - for (j = 0; j < oloop; j++) - { - printf(" %2.2x", *(ctx.slavelist[0].outputs + j)); - } - - printf(" I:"); - for (j = 0; j < iloop; j++) - { - printf(" %2.2x", *(ctx.slavelist[0].inputs + j)); - } - printf(" T:%" PRId64 "\r", ctx.DCtime); - needlf = TRUE; - } - osal_usleep(5000); - } - inOP = FALSE; - } - else - { - printf("Not all slaves reached operational state.\n"); - ecx_readstate(&ctx); - for (i = 1; i <= ctx.slavecount; i++) - { - ec_slavet *slave = &ctx.slavelist[i]; - if (slave->state != EC_STATE_OPERATIONAL) - { - printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", - i, slave->state, slave->ALstatuscode, ec_ALstatuscode2string(slave->ALstatuscode)); - } - } - } - printf("\nRequest init state for all slaves\n"); - ctx.slavelist[0].state = EC_STATE_INIT; - /* request INIT state for all slaves */ - ecx_writestate(&ctx, 0); - } - else - { - printf("No slaves found!\n"); - } - printf("End simple test, close socket\n"); - /* stop SOEM, close socket */ - ecx_close(&ctx); - } - else - { - printf("No socket connection on %s\nExecute as root\n", ifname); - } -} - -OSAL_THREAD_FUNC ecatcheck(void *ptr) -{ - int slaveix; - (void)ptr; /* Not used */ - - while (1) - { - if (inOP && ((wkc < expectedWKC) || ctx.grouplist[currentgroup].docheckstate)) - { - printf("check wkc=%d/%d docheckstate=%d!\n", wkc, expectedWKC, ctx.grouplist[currentgroup].docheckstate); - if (needlf) - { - needlf = FALSE; - printf("\n"); - } - /* one or more slaves are not responding */ - ctx.grouplist[currentgroup].docheckstate = FALSE; - ecx_readstate(&ctx); - for (slaveix = 1; slaveix <= ctx.slavecount; slaveix++) - { - ec_slavet *slave = &ctx.slavelist[slaveix]; - - if ((slave->group == currentgroup) && (slave->state != EC_STATE_OPERATIONAL)) - { - ctx.grouplist[currentgroup].docheckstate = TRUE; - if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) - { - printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slaveix); - slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK); - ecx_writestate(&ctx, slaveix); - } - else if (slave->state == EC_STATE_SAFE_OP) - { - printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slaveix); - slave->state = EC_STATE_OPERATIONAL; - if (slave->mbxhandlerstate == ECT_MBXH_LOST) slave->mbxhandlerstate = ECT_MBXH_CYCLIC; - ecx_writestate(&ctx, slaveix); - } - else if (slave->state > EC_STATE_NONE) - { - if (ecx_reconfig_slave(&ctx, slaveix, EC_TIMEOUTMON) >= EC_STATE_PRE_OP) - { - slave->islost = FALSE; - printf("MESSAGE : slave %d reconfigured\n", slaveix); - } - } - else if (!slave->islost) - { - /* re-check state */ - ecx_statecheck(&ctx, slaveix, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); - if (slave->state == EC_STATE_NONE) - { - slave->islost = TRUE; - slave->mbxhandlerstate = ECT_MBXH_LOST; - /* zero input data for this slave */ - if (slave->Ibytes) - { - memset(slave->inputs, 0x00, slave->Ibytes); - printf("zero inputs %p %d\n\r", slave->inputs, slave->Ibytes); - } - printf("ERROR : slave %d lost\n", slaveix); - } - } - } - if (slave->islost) - { - if (slave->state <= EC_STATE_INIT) - { - if (ecx_recover_slave(&ctx, slaveix, EC_TIMEOUTMON)) - { - slave->islost = FALSE; - printf("MESSAGE : slave %d recovered\n", slaveix); - } - } - else - { - slave->islost = FALSE; - printf("MESSAGE : slave %d found\n", slaveix); - } - } - } - if (!ctx.grouplist[currentgroup].docheckstate) - printf("OK : all slaves resumed OPERATIONAL.\n"); - } - osal_usleep(10000); - } -} - -int main(int argc, char *argv[]) -{ - printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); - - if (argc > 1) - { - /* create thread to handle slave error handling in OP */ - osal_thread_create(&thread1, 128000, &ecatcheck, NULL); - /* start cyclic part */ - simpletest(argv[1]); - } - else - { - ec_adaptert *adapter = NULL; - ec_adaptert *head = NULL; - printf("Usage: simple_test ifname1\nifname = eth0 for example\n"); - - printf("\nAvailable adapters:\n"); - head = adapter = ec_find_adapters(); - while (adapter != NULL) - { - printf(" - %s (%s)\n", adapter->name, adapter->desc); - adapter = adapter->next; - } - ec_free_adapters(head); - } - - printf("End program\n"); - return (0); -}