ethercatdc.c

Go to the documentation of this file.
00001 /*
00002  * Simple Open EtherCAT Master Library 
00003  *
00004  * File    : ethercatdc.c
00005  * Version : 1.2.5
00006  * Date    : 09-04-2011
00007  * Copyright (C) 2005-2011 Speciaal Machinefabriek Ketels v.o.f.
00008  * Copyright (C) 2005-2011 Arthur Ketels
00009  * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 
00010  *
00011  * SOEM is free software; you can redistribute it and/or modify it under
00012  * the terms of the GNU General Public License version 2 as published by the Free
00013  * Software Foundation.
00014  *
00015  * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY
00016  * WARRANTY; without even the implied warranty of MERCHANTABILITY or
00017  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
00018  * for more details.
00019  *
00020  * As a special exception, if other files instantiate templates or use macros
00021  * or inline functions from this file, or you compile this file and link it
00022  * with other works to produce a work based on this file, this file does not
00023  * by itself cause the resulting work to be covered by the GNU General Public
00024  * License. However the source code for this file must still be made available
00025  * in accordance with section (3) of the GNU General Public License.
00026  *
00027  * This exception does not invalidate any other reasons why a work based on
00028  * this file might be covered by the GNU General Public License.
00029  *
00030  * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual
00031  * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for
00032  * the sole purpose of creating, using and/or selling or otherwise distributing
00033  * an EtherCAT network master provided that an EtherCAT Master License is obtained
00034  * from Beckhoff Automation GmbH.
00035  *
00036  * In case you did not receive a copy of the EtherCAT Master License along with
00037  * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany
00038  * (www.beckhoff.com).
00039  */
00040 
00041 /** \file
00042  * \brief
00043  * Distributed Clock EtherCAT functions. 
00044  *
00045  */
00046 #include "ethercattype.h"
00047 #include "nicdrv.h"
00048 #include "ethercatbase.h"
00049 #include "ethercatmain.h"
00050 #include "ethercatdc.h"
00051 
00052 #define PORTM0 0x01
00053 #define PORTM1 0x02
00054 #define PORTM2 0x04
00055 #define PORTM3 0x08
00056 
00057 /** 1st sync pulse delay in ns here 100ms */
00058 #define SyncDelay       ((int32)100000000)
00059 
00060 /**
00061  * Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset.
00062  *
00063  * @param [in] slave            Slave number.
00064  * @param [in] act              TRUE = active, FALSE = deactivated
00065  * @param [in] CyclTime         Cycltime in ns.
00066  * @param [in] CyclShift        CyclShift in ns.
00067  */
00068 void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift)
00069 {
00070     uint8 h, RA;
00071     uint16 wc, slaveh;
00072     int64 t, t1;
00073     int32 tc;
00074 
00075     slaveh = ec_slave[slave].configadr;
00076     RA = 0;
00077 
00078     /* stop cyclic operation, ready for next trigger */
00079     wc = ec_FPWR(slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); 
00080     if (act)
00081     {
00082         RA = 1 + 2;    /* act cyclic operation and sync0, sync1 deactivated */
00083     }
00084     h = 0;
00085     wc = ec_FPWR(slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
00086     wc = ec_FPRD(slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
00087   t1 = etohll(t1);
00088 
00089     /* Calculate first trigger time, always a whole multiple of CyclTime rounded up
00090     plus the shifttime (can be negative)
00091     This insures best sychronisation between slaves, slaves with the same CyclTime
00092     will sync at the same moment (you can use CyclShift to shift the sync) */
00093     if (CyclTime > 0)
00094     {
00095         t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift;
00096     }
00097     else
00098     {
00099         t = t1 + SyncDelay + CyclShift;
00100         /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
00101     }
00102   t = htoell(t);
00103     wc = ec_FPWR(slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
00104     tc = htoel(CyclTime);
00105     wc = ec_FPWR(slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
00106     wc = ec_FPWR(slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
00107 }
00108 
00109 /**
00110  * Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset.
00111  *
00112  * @param [in] slave            Slave number.
00113  * @param [in] act              TRUE = active, FALSE = deactivated
00114  * @param [in] CyclTime0        Cycltime SYNC0 in ns.
00115  * @param [in] CyclTime1        Cycltime SYNC1 in ns. This time is a delta time in relation to
00116                 the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time
00117                 as SYNC0.
00118  * @param [in] CyclShift        CyclShift in ns.
00119  */
00120 void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift)
00121 {
00122     uint8 h, RA;
00123     uint16 wc, slaveh;
00124     int64 t, t1;
00125     int32 tc;
00126 
00127     slaveh = ec_slave[slave].configadr;
00128     RA = 0;
00129 
00130     /* stop cyclic operation, ready for next trigger */
00131     wc = ec_FPWR(slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); 
00132     if (act)
00133     {
00134         RA = 1 + 2 + 4;    /* act cyclic operation and sync0 + sync1 */
00135     }
00136     h = 0;
00137     wc = ec_FPWR(slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
00138     wc = ec_FPRD(slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
00139   t1 = etohll(t1);
00140 
00141     /* Calculate first trigger time, always a whole multiple of CyclTime rounded up
00142     plus the shifttime (can be negative)
00143     This insures best sychronisation between slaves, slaves with the same CyclTime
00144     will sync at the same moment (you can use CyclShift to shift the sync) */
00145     if (CyclTime0 > 0)
00146     {
00147         t = ((t1 + SyncDelay) / CyclTime0) * CyclTime0 + CyclTime0 + CyclShift;
00148     }
00149     else
00150     {
00151         t = t1 + SyncDelay + CyclShift;
00152         /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
00153     }
00154   t = htoell(t);
00155     wc = ec_FPWR(slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
00156     tc = htoel(CyclTime0);
00157     wc = ec_FPWR(slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
00158     tc = htoel(CyclTime1);
00159     wc = ec_FPWR(slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */
00160     wc = ec_FPWR(slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
00161 }
00162 
00163 /* latched port time of slave */
00164 int32 ec_porttime(uint16 slave, uint8 port)
00165 {
00166   int32 ts;
00167   switch (port)
00168   {
00169     case 0:
00170       ts = ec_slave[slave].DCrtA;
00171       break;
00172     case 1:
00173       ts = ec_slave[slave].DCrtB;
00174       break;
00175     case 2:
00176       ts = ec_slave[slave].DCrtC;
00177       break;
00178     case 3:
00179       ts = ec_slave[slave].DCrtD;
00180       break;
00181     default:
00182       ts = 0;
00183   }
00184   return ts;
00185 }
00186 
00187 /* calculate previous active port of a slave */
00188 uint8 ec_prevport(uint16 slave, uint8 port)
00189 {
00190   uint8 pport = port;
00191   uint8 aport = ec_slave[slave].activeports;
00192   switch(port)
00193   {
00194     case 0:
00195       if(aport & PORTM2)
00196         pport = 2;
00197       else if (aport & PORTM1)
00198         pport = 1;
00199       else if (aport & PORTM2)
00200         pport = 3;
00201       break;
00202     case 1:
00203       if(aport & PORTM3)
00204         pport = 3;
00205       else if (aport & PORTM0)
00206         pport = 0;
00207       else if (aport & PORTM2)
00208         pport = 2;
00209       break;
00210     case 2:
00211       if(aport & PORTM1)
00212         pport = 1;
00213       else if (aport & PORTM3)
00214         pport = 3;
00215       else if (aport & PORTM0)
00216         pport = 0;
00217       break;
00218     case 3:
00219       if(aport & PORTM0)
00220         pport = 0;
00221       else if (aport & PORTM2)
00222         pport = 2;
00223       else if (aport & PORTM1)
00224         pport = 1;
00225       break;
00226   }   
00227   return pport;
00228 }
00229 
00230 /* search unconsumed ports in parent, consume and return first open port */
00231 uint8 ec_parentport(uint16 parent)
00232 {
00233   uint8 parentport = 0;
00234   uint8 b;
00235   /* search order is important, here 3 - 1 - 2 - 0 */
00236   b = ec_slave[parent].consumedports;
00237   if (b & PORTM3)
00238   {
00239     parentport = 3;
00240     b &= (uint8)~PORTM3;
00241   }
00242   else if (b & PORTM1)
00243   {
00244     parentport = 1;
00245     b &= (uint8)~PORTM1;
00246   }
00247   else if (b & PORTM2)
00248   {
00249     parentport = 2;
00250     b &= (uint8)~PORTM2;
00251   }
00252   else if (b & PORTM0)
00253   {
00254     parentport = 0;
00255     b &= (uint8)~PORTM0;
00256   }
00257   ec_slave[parent].consumedports = b;
00258   return parentport;
00259 }
00260 
00261 /**
00262  * Locate DC slaves, measure propagation delays.
00263  *
00264  * return boolean if slaves are found with DC
00265  */
00266 boolean ec_configdc(void)
00267 {
00268     uint16 i, wc, slaveh, parent, child;
00269   uint16 parenthold = 0;
00270   uint16 prevDCslave = 0;
00271     int32 ht, dt1, dt2, dt3;
00272     int64 hrt;
00273   uint8 entryport;
00274   int8 nlist;
00275   int8 plist[4];
00276   int32 tlist[4];
00277 
00278   ec_slave[0].hasdc = FALSE;
00279   ec_group[0].hasdc = FALSE;
00280     ht = 0;
00281     ec_BWR(0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);  /* latch DCrecvTimeA of all slaves */
00282     for (i = 1; i <= ec_slavecount; i++)
00283     {
00284     ec_slave[i].consumedports = ec_slave[i].activeports;
00285         if (ec_slave[i].hasdc)
00286         {
00287             if (!ec_slave[0].hasdc)
00288             {
00289         ec_slave[0].hasdc = TRUE;
00290         ec_slave[0].DCnext = i;
00291         ec_slave[i].DCprevious = 0;
00292         ec_group[0].hasdc = TRUE;
00293         ec_group[0].DCnext = i;
00294             }
00295       else
00296       {
00297         ec_slave[prevDCslave].DCnext = i;
00298         ec_slave[i].DCprevious = prevDCslave;
00299       }
00300       /* this branch has DC slave so remove parenthold */
00301       parenthold = 0;
00302       prevDCslave = i;
00303             slaveh = ec_slave[i].configadr;
00304             wc = ec_FPRD(slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);
00305             ec_slave[i].DCrtA = etohl(ht);
00306             /* 64bit latched DCrecvTimeA of each specific slave */
00307             wc = ec_FPRD(slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET);
00308             /* use it as offset in order to set local time around 0 */
00309             hrt = htoell(-etohll(hrt));
00310             /* save it in the offset register */
00311             wc = ec_FPWR(slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET);
00312             wc = ec_FPRD(slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET);
00313             ec_slave[i].DCrtB = etohl(ht);
00314             wc = ec_FPRD(slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET);
00315             ec_slave[i].DCrtC = etohl(ht);
00316             wc = ec_FPRD(slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET);
00317             ec_slave[i].DCrtD = etohl(ht);
00318 
00319       /* make list of active ports and their time stamps */
00320       nlist = 0;
00321       if (ec_slave[i].activeports & PORTM0) 
00322       {
00323         plist[nlist] = 0;
00324         tlist[nlist] = ec_slave[i].DCrtA;
00325         nlist++;
00326       }
00327       if (ec_slave[i].activeports & PORTM3) 
00328       {
00329         plist[nlist] = 3;
00330         tlist[nlist] = ec_slave[i].DCrtD;
00331         nlist++;
00332       }
00333       if (ec_slave[i].activeports & PORTM1) 
00334       {
00335         plist[nlist] = 1;
00336         tlist[nlist] = ec_slave[i].DCrtB;
00337         nlist++;
00338       }
00339       if (ec_slave[i].activeports & PORTM2) 
00340       {
00341         plist[nlist] = 2;
00342         tlist[nlist] = ec_slave[i].DCrtC;
00343         nlist++;
00344       }
00345       /* entryport is port with the lowest timestamp */
00346       entryport = 0;
00347       if((nlist > 1) && (tlist[1] < tlist[entryport]))
00348       {
00349         entryport = 1;
00350       }     
00351       if((nlist > 2) && (tlist[2] < tlist[entryport]))
00352       {
00353         entryport = 2;
00354       }
00355       if((nlist > 3) && (tlist[3] < tlist[entryport]))
00356       {
00357         entryport = 3;
00358       }
00359       entryport = plist[entryport];
00360       ec_slave[i].entryport = entryport;
00361       /* consume entryport from activeports */
00362       ec_slave[i].consumedports &= (uint8)~(1 << entryport);
00363 
00364             /* finding DC parent of current */
00365             parent = i;
00366             do
00367             {
00368                 child = parent;
00369                 parent = ec_slave[parent].parent;
00370             }
00371             while (!((parent == 0) || (ec_slave[parent].hasdc)));
00372       /* only calculate propagation delay if slave is not the first */
00373             if (parent > 0)
00374             {
00375         /* find port on parent this slave is connected to */
00376         ec_slave[i].parentport = ec_parentport(parent);
00377         if (ec_slave[parent].topology == 1)
00378         {
00379           ec_slave[i].parentport = ec_slave[parent].entryport;
00380         }
00381 
00382         dt1 = 0;
00383         dt2 = 0;
00384         /* delta time of (parentport - 1) - parentport */
00385         /* note: order of ports is 0 - 3 - 1 -2 */
00386         /* non active ports are skipped */
00387         dt3 = ec_porttime(parent, ec_slave[i].parentport) -
00388             ec_porttime(parent, ec_prevport(parent, ec_slave[i].parentport));
00389         /* current slave has children */
00390         /* those childrens delays need to be substacted */
00391         if (ec_slave[i].topology > 1)
00392         {
00393           dt1 = ec_porttime(i, ec_prevport(i, ec_slave[i].entryport)) -
00394                 ec_porttime(i, ec_slave[i].entryport);
00395         }
00396         /* we are only interrested in positive diference */
00397         if (dt1 > dt3) dt1 = -dt1;
00398         /* current slave is not the first child of parent */
00399         /* previous childs delays need to be added */
00400         if ((child - parent) > 1)
00401         {
00402           dt2 = ec_porttime(parent, ec_prevport(parent, ec_slave[i].parentport)) -
00403                 ec_porttime(parent, ec_slave[parent].entryport);
00404         }
00405         if (dt2 < 0) dt2 = -dt2;
00406 
00407         /* calculate current slave delay from delta times */
00408         /* assumption : forward delay equals return delay */
00409                 ec_slave[i].pdelay = ((dt3 - dt1) / 2) + dt2 + ec_slave[parent].pdelay;
00410                 ht = htoel(ec_slave[i].pdelay);
00411                 /* write propagation delay*/
00412                 wc = ec_FPWR(slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET);
00413             }
00414         }
00415         else
00416         {
00417             ec_slave[i].DCrtA = 0;
00418             ec_slave[i].DCrtB = 0;
00419             ec_slave[i].DCrtC = 0;
00420             ec_slave[i].DCrtD = 0;
00421       parent = ec_slave[i].parent;
00422       /* if non DC slave found on first position on branch hold root parent */
00423       if ( (parent > 0) && (ec_slave[parent].topology > 2))
00424         parenthold = parent;
00425       /* if branch has no DC slaves consume port on root parent */
00426       if ( parenthold && (ec_slave[i].topology == 1))
00427       {
00428         ec_parentport(parenthold);
00429         parenthold = 0;
00430       }
00431         }
00432     }
00433 
00434     return ec_slave[0].hasdc;
00435 }
Generated by  doxygen 1.6.3