From 57b57fe71db9b722ca06f69c5d387d3d4a84c5d5 Mon Sep 17 00:00:00 2001 From: Martin Troxler Date: Tue, 16 Nov 2010 15:32:27 +0100 Subject: [PATCH] Use ESC port order (0-3-1-2) for bus topology and DC delay calculation --- master/fsm_master.c | 4 ++ master/master.c | 11 +++- master/slave.c | 128 ++++++++++++++++++++++++++++++-------------- 3 files changed, 102 insertions(+), 41 deletions(-) diff --git a/master/fsm_master.c b/master/fsm_master.c index df53041c..e0b41971 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -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 /*****************************************************************************/ diff --git a/master/master.c b/master/master.c index af33527a..e1983c08 100644 --- a/master/master.c +++ b/master/master.c @@ -1814,7 +1814,8 @@ int ec_master_calc_topology_rec( 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 @@ int ec_master_calc_topology_rec( 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; diff --git a/master/slave.c b/master/slave.c index 7a0ec9ab..9f67f3c6 100644 --- a/master/slave.c +++ b/master/slave.c @@ -801,6 +801,59 @@ void ec_slave_attach_pdo_names( } } +/*****************************************************************************/ + +/** 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. @@ -810,13 +863,11 @@ uint32_t ec_slave_calc_rtt_sum( ) { 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 *ec_slave_find_next_dc_slave( 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 @@ void ec_slave_calc_port_delays( ) { 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; + 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); - 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; + 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 @@ void ec_slave_calc_transmission_delays_rec( ) { unsigned int i; - ec_slave_t *next, *next_dc; + ec_slave_t *next_dc; -#if 0 +#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;