From 0ca469847e7881bde32dbd69c2fdbe95e330cef8 Mon Sep 17 00:00:00 2001 From: Stefano Dalla Gasperina Date: Tue, 17 Mar 2020 04:17:44 -0500 Subject: [PATCH 1/5] Initial commit --- test/linux/my_test/CMakeLists.txt | 4 + test/linux/my_test/my_test.c | 362 ++++++++++++++++++++++++++++++ 2 files changed, 366 insertions(+) create mode 100644 test/linux/my_test/CMakeLists.txt create mode 100644 test/linux/my_test/my_test.c diff --git a/test/linux/my_test/CMakeLists.txt b/test/linux/my_test/CMakeLists.txt new file mode 100644 index 00000000..3f3786c1 --- /dev/null +++ b/test/linux/my_test/CMakeLists.txt @@ -0,0 +1,4 @@ +set(SOURCES my_test.c) +add_executable(my_test ${SOURCES}) +target_link_libraries(my_test soem) +install(TARGETS my_test DESTINATION bin) diff --git a/test/linux/my_test/my_test.c b/test/linux/my_test/my_test.c new file mode 100644 index 00000000..4b350491 --- /dev/null +++ b/test/linux/my_test/my_test.c @@ -0,0 +1,362 @@ +/** \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 +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define NSEC_PER_SEC 1000000000 +#define EC_TIMEOUTMON 500 + +struct sched_param schedp; +char IOmap[4096]; +pthread_t thread1, thread2; +struct timeval tv, t1, t2; +int dorun = 0; +int deltat, tmax = 0; +int64 toff, gl_delta; +int DCdiff; +int os; +uint8 ob; +uint16 ob2; +uint8 *digout = 0; +int expectedWKC; +boolean needlf; +volatile int wkc; +boolean inOP; +uint8 currentgroup = 0; + +// Slave Distributed Clock Configuration + +boolean dcsync_enable = FALSE; + +static int slave_dc_config(uint16 slave) +{ +// ec_dcsync0(slave, active, cycletime, calc and copy time) + ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); +printf("ec_dcsync0 called on slave %u\n",slave); +//ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); +return 0; +} + + +void redtest(char *ifname) +{ + int cnt, i, oloop, iloop; + int j; + + printf("Starting Redundant test\n"); + + /* initialise SOEM, bind socket to ifname */ +// if (ec_init_redundant(ifname, ifname2)) + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map +// if ( ec_config(FALSE, &IOmap) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + // PO2SOconfig is for registering a hook function that will be called when the slave does the transition + // between Pre-OP and Safe-OP. + if ((ec_slavecount >= 1)) { + for (cnt = 1; cnt <= ec_slavecount; cnt++) { +// if (ec_slave[cnt].eep_id == 0x1c2B3052) { // eep_id == ProductCode on ESI +// el7211_pos = cnt; + printf("Found %s at position %d\n", ec_slave[cnt].name, cnt); + ec_slave[cnt].PO2SOconfig = &slave_dc_config; + + } + } + + ec_configdc(); + + ec_config_map(&IOmap); + + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* read indevidual slave state and store in ec_slave[] */ + ec_readstate(); + for(cnt = 1; cnt <= ec_slavecount ; cnt++) + { + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); +// printf(" Out:%8.8x,%4d In:%8.8x,%4d\n", +// (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); + /* check for EL2004 or EL2008 */ +// if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052))) +// { +// digout = ec_slave[cnt].outputs; +// } + } + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + + printf("Request operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* activate cyclic process data */ + dorun = 1; + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + if (ec_slave[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 %12ld, dt %12ld, O:", + dorun, wkc , ec_DCtime, gl_delta); + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf("\r"); + fflush(stdout); + osal_usleep(20000); + } + dorun = 0; + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + printf("Request safe operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_SAFE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End redundant test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +/* add ns to timespec */ +void add_timespec(struct timespec *ts, int64 addtime) +{ + int64 sec, nsec; + + nsec = addtime % NSEC_PER_SEC; + sec = (addtime - nsec) / NSEC_PER_SEC; + ts->tv_sec += sec; + ts->tv_nsec += nsec; + if ( ts->tv_nsec > NSEC_PER_SEC ) + { + nsec = ts->tv_nsec % NSEC_PER_SEC; + ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; + ts->tv_nsec = nsec; + } +} + +/* 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) +{ + struct timespec ts, tleft; + int ht; + int64 cycletime; + + clock_gettime(CLOCK_MONOTONIC, &ts); + ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ + ts.tv_nsec = ht * 1000000; + cycletime = *(int*)ptr * 1000; /* cycletime in ns */ + toff = 0; + dorun = 0; + ec_send_processdata(); + while(1) + { + /* calculate next cycle start */ + add_timespec(&ts, cycletime + toff); + /* wait to cycle start */ + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); + if (dorun>0) + { + wkc = ec_receive_processdata(EC_TIMEOUTRET); + + dorun++; + /* if we have some digital output, cycle */ + if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); + + if (ec_slave[0].hasdc) + { + /* calulate toff to get linux time and DC synced */ + ec_sync(ec_DCtime, cycletime, &toff); + } + ec_send_processdata(); + } + } +} + +OSAL_THREAD_FUNC ecatcheck() //void *ptr ) +{ + int slave; + + while(1) + { + if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } +} + +#define stack64k (64 * 1024) + +int main(int argc, char *argv[]) +{ + int ctime; + + printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); + + if (argc > 2) + { + dorun = 0; + ctime = atoi(argv[2]); + + /* create RT thread */ + osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime); + + /* create thread to handle slave error handling in OP */ + osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL); + + /* start acyclic part */ + redtest(argv[1]); + } + else + { + printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); + } + + printf("End program\n"); + + return (0); +} From abc0b63ff6bdd145571c2e30d3c40ef34a996897 Mon Sep 17 00:00:00 2001 From: Stefano Dalla Gasperina Date: Tue, 17 Mar 2020 07:04:13 -0500 Subject: [PATCH 2/5] Commit dcsync --- CMakeLists.txt | 1 + test/linux/my_test/CMakeLists.txt | 4 - test/linux/my_test/my_test.c | 362 ------------------------------ 3 files changed, 1 insertion(+), 366 deletions(-) delete mode 100644 test/linux/my_test/CMakeLists.txt delete mode 100644 test/linux/my_test/my_test.c diff --git a/CMakeLists.txt b/CMakeLists.txt index faad444c..60987f3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,4 +100,5 @@ if(BUILD_TESTS) add_subdirectory(test/linux/slaveinfo) add_subdirectory(test/linux/eepromtool) add_subdirectory(test/linux/simple_test) + add_subdirectory(test/linux/dcsync_test) endif() diff --git a/test/linux/my_test/CMakeLists.txt b/test/linux/my_test/CMakeLists.txt deleted file mode 100644 index 3f3786c1..00000000 --- a/test/linux/my_test/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -set(SOURCES my_test.c) -add_executable(my_test ${SOURCES}) -target_link_libraries(my_test soem) -install(TARGETS my_test DESTINATION bin) diff --git a/test/linux/my_test/my_test.c b/test/linux/my_test/my_test.c deleted file mode 100644 index 4b350491..00000000 --- a/test/linux/my_test/my_test.c +++ /dev/null @@ -1,362 +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 -#include -#include -#include -#include -#include - -#include "ethercat.h" - -#define NSEC_PER_SEC 1000000000 -#define EC_TIMEOUTMON 500 - -struct sched_param schedp; -char IOmap[4096]; -pthread_t thread1, thread2; -struct timeval tv, t1, t2; -int dorun = 0; -int deltat, tmax = 0; -int64 toff, gl_delta; -int DCdiff; -int os; -uint8 ob; -uint16 ob2; -uint8 *digout = 0; -int expectedWKC; -boolean needlf; -volatile int wkc; -boolean inOP; -uint8 currentgroup = 0; - -// Slave Distributed Clock Configuration - -boolean dcsync_enable = FALSE; - -static int slave_dc_config(uint16 slave) -{ -// ec_dcsync0(slave, active, cycletime, calc and copy time) - ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); -printf("ec_dcsync0 called on slave %u\n",slave); -//ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); -return 0; -} - - -void redtest(char *ifname) -{ - int cnt, i, oloop, iloop; - int j; - - printf("Starting Redundant test\n"); - - /* initialise SOEM, bind socket to ifname */ -// if (ec_init_redundant(ifname, ifname2)) - if (ec_init(ifname)) - { - printf("ec_init on %s succeeded.\n",ifname); - /* find and auto-config slaves */ - if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map -// if ( ec_config(FALSE, &IOmap) > 0 ) - { - printf("%d slaves found and configured.\n",ec_slavecount); - - // PO2SOconfig is for registering a hook function that will be called when the slave does the transition - // between Pre-OP and Safe-OP. - if ((ec_slavecount >= 1)) { - for (cnt = 1; cnt <= ec_slavecount; cnt++) { -// if (ec_slave[cnt].eep_id == 0x1c2B3052) { // eep_id == ProductCode on ESI -// el7211_pos = cnt; - printf("Found %s at position %d\n", ec_slave[cnt].name, cnt); - ec_slave[cnt].PO2SOconfig = &slave_dc_config; - - } - } - - ec_configdc(); - - ec_config_map(&IOmap); - - /* wait for all slaves to reach SAFE_OP state */ - ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - - /* read indevidual slave state and store in ec_slave[] */ - ec_readstate(); - for(cnt = 1; cnt <= ec_slavecount ; cnt++) - { - printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", - cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, - ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); -// printf(" Out:%8.8x,%4d In:%8.8x,%4d\n", -// (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); - /* check for EL2004 or EL2008 */ -// if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052))) -// { -// digout = ec_slave[cnt].outputs; -// } - } - expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; - printf("Calculated workcounter %d\n", expectedWKC); - - printf("Request operational state for all slaves\n"); - ec_slave[0].state = EC_STATE_OPERATIONAL; - /* request OP state for all slaves */ - ec_writestate(0); - /* activate cyclic process data */ - dorun = 1; - /* wait for all slaves to reach OP state */ - ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); - oloop = ec_slave[0].Obytes; - if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; - if (oloop > 8) oloop = 8; - iloop = ec_slave[0].Ibytes; - if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; - if (iloop > 8) iloop = 8; - if (ec_slave[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 %12ld, dt %12ld, O:", - dorun, wkc , ec_DCtime, gl_delta); - for(j = 0 ; j < oloop; j++) - { - printf(" %2.2x", *(ec_slave[0].outputs + j)); - } - printf(" I:"); - for(j = 0 ; j < iloop; j++) - { - printf(" %2.2x", *(ec_slave[0].inputs + j)); - } - printf("\r"); - fflush(stdout); - osal_usleep(20000); - } - dorun = 0; - inOP = FALSE; - } - else - { - printf("Not all slaves reached operational state.\n"); - ec_readstate(); - for(i = 1; i<=ec_slavecount ; i++) - { - if(ec_slave[i].state != EC_STATE_OPERATIONAL) - { - printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", - i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); - } - } - } - printf("Request safe operational state for all slaves\n"); - ec_slave[0].state = EC_STATE_SAFE_OP; - /* request SAFE_OP state for all slaves */ - ec_writestate(0); - } - else - { - printf("No slaves found!\n"); - } - printf("End redundant test, close socket\n"); - /* stop SOEM, close socket */ - ec_close(); - } - else - { - printf("No socket connection on %s\nExcecute as root\n",ifname); - } -} - -/* add ns to timespec */ -void add_timespec(struct timespec *ts, int64 addtime) -{ - int64 sec, nsec; - - nsec = addtime % NSEC_PER_SEC; - sec = (addtime - nsec) / NSEC_PER_SEC; - ts->tv_sec += sec; - ts->tv_nsec += nsec; - if ( ts->tv_nsec > NSEC_PER_SEC ) - { - nsec = ts->tv_nsec % NSEC_PER_SEC; - ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; - ts->tv_nsec = nsec; - } -} - -/* 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) -{ - struct timespec ts, tleft; - int ht; - int64 cycletime; - - clock_gettime(CLOCK_MONOTONIC, &ts); - ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ - ts.tv_nsec = ht * 1000000; - cycletime = *(int*)ptr * 1000; /* cycletime in ns */ - toff = 0; - dorun = 0; - ec_send_processdata(); - while(1) - { - /* calculate next cycle start */ - add_timespec(&ts, cycletime + toff); - /* wait to cycle start */ - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); - if (dorun>0) - { - wkc = ec_receive_processdata(EC_TIMEOUTRET); - - dorun++; - /* if we have some digital output, cycle */ - if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); - - if (ec_slave[0].hasdc) - { - /* calulate toff to get linux time and DC synced */ - ec_sync(ec_DCtime, cycletime, &toff); - } - ec_send_processdata(); - } - } -} - -OSAL_THREAD_FUNC ecatcheck() //void *ptr ) -{ - int slave; - - while(1) - { - if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) - { - if (needlf) - { - needlf = FALSE; - printf("\n"); - } - /* one ore more slaves are not responding */ - ec_group[currentgroup].docheckstate = FALSE; - ec_readstate(); - for (slave = 1; slave <= ec_slavecount; slave++) - { - if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) - { - ec_group[currentgroup].docheckstate = TRUE; - if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) - { - printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); - ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); - ec_writestate(slave); - } - else if(ec_slave[slave].state == EC_STATE_SAFE_OP) - { - printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); - ec_slave[slave].state = EC_STATE_OPERATIONAL; - ec_writestate(slave); - } - else if(ec_slave[slave].state > EC_STATE_NONE) - { - if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) - { - ec_slave[slave].islost = FALSE; - printf("MESSAGE : slave %d reconfigured\n",slave); - } - } - else if(!ec_slave[slave].islost) - { - /* re-check state */ - ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); - if (ec_slave[slave].state == EC_STATE_NONE) - { - ec_slave[slave].islost = TRUE; - printf("ERROR : slave %d lost\n",slave); - } - } - } - if (ec_slave[slave].islost) - { - if(ec_slave[slave].state == EC_STATE_NONE) - { - if (ec_recover_slave(slave, EC_TIMEOUTMON)) - { - ec_slave[slave].islost = FALSE; - printf("MESSAGE : slave %d recovered\n",slave); - } - } - else - { - ec_slave[slave].islost = FALSE; - printf("MESSAGE : slave %d found\n",slave); - } - } - } - if(!ec_group[currentgroup].docheckstate) - printf("OK : all slaves resumed OPERATIONAL.\n"); - } - osal_usleep(10000); - } -} - -#define stack64k (64 * 1024) - -int main(int argc, char *argv[]) -{ - int ctime; - - printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); - - if (argc > 2) - { - dorun = 0; - ctime = atoi(argv[2]); - - /* create RT thread */ - osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime); - - /* create thread to handle slave error handling in OP */ - osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL); - - /* start acyclic part */ - redtest(argv[1]); - } - else - { - printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); - } - - printf("End program\n"); - - return (0); -} From 38a26d4f5a0754370166eb7b1cb6fbe2c4ddd0ff Mon Sep 17 00:00:00 2001 From: Stefano Dalla Gasperina Date: Tue, 17 Mar 2020 07:05:15 -0500 Subject: [PATCH 3/5] Commit dcsync --- test/linux/dcsync_test/CMakeLists.txt | 4 + test/linux/dcsync_test/dcsync_test.c | 362 ++++++++++++++++++ test/linux/dcsync_test/dcsync_test.c.autosave | 361 +++++++++++++++++ 3 files changed, 727 insertions(+) create mode 100644 test/linux/dcsync_test/CMakeLists.txt create mode 100644 test/linux/dcsync_test/dcsync_test.c create mode 100644 test/linux/dcsync_test/dcsync_test.c.autosave diff --git a/test/linux/dcsync_test/CMakeLists.txt b/test/linux/dcsync_test/CMakeLists.txt new file mode 100644 index 00000000..22b5eab0 --- /dev/null +++ b/test/linux/dcsync_test/CMakeLists.txt @@ -0,0 +1,4 @@ +set(SOURCES dcsync_test.c) +add_executable(dcsync_test ${SOURCES}) +target_link_libraries(dcsync_test soem) +install(TARGETS dcsync_test DESTINATION bin) diff --git a/test/linux/dcsync_test/dcsync_test.c b/test/linux/dcsync_test/dcsync_test.c new file mode 100644 index 00000000..3531b0f9 --- /dev/null +++ b/test/linux/dcsync_test/dcsync_test.c @@ -0,0 +1,362 @@ +/** \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 +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define NSEC_PER_SEC 1000000000 +#define EC_TIMEOUTMON 500 + +struct sched_param schedp; +char IOmap[4096]; +pthread_t thread1, thread2; +struct timeval tv, t1, t2; +int dorun = 0; +int deltat, tmax = 0; +int64 toff, gl_delta; +int DCdiff; +int os; +uint8 ob; +uint16 ob2; +uint8 *digout = 0; +int expectedWKC; +boolean needlf; +volatile int wkc; +boolean inOP; +uint8 currentgroup = 0; + +// Slave Distributed Clock Configuration + +boolean dcsync_enable = TRUE; + +static int slave_dc_config(uint16 slave) +{ +// ec_dcsync0(slave, active, cycletime, calc and copy time) + ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); +printf("ec_dcsync0 called on slave %u\n",slave); +//ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); +return 0; +} + + +void redtest(char *ifname) +{ + int cnt, i, oloop, iloop; + int j; + + printf("Starting Redundant test\n"); + + /* initialise SOEM, bind socket to ifname */ +// if (ec_init_redundant(ifname, ifname2)) + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map +// if ( ec_config(FALSE, &IOmap) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + // PO2SOconfig is for registering a hook function that will be called when the slave does the transition + // between Pre-OP and Safe-OP. + if ((ec_slavecount >= 1)) { + for (cnt = 1; cnt <= ec_slavecount; cnt++) { +// if (ec_slave[cnt].eep_id == 0x1c2B3052) { // eep_id == ProductCode on ESI +// el7211_pos = cnt; + printf("Found %s at position %d\n", ec_slave[cnt].name, cnt); + ec_slave[cnt].PO2SOconfig = &slave_dc_config; + + } + } + + ec_configdc(); + + ec_config_map(&IOmap); + + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* read indevidual slave state and store in ec_slave[] */ + ec_readstate(); + for(cnt = 1; cnt <= ec_slavecount ; cnt++) + { + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); +// printf(" Out:%8.8x,%4d In:%8.8x,%4d\n", +// (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); + /* check for EL2004 or EL2008 */ +// if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052))) +// { +// digout = ec_slave[cnt].outputs; +// } + } + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + + printf("Request operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* activate cyclic process data */ + dorun = 1; + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + if (ec_slave[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 %12ld, dt %12ld, O:", + dorun, wkc , ec_DCtime, gl_delta); + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf("\r"); + fflush(stdout); + osal_usleep(20000); + } + dorun = 0; + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + printf("Request safe operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_SAFE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End redundant test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +/* add ns to timespec */ +void add_timespec(struct timespec *ts, int64 addtime) +{ + int64 sec, nsec; + + nsec = addtime % NSEC_PER_SEC; + sec = (addtime - nsec) / NSEC_PER_SEC; + ts->tv_sec += sec; + ts->tv_nsec += nsec; + if ( ts->tv_nsec > NSEC_PER_SEC ) + { + nsec = ts->tv_nsec % NSEC_PER_SEC; + ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; + ts->tv_nsec = nsec; + } +} + +/* 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) +{ + struct timespec ts, tleft; + int ht; + int64 cycletime; + + clock_gettime(CLOCK_MONOTONIC, &ts); + ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ + ts.tv_nsec = ht * 1000000; + cycletime = *(int*)ptr * 1000; /* cycletime in ns */ + toff = 0; + dorun = 0; + ec_send_processdata(); + while(1) + { + /* calculate next cycle start */ + add_timespec(&ts, cycletime + toff); + /* wait to cycle start */ + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); + if (dorun>0) + { + wkc = ec_receive_processdata(EC_TIMEOUTRET); + + dorun++; + /* if we have some digital output, cycle */ + if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); + + if (ec_slave[0].hasdc) + { + /* calulate toff to get linux time and DC synced */ + ec_sync(ec_DCtime, cycletime, &toff); + } + ec_send_processdata(); + } + } +} + +OSAL_THREAD_FUNC ecatcheck() //void *ptr ) +{ + int slave; + + while(1) + { + if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } +} + +#define stack64k (64 * 1024) + +int main(int argc, char *argv[]) +{ + int ctime; + + printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); + + if (argc > 2) + { + dorun = 0; + ctime = atoi(argv[2]); + + /* create RT thread */ + osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime); + + /* create thread to handle slave error handling in OP */ + osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL); + + /* start acyclic part */ + redtest(argv[1]); + } + else + { + printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); + } + + printf("End program\n"); + + return (0); +} diff --git a/test/linux/dcsync_test/dcsync_test.c.autosave b/test/linux/dcsync_test/dcsync_test.c.autosave new file mode 100644 index 00000000..2a8d0bcd --- /dev/null +++ b/test/linux/dcsync_test/dcsync_test.c.autosave @@ -0,0 +1,361 @@ +/** \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 +#include +#include +#include +#include +#include + +#include "ethercat.h" + +#define NSEC_PER_SEC 1000000000 +#define EC_TIMEOUTMON 500 + +struct sched_param schedp; +char IOmap[4096]; +pthread_t thread1, thread2; +struct timeval tv, t1, t2; +int dorun = 0; +int deltat, tmax = 0; +int64 toff, gl_delta; +int DCdiff; +int os; +uint8 ob; +uint16 ob2; +uint8 *digout = 0; +int expectedWKC; +boolean needlf; +volatile int wkc; +boolean inOP; +uint8 currentgroup = 0; + +// Slave Distributed Clock Configuration +boolean dcsync_enable = TRUE; + +static int slave_dc_config(uint16 slave) +{ +// ec_dcsync0(slave, active, cycletime, calc and copy time) + ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); + printf("ec_dcsync0 called on slave %u\n",slave); + //ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); + return 0; +} + + +void redtest(char *ifname) +{ + int cnt, i, oloop, iloop; + int j; + + printf("Starting Redundant test\n"); + + /* initialise SOEM, bind socket to ifname */ +// if (ec_init_redundant(ifname, ifname2)) + if (ec_init(ifname)) + { + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map +// if ( ec_config(FALSE, &IOmap) > 0 ) + { + printf("%d slaves found and configured.\n",ec_slavecount); + + // PO2SOconfig is for registering a hook function that will be called when the slave does the transition + // between Pre-OP and Safe-OP. + if ((ec_slavecount >= 1)) { + for (cnt = 1; cnt <= ec_slavecount; cnt++) { +// if (ec_slave[cnt].eep_id == 0x1c2B3052) { // eep_id == ProductCode on ESI +// el7211_pos = cnt; + printf("Found %s at position %d\n", ec_slave[cnt].name, cnt); + ec_slave[cnt].PO2SOconfig = &slave_dc_config; + + } + } + + ec_configdc(); + + ec_config_map(&IOmap); + + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + + /* read indevidual slave state and store in ec_slave[] */ + ec_readstate(); + for(cnt = 1; cnt <= ec_slavecount ; cnt++) + { + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); +// printf(" Out:%8.8x,%4d In:%8.8x,%4d\n", +// (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); + /* check for EL2004 or EL2008 */ +// if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052))) +// { +// digout = ec_slave[cnt].outputs; +// } + } + expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + printf("Calculated workcounter %d\n", expectedWKC); + + printf("Request operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* activate cyclic process data */ + dorun = 1; + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); + oloop = ec_slave[0].Obytes; + if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; + if (oloop > 8) oloop = 8; + iloop = ec_slave[0].Ibytes; + if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; + if (iloop > 8) iloop = 8; + if (ec_slave[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 %12ld, dt %12ld, O:", + dorun, wkc , ec_DCtime, gl_delta); + for(j = 0 ; j < oloop; j++) + { + printf(" %2.2x", *(ec_slave[0].outputs + j)); + } + printf(" I:"); + for(j = 0 ; j < iloop; j++) + { + printf(" %2.2x", *(ec_slave[0].inputs + j)); + } + printf("\r"); + fflush(stdout); + osal_usleep(20000); + } + dorun = 0; + inOP = FALSE; + } + else + { + printf("Not all slaves reached operational state.\n"); + ec_readstate(); + for(i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_OPERATIONAL) + { + printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + printf("Request safe operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_SAFE_OP; + /* request SAFE_OP state for all slaves */ + ec_writestate(0); + } + else + { + printf("No slaves found!\n"); + } + printf("End redundant test, close socket\n"); + /* stop SOEM, close socket */ + ec_close(); + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + } +} + +/* add ns to timespec */ +void add_timespec(struct timespec *ts, int64 addtime) +{ + int64 sec, nsec; + + nsec = addtime % NSEC_PER_SEC; + sec = (addtime - nsec) / NSEC_PER_SEC; + ts->tv_sec += sec; + ts->tv_nsec += nsec; + if ( ts->tv_nsec > NSEC_PER_SEC ) + { + nsec = ts->tv_nsec % NSEC_PER_SEC; + ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; + ts->tv_nsec = nsec; + } +} + +/* 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) +{ + struct timespec ts, tleft; + int ht; + int64 cycletime; + + clock_gettime(CLOCK_MONOTONIC, &ts); + ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ + ts.tv_nsec = ht * 1000000; + cycletime = *(int*)ptr * 1000; /* cycletime in ns */ + toff = 0; + dorun = 0; + ec_send_processdata(); + while(1) + { + /* calculate next cycle start */ + add_timespec(&ts, cycletime + toff); + /* wait to cycle start */ + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); + if (dorun>0) + { + wkc = ec_receive_processdata(EC_TIMEOUTRET); + + dorun++; + /* if we have some digital output, cycle */ + if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); + + if (ec_slave[0].hasdc) + { + /* calulate toff to get linux time and DC synced */ + ec_sync(ec_DCtime, cycletime, &toff); + } + ec_send_processdata(); + } + } +} + +OSAL_THREAD_FUNC ecatcheck() //void *ptr ) +{ + int slave; + + while(1) + { + if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) + { + if (needlf) + { + needlf = FALSE; + printf("\n"); + } + /* one ore more slaves are not responding */ + ec_group[currentgroup].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[currentgroup].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, EC_TIMEOUTMON)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + if(!ec_group[currentgroup].docheckstate) + printf("OK : all slaves resumed OPERATIONAL.\n"); + } + osal_usleep(10000); + } +} + +#define stack64k (64 * 1024) + +int main(int argc, char *argv[]) +{ + int ctime; + + printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); + + if (argc > 2) + { + dorun = 0; + ctime = atoi(argv[2]); + + /* create RT thread */ + osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime); + + /* create thread to handle slave error handling in OP */ + osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL); + + /* start acyclic part */ + redtest(argv[1]); + } + else + { + printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); + } + + printf("End program\n"); + + return (0); +} From 6389202bd616ae30f3452fade510cd5cf3c778dc Mon Sep 17 00:00:00 2001 From: Stefano Dalla Gasperina Date: Tue, 17 Mar 2020 07:05:29 -0500 Subject: [PATCH 4/5] Commit dcsync --- test/linux/dcsync_test/dcsync_test.c | 7 +- test/linux/dcsync_test/dcsync_test.c.autosave | 361 ------------------ 2 files changed, 3 insertions(+), 365 deletions(-) delete mode 100644 test/linux/dcsync_test/dcsync_test.c.autosave diff --git a/test/linux/dcsync_test/dcsync_test.c b/test/linux/dcsync_test/dcsync_test.c index 3531b0f9..2a8d0bcd 100644 --- a/test/linux/dcsync_test/dcsync_test.c +++ b/test/linux/dcsync_test/dcsync_test.c @@ -45,16 +45,15 @@ boolean inOP; uint8 currentgroup = 0; // Slave Distributed Clock Configuration - boolean dcsync_enable = TRUE; static int slave_dc_config(uint16 slave) { // ec_dcsync0(slave, active, cycletime, calc and copy time) ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); -printf("ec_dcsync0 called on slave %u\n",slave); -//ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); -return 0; + printf("ec_dcsync0 called on slave %u\n",slave); + //ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); + return 0; } diff --git a/test/linux/dcsync_test/dcsync_test.c.autosave b/test/linux/dcsync_test/dcsync_test.c.autosave deleted file mode 100644 index 2a8d0bcd..00000000 --- a/test/linux/dcsync_test/dcsync_test.c.autosave +++ /dev/null @@ -1,361 +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 -#include -#include -#include -#include -#include - -#include "ethercat.h" - -#define NSEC_PER_SEC 1000000000 -#define EC_TIMEOUTMON 500 - -struct sched_param schedp; -char IOmap[4096]; -pthread_t thread1, thread2; -struct timeval tv, t1, t2; -int dorun = 0; -int deltat, tmax = 0; -int64 toff, gl_delta; -int DCdiff; -int os; -uint8 ob; -uint16 ob2; -uint8 *digout = 0; -int expectedWKC; -boolean needlf; -volatile int wkc; -boolean inOP; -uint8 currentgroup = 0; - -// Slave Distributed Clock Configuration -boolean dcsync_enable = TRUE; - -static int slave_dc_config(uint16 slave) -{ -// ec_dcsync0(slave, active, cycletime, calc and copy time) - ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); - printf("ec_dcsync0 called on slave %u\n",slave); - //ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); - return 0; -} - - -void redtest(char *ifname) -{ - int cnt, i, oloop, iloop; - int j; - - printf("Starting Redundant test\n"); - - /* initialise SOEM, bind socket to ifname */ -// if (ec_init_redundant(ifname, ifname2)) - if (ec_init(ifname)) - { - printf("ec_init on %s succeeded.\n",ifname); - /* find and auto-config slaves */ - if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map -// if ( ec_config(FALSE, &IOmap) > 0 ) - { - printf("%d slaves found and configured.\n",ec_slavecount); - - // PO2SOconfig is for registering a hook function that will be called when the slave does the transition - // between Pre-OP and Safe-OP. - if ((ec_slavecount >= 1)) { - for (cnt = 1; cnt <= ec_slavecount; cnt++) { -// if (ec_slave[cnt].eep_id == 0x1c2B3052) { // eep_id == ProductCode on ESI -// el7211_pos = cnt; - printf("Found %s at position %d\n", ec_slave[cnt].name, cnt); - ec_slave[cnt].PO2SOconfig = &slave_dc_config; - - } - } - - ec_configdc(); - - ec_config_map(&IOmap); - - /* wait for all slaves to reach SAFE_OP state */ - ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); - - /* read indevidual slave state and store in ec_slave[] */ - ec_readstate(); - for(cnt = 1; cnt <= ec_slavecount ; cnt++) - { - printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", - cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, - ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); -// printf(" Out:%8.8x,%4d In:%8.8x,%4d\n", -// (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); - /* check for EL2004 or EL2008 */ -// if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052))) -// { -// digout = ec_slave[cnt].outputs; -// } - } - expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; - printf("Calculated workcounter %d\n", expectedWKC); - - printf("Request operational state for all slaves\n"); - ec_slave[0].state = EC_STATE_OPERATIONAL; - /* request OP state for all slaves */ - ec_writestate(0); - /* activate cyclic process data */ - dorun = 1; - /* wait for all slaves to reach OP state */ - ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); - oloop = ec_slave[0].Obytes; - if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; - if (oloop > 8) oloop = 8; - iloop = ec_slave[0].Ibytes; - if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; - if (iloop > 8) iloop = 8; - if (ec_slave[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 %12ld, dt %12ld, O:", - dorun, wkc , ec_DCtime, gl_delta); - for(j = 0 ; j < oloop; j++) - { - printf(" %2.2x", *(ec_slave[0].outputs + j)); - } - printf(" I:"); - for(j = 0 ; j < iloop; j++) - { - printf(" %2.2x", *(ec_slave[0].inputs + j)); - } - printf("\r"); - fflush(stdout); - osal_usleep(20000); - } - dorun = 0; - inOP = FALSE; - } - else - { - printf("Not all slaves reached operational state.\n"); - ec_readstate(); - for(i = 1; i<=ec_slavecount ; i++) - { - if(ec_slave[i].state != EC_STATE_OPERATIONAL) - { - printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", - i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); - } - } - } - printf("Request safe operational state for all slaves\n"); - ec_slave[0].state = EC_STATE_SAFE_OP; - /* request SAFE_OP state for all slaves */ - ec_writestate(0); - } - else - { - printf("No slaves found!\n"); - } - printf("End redundant test, close socket\n"); - /* stop SOEM, close socket */ - ec_close(); - } - else - { - printf("No socket connection on %s\nExcecute as root\n",ifname); - } -} - -/* add ns to timespec */ -void add_timespec(struct timespec *ts, int64 addtime) -{ - int64 sec, nsec; - - nsec = addtime % NSEC_PER_SEC; - sec = (addtime - nsec) / NSEC_PER_SEC; - ts->tv_sec += sec; - ts->tv_nsec += nsec; - if ( ts->tv_nsec > NSEC_PER_SEC ) - { - nsec = ts->tv_nsec % NSEC_PER_SEC; - ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; - ts->tv_nsec = nsec; - } -} - -/* 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) -{ - struct timespec ts, tleft; - int ht; - int64 cycletime; - - clock_gettime(CLOCK_MONOTONIC, &ts); - ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ - ts.tv_nsec = ht * 1000000; - cycletime = *(int*)ptr * 1000; /* cycletime in ns */ - toff = 0; - dorun = 0; - ec_send_processdata(); - while(1) - { - /* calculate next cycle start */ - add_timespec(&ts, cycletime + toff); - /* wait to cycle start */ - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); - if (dorun>0) - { - wkc = ec_receive_processdata(EC_TIMEOUTRET); - - dorun++; - /* if we have some digital output, cycle */ - if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); - - if (ec_slave[0].hasdc) - { - /* calulate toff to get linux time and DC synced */ - ec_sync(ec_DCtime, cycletime, &toff); - } - ec_send_processdata(); - } - } -} - -OSAL_THREAD_FUNC ecatcheck() //void *ptr ) -{ - int slave; - - while(1) - { - if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) - { - if (needlf) - { - needlf = FALSE; - printf("\n"); - } - /* one ore more slaves are not responding */ - ec_group[currentgroup].docheckstate = FALSE; - ec_readstate(); - for (slave = 1; slave <= ec_slavecount; slave++) - { - if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) - { - ec_group[currentgroup].docheckstate = TRUE; - if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) - { - printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); - ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); - ec_writestate(slave); - } - else if(ec_slave[slave].state == EC_STATE_SAFE_OP) - { - printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); - ec_slave[slave].state = EC_STATE_OPERATIONAL; - ec_writestate(slave); - } - else if(ec_slave[slave].state > EC_STATE_NONE) - { - if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) - { - ec_slave[slave].islost = FALSE; - printf("MESSAGE : slave %d reconfigured\n",slave); - } - } - else if(!ec_slave[slave].islost) - { - /* re-check state */ - ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); - if (ec_slave[slave].state == EC_STATE_NONE) - { - ec_slave[slave].islost = TRUE; - printf("ERROR : slave %d lost\n",slave); - } - } - } - if (ec_slave[slave].islost) - { - if(ec_slave[slave].state == EC_STATE_NONE) - { - if (ec_recover_slave(slave, EC_TIMEOUTMON)) - { - ec_slave[slave].islost = FALSE; - printf("MESSAGE : slave %d recovered\n",slave); - } - } - else - { - ec_slave[slave].islost = FALSE; - printf("MESSAGE : slave %d found\n",slave); - } - } - } - if(!ec_group[currentgroup].docheckstate) - printf("OK : all slaves resumed OPERATIONAL.\n"); - } - osal_usleep(10000); - } -} - -#define stack64k (64 * 1024) - -int main(int argc, char *argv[]) -{ - int ctime; - - printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); - - if (argc > 2) - { - dorun = 0; - ctime = atoi(argv[2]); - - /* create RT thread */ - osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime); - - /* create thread to handle slave error handling in OP */ - osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL); - - /* start acyclic part */ - redtest(argv[1]); - } - else - { - printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); - } - - printf("End program\n"); - - return (0); -} From c5c5f4d6c6bce85d6a3c36b41f57d24dc53a39ea Mon Sep 17 00:00:00 2001 From: Stefano Dalla Gasperina Date: Tue, 17 Mar 2020 10:19:35 -0500 Subject: [PATCH 5/5] Comments and minor fix --- test/linux/dcsync_test/dcsync_test.c | 49 ++++++++++++++-------------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/test/linux/dcsync_test/dcsync_test.c b/test/linux/dcsync_test/dcsync_test.c index 2a8d0bcd..1e7be11c 100644 --- a/test/linux/dcsync_test/dcsync_test.c +++ b/test/linux/dcsync_test/dcsync_test.c @@ -1,13 +1,15 @@ /** \file - * \brief Example code for Simple Open EtherCAT master - * - * Usage : red_test [ifname1] [ifname2] [cycletime] + * \brief Example code for Simple Open EtherCAT master with Distributed Clock Sync + * + * Usage : dcsync_test [ifname1] [cycletime] * ifname is NIC interface, f.e. eth0 - * cycletime in us, f.e. 500 + * cycletime in us, f.e. 1000 * - * This is a redundancy test. + * This is a script based on redundancy test to run EtherCAT master with DC Sync. * * (c)Arthur Ketels 2008 + * + * modified by Stefano Dalla Gasperina 2020 */ #include @@ -44,7 +46,7 @@ volatile int wkc; boolean inOP; uint8 currentgroup = 0; -// Slave Distributed Clock Configuration +/* Slave Distributed Clock Configuration */ boolean dcsync_enable = TRUE; static int slave_dc_config(uint16 slave) @@ -52,7 +54,6 @@ static int slave_dc_config(uint16 slave) // ec_dcsync0(slave, active, cycletime, calc and copy time) ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); printf("ec_dcsync0 called on slave %u\n",slave); - //ec_dcsync01(slave, TRUE, 2000000U, 0U, 0U); return 0; } @@ -62,16 +63,14 @@ void redtest(char *ifname) int cnt, i, oloop, iloop; int j; - printf("Starting Redundant test\n"); + printf("Starting DC-sync test\n"); /* initialise SOEM, bind socket to ifname */ -// if (ec_init_redundant(ifname, ifname2)) if (ec_init(ifname)) { printf("ec_init on %s succeeded.\n",ifname); /* find and auto-config slaves */ if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map -// if ( ec_config(FALSE, &IOmap) > 0 ) { printf("%d slaves found and configured.\n",ec_slavecount); @@ -79,14 +78,12 @@ void redtest(char *ifname) // between Pre-OP and Safe-OP. if ((ec_slavecount >= 1)) { for (cnt = 1; cnt <= ec_slavecount; cnt++) { -// if (ec_slave[cnt].eep_id == 0x1c2B3052) { // eep_id == ProductCode on ESI -// el7211_pos = cnt; printf("Found %s at position %d\n", ec_slave[cnt].name, cnt); ec_slave[cnt].PO2SOconfig = &slave_dc_config; - } } + /* Locate DC slaves, measure propagation delays. */ ec_configdc(); ec_config_map(&IOmap); @@ -98,16 +95,13 @@ void redtest(char *ifname) ec_readstate(); for(cnt = 1; cnt <= ec_slavecount ; cnt++) { + /* BEGIN USER CODE */ + printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); -// printf(" Out:%8.8x,%4d In:%8.8x,%4d\n", -// (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes); - /* check for EL2004 or EL2008 */ -// if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052))) -// { -// digout = ec_slave[cnt].outputs; -// } + + /* END USER CODE */ } expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; printf("Calculated workcounter %d\n", expectedWKC); @@ -133,6 +127,8 @@ void redtest(char *ifname) /* acyclic loop 5000 x 20ms = 10s */ for(i = 1; i <= 5000; i++) { + /* BEGIN USER CODE */ + printf("Processdata cycle %5d , Wck %3d, DCtime %12ld, dt %12ld, O:", dorun, wkc , ec_DCtime, gl_delta); for(j = 0 ; j < oloop; j++) @@ -147,6 +143,9 @@ void redtest(char *ifname) printf("\r"); fflush(stdout); osal_usleep(20000); + + /* END USER CODE */ + } dorun = 0; inOP = FALSE; @@ -173,7 +172,7 @@ void redtest(char *ifname) { printf("No slaves found!\n"); } - printf("End redundant test, close socket\n"); + printf("End DC-sync test, close socket\n"); /* stop SOEM, close socket */ ec_close(); } @@ -236,6 +235,7 @@ OSAL_THREAD_FUNC_RT ecatthread(void *ptr) clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); if (dorun>0) { + /* BEGIN USER CODE */ wkc = ec_receive_processdata(EC_TIMEOUTRET); dorun++; @@ -248,11 +248,12 @@ OSAL_THREAD_FUNC_RT ecatthread(void *ptr) ec_sync(ec_DCtime, cycletime, &toff); } ec_send_processdata(); + /* END USER CODE */ } } } -OSAL_THREAD_FUNC ecatcheck() //void *ptr ) +OSAL_THREAD_FUNC ecatcheck() { int slave; @@ -334,7 +335,7 @@ int main(int argc, char *argv[]) { int ctime; - printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n"); + printf("SOEM (Simple Open EtherCAT Master)\nDC-sync test\n"); if (argc > 2) { @@ -352,7 +353,7 @@ int main(int argc, char *argv[]) } else { - printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n"); + printf("Usage: dcsync_test ifname cycletime\nifname = eth0 for example\ncycletime in us\n"); } printf("End program\n");