# HG changeset patch # User Florian Pose # Date 1327933054 -3600 # Node ID 622a7faa07df39092c8a654acba2496e74e99651 # Parent dce13f854817532967cafd8783cf40cc00a57394 Added fix for ESC port order (DC delay calculation). diff -r dce13f854817 -r 622a7faa07df master/master.c --- a/master/master.c Mon Jan 30 15:12:21 2012 +0100 +++ b/master/master.c Mon Jan 30 15:17:34 2012 +0100 @@ -1913,24 +1913,33 @@ ) { ec_slave_t *slave = master->slaves + *slave_position; - unsigned int i; + unsigned int port_index; int ret; + static const unsigned int next_table[EC_MAX_PORTS] = { + 3, 2, 0, 1 + }; + slave->ports[0].next_slave = port0_slave; - for (i = 1; i < EC_MAX_PORTS; i++) { - if (!slave->ports[i].link.loop_closed) { + port_index = 3; + while (port_index != 0) { + if (!slave->ports[port_index].link.loop_closed) { *slave_position = *slave_position + 1; if (*slave_position < master->slave_count) { - slave->ports[i].next_slave = master->slaves + *slave_position; + slave->ports[port_index].next_slave = + master->slaves + *slave_position; ret = ec_master_calc_topology_rec(master, slave, slave_position); - if (ret) + if (ret) { return ret; + } } else { return -1; } } + + port_index = next_table[port_index]; } return 0; diff -r dce13f854817 -r 622a7faa07df master/slave.c --- a/master/slave.c Mon Jan 30 15:12:21 2012 +0100 +++ b/master/slave.c Mon Jan 30 15:17:34 2012 +0100 @@ -803,6 +803,62 @@ /*****************************************************************************/ +/** Returns the previous connected port of a given port. + */ +unsigned int ec_slave_get_previous_port( + ec_slave_t *slave, /**< EtherCAT slave. */ + unsigned int port_index /**< Port index. */ + ) +{ + static const unsigned int prev_table[EC_MAX_PORTS] = { + 2, 3, 1, 0 + }; + + if (port_index >= EC_MAX_PORTS) { + EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n", + __func__, port_index); + } + + do { + port_index = prev_table[port_index]; + if (slave->ports[port_index].next_slave) { + return port_index; + } + } while (port_index); + + 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 port_index /**< Port index. */ + ) +{ + static const unsigned int next_table[EC_MAX_PORTS] = { + 3, 2, 0, 1 + }; + + if (port_index >= EC_MAX_PORTS) { + EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n", + __func__, port_index); + } + + do { + port_index = next_table[port_index]; + if (slave->ports[port_index].next_slave) { + return port_index; + } + } while (port_index); + + return 0; +} + +/*****************************************************************************/ + /** Calculates the sum of round-trip-times of connected ports 1-3. */ uint32_t ec_slave_calc_rtt_sum( @@ -810,13 +866,16 @@ ) { 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 port_index = ec_slave_get_next_port(slave, 0); + + while (port_index != 0) { + unsigned int prev_index = + ec_slave_get_previous_port(slave, port_index); + + rtt = slave->ports[port_index].receive_time - + slave->ports[prev_index].receive_time; + rtt_sum += rtt; + port_index = ec_slave_get_next_port(slave, port_index); } return rtt_sum; @@ -830,20 +889,25 @@ ec_slave_t *slave /**< EtherCAT slave. */ ) { + unsigned int port_index; 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++) { - ec_slave_t *next = slave->ports[i].next_slave; + port_index = ec_slave_get_next_port(slave, 0); + + while (port_index != 0) { + ec_slave_t *next = slave->ports[port_index].next_slave; + if (next) { dc_slave = ec_slave_find_next_dc_slave(next); - if (dc_slave) + + if (dc_slave) { break; + } } + port_index = ec_slave_get_next_port(slave, port_index); } } @@ -858,33 +922,41 @@ ec_slave_t *slave /**< EtherCAT slave. */ ) { - unsigned int i; - ec_slave_t *next, *next_dc; + unsigned int port_index; + ec_slave_t *next_slave, *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; + port_index = ec_slave_get_next_port(slave, 0); + + while (port_index != 0) { + next_slave = slave->ports[port_index].next_slave; + next_dc = ec_slave_find_next_dc_slave(next_slave); + + if (next_dc) { + unsigned int prev_port = + ec_slave_get_previous_port(slave, port_index); + + rtt = slave->ports[port_index].receive_time - + slave->ports[prev_port].receive_time; + next_rtt_sum = ec_slave_calc_rtt_sum(next_dc); + + slave->ports[port_index].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, port_index, rtt, next_rtt_sum, + slave->ports[port_index].delay_to_next_dc); #endif + } + + port_index = ec_slave_get_next_port(slave, port_index); } } @@ -898,28 +970,27 @@ ) { unsigned int i; - ec_slave_t *next, *next_dc; - + ec_slave_t *next_dc; + + EC_SLAVE_DBG(slave, 1, "%s(delay = %u ns)\n", __func__, *delay); + + slave->transmission_delay = *delay; + + i = ec_slave_get_next_port(slave, 0); + + while (i != 0) { + ec_slave_port_t *port = &slave->ports[i]; + 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\n", *delay); + EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", + slave->ring_position, i, *delay); #endif - - slave->transmission_delay = *delay; - - for (i = 1; i < EC_MAX_PORTS; i++) { - 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; -#if 0 - 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;