Diff: SOEM/ethercatdc.c

Revision:

0:543d6784d4cc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SOEM/ethercatdc.c	Tue Jun 11 10:29:09 2019 +0000
@@ -0,0 +1,448 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+/** \file
+ * \brief
+ * Distributed Clock EtherCAT functions.
+ *
+ */
+#include "oshw.h"
+#include "osal.h"
+#include "ethercattype.h"
+#include "ethercatbase.h"
+#include "ethercatmain.h"
+#include "ethercatdc.h"
+
+#define PORTM0 0x01
+#define PORTM1 0x02
+#define PORTM2 0x04
+#define PORTM3 0x08
+
+/** 1st sync pulse delay in ns here 100ms */
+#define SyncDelay       ((int32)100000000)
+
+/**
+ * Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset.
+ *
+ * @param[in]  context        = context struct
+ * @param [in] slave            Slave number.
+ * @param [in] act              TRUE = active, FALSE = deactivated
+ * @param [in] CyclTime         Cycltime in ns.
+ * @param [in] CyclShift        CyclShift in ns.
+ */
+void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
+{
+   uint8 h, RA;
+   uint16 slaveh;
+   int64 t, t1;
+   int32 tc;
+
+   slaveh = context->slavelist[slave].configadr;
+   RA = 0;
+
+   /* stop cyclic operation, ready for next trigger */
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
+   if (act)
+   {
+       RA = 1 + 2;    /* act cyclic operation and sync0, sync1 deactivated */
+   }
+   h = 0;
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
+   t1 = 0;
+   (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
+   t1 = etohll(t1);
+
+   /* Calculate first trigger time, always a whole multiple of CyclTime rounded up
+   plus the shifttime (can be negative)
+   This insures best synchronization between slaves, slaves with the same CyclTime
+   will sync at the same moment (you can use CyclShift to shift the sync) */
+   if (CyclTime > 0)
+   {
+       t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift;
+   }
+   else
+   {
+      t = t1 + SyncDelay + CyclShift;
+      /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
+   }
+   t = htoell(t);
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
+   tc = htoel(CyclTime);
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
+
+    // update ec_slave state
+    context->slavelist[slave].DCactive = (uint8)act;
+    context->slavelist[slave].DCshift = CyclShift;
+    context->slavelist[slave].DCcycle = CyclTime;
+}
+
+/**
+ * Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset.
+ *
+ * @param[in]  context        = context struct
+ * @param [in] slave            Slave number.
+ * @param [in] act              TRUE = active, FALSE = deactivated
+ * @param [in] CyclTime0        Cycltime SYNC0 in ns.
+ * @param [in] CyclTime1        Cycltime SYNC1 in ns. This time is a delta time in relation to
+                                the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time
+                                as SYNC0.
+ * @param [in] CyclShift        CyclShift in ns.
+ */
+void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
+{
+   uint8 h, RA;
+   uint16 slaveh;
+   int64 t, t1;
+   int32 tc;
+   uint32 TrueCyclTime;
+
+   /* Sync1 can be used as a multiple of Sync0, use true cycle time */
+   TrueCyclTime = ((CyclTime1 / CyclTime0) + 1) * CyclTime0;
+
+   slaveh = context->slavelist[slave].configadr;
+   RA = 0;
+
+   /* stop cyclic operation, ready for next trigger */
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
+   if (act)
+   {
+      RA = 1 + 2 + 4;    /* act cyclic operation and sync0 + sync1 */
+   }
+   h = 0;
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
+   t1 = 0;
+   (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
+   t1 = etohll(t1);
+
+   /* Calculate first trigger time, always a whole multiple of TrueCyclTime rounded up
+   plus the shifttime (can be negative)
+   This insures best synchronization between slaves, slaves with the same CyclTime
+   will sync at the same moment (you can use CyclShift to shift the sync) */
+   if (CyclTime0 > 0)
+   {
+      t = ((t1 + SyncDelay) / TrueCyclTime) * TrueCyclTime + TrueCyclTime + CyclShift;
+   }
+   else
+   {
+      t = t1 + SyncDelay + CyclShift;
+      /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
+   }
+   t = htoell(t);
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
+   tc = htoel(CyclTime0);
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
+   tc = htoel(CyclTime1);
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */
+   (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
+
+    // update ec_slave state
+    context->slavelist[slave].DCactive = (uint8)act;
+    context->slavelist[slave].DCshift = CyclShift;
+    context->slavelist[slave].DCcycle = CyclTime0;
+}
+
+/* latched port time of slave */
+static int32 ecx_porttime(ecx_contextt *context, uint16 slave, uint8 port)
+{
+   int32 ts;
+   switch (port)
+   {
+      case 0:
+         ts = context->slavelist[slave].DCrtA;
+         break;
+      case 1:
+         ts = context->slavelist[slave].DCrtB;
+         break;
+      case 2:
+         ts = context->slavelist[slave].DCrtC;
+         break;
+      case 3:
+         ts = context->slavelist[slave].DCrtD;
+         break;
+      default:
+         ts = 0;
+         break;
+   }
+   return ts;
+}
+
+/* calculate previous active port of a slave */
+static uint8 ecx_prevport(ecx_contextt *context, uint16 slave, uint8 port)
+{
+   uint8 pport = port;
+   uint8 aport = context->slavelist[slave].activeports;
+   switch(port)
+   {
+      case 0:
+         if(aport & PORTM2)
+            pport = 2;
+         else if (aport & PORTM1)
+            pport = 1;
+         else if (aport & PORTM3)
+            pport = 3;
+         break;
+      case 1:
+         if(aport & PORTM3)
+            pport = 3;
+         else if (aport & PORTM0)
+            pport = 0;
+         else if (aport & PORTM2)
+            pport = 2;
+         break;
+      case 2:
+         if(aport & PORTM1)
+            pport = 1;
+         else if (aport & PORTM3)
+            pport = 3;
+         else if (aport & PORTM0)
+            pport = 0;
+         break;
+      case 3:
+         if(aport & PORTM0)
+            pport = 0;
+         else if (aport & PORTM2)
+            pport = 2;
+         else if (aport & PORTM1)
+            pport = 1;
+         break;
+   }
+   return pport;
+}
+
+/* search unconsumed ports in parent, consume and return first open port */
+static uint8 ecx_parentport(ecx_contextt *context, uint16 parent)
+{
+   uint8 parentport = 0;
+   uint8 b;
+   /* search order is important, here 3 - 1 - 2 - 0 */
+   b = context->slavelist[parent].consumedports;
+   if (b & PORTM3)
+   {
+      parentport = 3;
+      b &= (uint8)~PORTM3;
+   }
+   else if (b & PORTM1)
+   {
+      parentport = 1;
+      b &= (uint8)~PORTM1;
+   }
+   else if (b & PORTM2)
+   {
+      parentport = 2;
+      b &= (uint8)~PORTM2;
+   }
+   else if (b & PORTM0)
+   {
+      parentport = 0;
+      b &= (uint8)~PORTM0;
+   }
+   context->slavelist[parent].consumedports = b;
+   return parentport;
+}
+
+/**
+ * Locate DC slaves, measure propagation delays.
+ *
+ * @param[in]  context        = context struct
+ * @return boolean if slaves are found with DC
+ */
+boolean ecx_configdc(ecx_contextt *context)
+{
+   uint16 i, slaveh, parent, child;
+   uint16 parenthold = 0;
+   uint16 prevDCslave = 0;
+   int32 ht, dt1, dt2, dt3;
+   int64 hrt;
+   uint8 entryport;
+   int8 nlist;
+   int8 plist[4];
+   int32 tlist[4];
+   ec_timet mastertime;
+   uint64 mastertime64;
+
+   context->slavelist[0].hasdc = FALSE;
+   context->grouplist[0].hasdc = FALSE;
+   ht = 0;
+
+   ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);  /* latch DCrecvTimeA of all slaves */
+   mastertime = osal_current_time();
+   mastertime.sec -= 946684800UL;  /* EtherCAT uses 2000-01-01 as epoch start instead of 1970-01-01 */
+   mastertime64 = (((uint64)mastertime.sec * 1000000) + (uint64)mastertime.usec) * 1000;
+   for (i = 1; i <= *(context->slavecount); i++)
+   {
+      context->slavelist[i].consumedports = context->slavelist[i].activeports;
+      if (context->slavelist[i].hasdc)
+      {
+         if (!context->slavelist[0].hasdc)
+         {
+            context->slavelist[0].hasdc = TRUE;
+            context->slavelist[0].DCnext = i;
+            context->slavelist[i].DCprevious = 0;
+            context->grouplist[context->slavelist[i].group].hasdc = TRUE;
+            context->grouplist[context->slavelist[i].group].DCnext = i;
+         }
+         else
+         {
+            context->slavelist[prevDCslave].DCnext = i;
+            context->slavelist[i].DCprevious = prevDCslave;
+         }
+         /* this branch has DC slave so remove parenthold */
+         parenthold = 0;
+         prevDCslave = i;
+         slaveh = context->slavelist[i].configadr;
+         (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);
+         context->slavelist[i].DCrtA = etohl(ht);
+         /* 64bit latched DCrecvTimeA of each specific slave */
+         (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET);
+         /* use it as offset in order to set local time around 0 + mastertime */
+         hrt = htoell(-etohll(hrt) + mastertime64);
+         /* save it in the offset register */
+         (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET);
+         (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET);
+         context->slavelist[i].DCrtB = etohl(ht);
+         (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET);
+         context->slavelist[i].DCrtC = etohl(ht);
+         (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET);
+         context->slavelist[i].DCrtD = etohl(ht);
+
+         /* make list of active ports and their time stamps */
+         nlist = 0;
+         if (context->slavelist[i].activeports & PORTM0)
+         {
+            plist[nlist] = 0;
+            tlist[nlist] = context->slavelist[i].DCrtA;
+            nlist++;
+         }
+         if (context->slavelist[i].activeports & PORTM3)
+         {
+            plist[nlist] = 3;
+            tlist[nlist] = context->slavelist[i].DCrtD;
+            nlist++;
+         }
+         if (context->slavelist[i].activeports & PORTM1)
+         {
+            plist[nlist] = 1;
+            tlist[nlist] = context->slavelist[i].DCrtB;
+            nlist++;
+         }
+         if (context->slavelist[i].activeports & PORTM2)
+         {
+            plist[nlist] = 2;
+            tlist[nlist] = context->slavelist[i].DCrtC;
+            nlist++;
+         }
+         /* entryport is port with the lowest timestamp */
+         entryport = 0;
+         if((nlist > 1) && (tlist[1] < tlist[entryport]))
+         {
+            entryport = 1;
+         }
+         if((nlist > 2) && (tlist[2] < tlist[entryport]))
+         {
+            entryport = 2;
+         }
+         if((nlist > 3) && (tlist[3] < tlist[entryport]))
+         {
+            entryport = 3;
+         }
+         entryport = plist[entryport];
+         context->slavelist[i].entryport = entryport;
+         /* consume entryport from activeports */
+         context->slavelist[i].consumedports &= (uint8)~(1 << entryport);
+
+         /* finding DC parent of current */
+         parent = i;
+         do
+         {
+            child = parent;
+            parent = context->slavelist[parent].parent;
+         }
+         while (!((parent == 0) || (context->slavelist[parent].hasdc)));
+         /* only calculate propagation delay if slave is not the first */
+         if (parent > 0)
+         {
+            /* find port on parent this slave is connected to */
+            context->slavelist[i].parentport = ecx_parentport(context, parent);
+            if (context->slavelist[parent].topology == 1)
+            {
+               context->slavelist[i].parentport = context->slavelist[parent].entryport;
+            }
+
+            dt1 = 0;
+            dt2 = 0;
+            /* delta time of (parentport - 1) - parentport */
+            /* note: order of ports is 0 - 3 - 1 -2 */
+            /* non active ports are skipped */
+            dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) -
+                  ecx_porttime(context, parent,
+                    ecx_prevport(context, parent, context->slavelist[i].parentport));
+            /* current slave has children */
+            /* those children's delays need to be subtracted */
+            if (context->slavelist[i].topology > 1)
+            {
+               dt1 = ecx_porttime(context, i,
+                        ecx_prevport(context, i, context->slavelist[i].entryport)) -
+                     ecx_porttime(context, i, context->slavelist[i].entryport);
+            }
+            /* we are only interested in positive difference */
+            if (dt1 > dt3) dt1 = -dt1;
+            /* current slave is not the first child of parent */
+            /* previous child's delays need to be added */
+            if ((child - parent) > 1)
+            {
+               dt2 = ecx_porttime(context, parent,
+                        ecx_prevport(context, parent, context->slavelist[i].parentport)) -
+                     ecx_porttime(context, parent, context->slavelist[parent].entryport);
+            }
+            if (dt2 < 0) dt2 = -dt2;
+
+            /* calculate current slave delay from delta times */
+            /* assumption : forward delay equals return delay */
+            context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 +
+               context->slavelist[parent].pdelay;
+            ht = htoel(context->slavelist[i].pdelay);
+            /* write propagation delay*/
+            (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET);
+         }
+      }
+      else
+      {
+         context->slavelist[i].DCrtA = 0;
+         context->slavelist[i].DCrtB = 0;
+         context->slavelist[i].DCrtC = 0;
+         context->slavelist[i].DCrtD = 0;
+         parent = context->slavelist[i].parent;
+         /* if non DC slave found on first position on branch hold root parent */
+         if ( (parent > 0) && (context->slavelist[parent].topology > 2))
+            parenthold = parent;
+         /* if branch has no DC slaves consume port on root parent */
+         if ( parenthold && (context->slavelist[i].topology == 1))
+         {
+            ecx_parentport(context, parenthold);
+            parenthold = 0;
+         }
+      }
+   }
+
+   return context->slavelist[0].hasdc;
+}
+
+#ifdef EC_VER1
+void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
+{
+   ecx_dcsync0(&ecx_context, slave, act, CyclTime, CyclShift);
+}
+
+void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
+{
+   ecx_dcsync01(&ecx_context, slave, act, CyclTime0, CyclTime1, CyclShift);
+}
+
+boolean ec_configdc(void)
+{
+   return ecx_configdc(&ecx_context);
+}
+#endif