# HG changeset patch # User Martin Troxler # Date 1289917947 -3600 # Node ID 0330fdcbdd1fe96acd3be6156d8953711fdf83f6 # Parent 65bcd7e304cf97fb47e6b917ee229047220af9a7 Use ESC port order (0-3-1-2) for bus topology and DC delay calculation diff -r 65bcd7e304cf -r 0330fdcbdd1f master/fsm_master.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 /*****************************************************************************/ diff -r 65bcd7e304cf -r 0330fdcbdd1f master/master.c --- 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; diff -r 65bcd7e304cf -r 0330fdcbdd1f master/slave.c --- 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;