Added example with DC-sync

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()

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)

test/linux/dcsync_test/dcsync_test.c

@@ -0,0 +1,362 @@
+/** \file
+ * \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. 1000
+ *
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <sys/time.h>
+#include <unistd.h>
+#include <sched.h>
+#include <string.h>
+#include <sys/time.h>
+#include <time.h>
+#include <pthread.h>
+#include <math.h>
+
+#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);
+    return 0;
+}
+
+
+void redtest(char *ifname)
+{
+   int cnt, i, oloop, iloop;
+   int j;
+
+   printf("Starting DC-sync test\n");
+
+   /* initialise SOEM, bind socket to ifname */
+   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
+      {
+         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++) {
+                     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);
+
+         /* 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++)
+         {
+            /* 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);
+
+            /* END USER CODE */
+         }
+         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++)
+            {
+                /* 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++)
+               {
+                  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);
+
+               /* END USER CODE */
+
+            }
+            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 DC-sync 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)
+      {
+         /* BEGIN USER CODE */
+         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();
+         /* END USER CODE */
+      }
+   }
+}
+
+OSAL_THREAD_FUNC ecatcheck()
+{
+    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)\nDC-sync 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: dcsync_test ifname cycletime\nifname = eth0 for example\ncycletime in us\n");
+   }
+
+   printf("End program\n");
+
+   return (0);
+}