Use ESC port order (0-3-1-2) for bus topology and DC delay calculation
authorMartin Troxler <ch1010277@ch10pc446>
Tue, 16 Nov 2010 15:32:27 +0100
changeset 1998 0330fdcbdd1f
parent 1997 65bcd7e304cf
child 1999 5f1269a71588
Use ESC port order (0-3-1-2) for bus topology and DC delay calculation
master/fsm_master.c
master/master.c
master/slave.c
--- a/master/fsm_master.c	Tue Nov 09 11:12:35 2010 +0100
+++ b/master/fsm_master.c	Tue Nov 16 15:32:27 2010 +0100
@@ -48,7 +48,11 @@
 
 /** Time difference [ns] to tolerate without setting a new system time offset.
  */
+#ifdef EC_HAVE_CYCLES
+#define EC_SYSTEM_TIME_TOLERANCE_NS 10000
+#else
 #define EC_SYSTEM_TIME_TOLERANCE_NS 100000000
+#endif
 
 /*****************************************************************************/
 
--- a/master/master.c	Tue Nov 09 11:12:35 2010 +0100
+++ b/master/master.c	Tue Nov 16 15:32:27 2010 +0100
@@ -1814,7 +1814,8 @@
 
     slave->ports[0].next_slave = port0_slave;
 
-    for (i = 1; i < EC_MAX_PORTS; i++) {
+    i = 3;
+    while (i != 0) {
         if (!slave->ports[i].link.loop_closed) {
             *slave_position = *slave_position + 1;
             if (*slave_position < master->slave_count) {
@@ -1827,6 +1828,14 @@
                 return -1;
             }
         }
+        switch (i)
+        {
+        case 0: i = 3; break;
+        case 1: i = 2; break;
+        case 3: i = 1; break;
+        case 2:
+        default:i = 0; break;
+        }
     }
 
     return 0;
--- a/master/slave.c	Tue Nov 09 11:12:35 2010 +0100
+++ b/master/slave.c	Tue Nov 16 15:32:27 2010 +0100
@@ -803,6 +803,59 @@
 
 /*****************************************************************************/
 
+/** returns the previous connected port of a given port.
+ */
+
+unsigned int ec_slave_get_previous_port(
+    ec_slave_t *slave, /**< EtherCAT slave. */
+    unsigned int i /**< Port index */
+    )
+{
+    do
+    {
+        switch (i)
+        {
+        case 0: i = 2; break;
+        case 1: i = 3; break;
+        case 2: i = 1; break;
+        case 3:
+        default:i = 0; break;
+        }
+        if (slave->ports[i].next_slave)
+            return i;
+    } while (i);
+    return 0;
+}
+
+/*****************************************************************************/
+
+/** returns the next connected port of a given port.
+ */
+
+unsigned int ec_slave_get_next_port(
+    ec_slave_t *slave, /**< EtherCAT slave. */
+    unsigned int i /**< Port index */
+    )
+{
+    do
+    {
+        switch (i)
+        {
+        case 0: i = 3; break;
+        case 1: i = 2; break;
+        case 3: i = 1; break;
+        case 2:
+        default:i = 0; break;
+        }
+        if (slave->ports[i].next_slave)
+            return i;
+    } while (i);
+    return 0;
+}
+
+
+/*****************************************************************************/
+
 /** Calculates the sum of round-trip-times of connected ports 1-3.
  */
 uint32_t ec_slave_calc_rtt_sum(
@@ -810,13 +863,11 @@
         )
 {
     uint32_t rtt_sum = 0, rtt;
-    unsigned int i;
-    
-    for (i = 1; i < EC_MAX_PORTS; i++) {
-        if (slave->ports[i].next_slave) {
-            rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time;
-            rtt_sum += rtt;
-        }
+    unsigned int i = ec_slave_get_next_port(slave,0);
+    while (i != 0) {
+        rtt = slave->ports[i].receive_time - slave->ports[ec_slave_get_previous_port(slave,i)].receive_time;
+        rtt_sum += rtt;
+        i = ec_slave_get_next_port(slave,i);
     }
 
     return rtt_sum;
@@ -830,20 +881,21 @@
         ec_slave_t *slave /**< EtherCAT slave. */
         )
 {
+    unsigned int i;
     ec_slave_t *dc_slave = NULL;
 
     if (slave->base_dc_supported) {
         dc_slave = slave;
     } else {
-        unsigned int i;
-
-        for (i = 1; i < EC_MAX_PORTS; i++) {
+        i = ec_slave_get_next_port(slave,0);
+        while (i != 0) {
             ec_slave_t *next = slave->ports[i].next_slave;
             if (next) {
                 dc_slave = ec_slave_find_next_dc_slave(next);
                 if (dc_slave)
                     break;
             }
+            i = ec_slave_get_next_port(slave,i);
         }
     }
 
@@ -859,32 +911,30 @@
         )
 {
     unsigned int i;
-    ec_slave_t *next, *next_dc;
+    ec_slave_t *next_dc;
     uint32_t rtt, next_rtt_sum;
 
     if (!slave->base_dc_supported)
         return;
 
-    for (i = 1; i < EC_MAX_PORTS; i++) {
-        next = slave->ports[i].next_slave;
-        if (!next)
-            continue;
-        next_dc = ec_slave_find_next_dc_slave(next);
-        if (!next_dc)
-            continue;
-
-        rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time;
-        next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
-
-        slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME
-        next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2;
+    i = ec_slave_get_next_port(slave,0);
+    while (i != 0) {
+        next_dc = ec_slave_find_next_dc_slave(slave->ports[i].next_slave);
+        if (next_dc) {
+            rtt = slave->ports[i].receive_time - slave->ports[ec_slave_get_previous_port(slave,i)].receive_time;
+            next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
+
+            slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME
+            next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2;
 
 #if 0
-        EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u"
-                " next_rtt_sum=%u delay=%u\n",
-                slave->ring_position, i, rtt, next_rtt_sum,
-                slave->ports[i].delay_to_next_dc);
+            EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u"
+                    " next_rtt_sum=%u delay=%u\n",
+                    slave->ring_position, i, rtt, next_rtt_sum,
+                    slave->ports[i].delay_to_next_dc);
 #endif
+        }
+        i = ec_slave_get_next_port(slave,i);
     }
 }
 
@@ -898,28 +948,26 @@
         )
 {
     unsigned int i;
-    ec_slave_t *next, *next_dc;
-
-#if 0
+    ec_slave_t *next_dc;
+
+#if 1
     EC_SLAVE_DBG(slave, 1, "%u\n", *delay);
 #endif
 
     slave->transmission_delay = *delay;
 
-    for (i = 1; i < EC_MAX_PORTS; i++) {
+    i = ec_slave_get_next_port(slave,0);
+    while (i != 0) {
         ec_slave_port_t *port = &slave->ports[i];
-        next = port->next_slave;
-        if (!next)
-            continue;
-        next_dc = ec_slave_find_next_dc_slave(next);
-        if (!next_dc)
-            continue;
-
-        *delay = *delay + port->delay_to_next_dc;
+        next_dc = ec_slave_find_next_dc_slave(port->next_slave);
+        if (next_dc) {
+            *delay = *delay + port->delay_to_next_dc;
 #if 0
-        EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay);
+            EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay);
 #endif
-        ec_slave_calc_transmission_delays_rec(next_dc, delay);
+            ec_slave_calc_transmission_delays_rec(next_dc, delay);
+        }
+        i = ec_slave_get_next_port(slave,i);
     }
 
     *delay = *delay + slave->ports[0].delay_to_next_dc;