master/slave.c
changeset 1998 0330fdcbdd1f
parent 1924 2917b262554b
child 2027 ac35f4d38a31
equal deleted inserted replaced
1997:65bcd7e304cf 1998:0330fdcbdd1f
   801     }
   801     }
   802 }
   802 }
   803 
   803 
   804 /*****************************************************************************/
   804 /*****************************************************************************/
   805 
   805 
       
   806 /** returns the previous connected port of a given port.
       
   807  */
       
   808 
       
   809 unsigned int ec_slave_get_previous_port(
       
   810     ec_slave_t *slave, /**< EtherCAT slave. */
       
   811     unsigned int i /**< Port index */
       
   812     )
       
   813 {
       
   814     do
       
   815     {
       
   816         switch (i)
       
   817         {
       
   818         case 0: i = 2; break;
       
   819         case 1: i = 3; break;
       
   820         case 2: i = 1; break;
       
   821         case 3:
       
   822         default:i = 0; break;
       
   823         }
       
   824         if (slave->ports[i].next_slave)
       
   825             return i;
       
   826     } while (i);
       
   827     return 0;
       
   828 }
       
   829 
       
   830 /*****************************************************************************/
       
   831 
       
   832 /** returns the next connected port of a given port.
       
   833  */
       
   834 
       
   835 unsigned int ec_slave_get_next_port(
       
   836     ec_slave_t *slave, /**< EtherCAT slave. */
       
   837     unsigned int i /**< Port index */
       
   838     )
       
   839 {
       
   840     do
       
   841     {
       
   842         switch (i)
       
   843         {
       
   844         case 0: i = 3; break;
       
   845         case 1: i = 2; break;
       
   846         case 3: i = 1; break;
       
   847         case 2:
       
   848         default:i = 0; break;
       
   849         }
       
   850         if (slave->ports[i].next_slave)
       
   851             return i;
       
   852     } while (i);
       
   853     return 0;
       
   854 }
       
   855 
       
   856 
       
   857 /*****************************************************************************/
       
   858 
   806 /** Calculates the sum of round-trip-times of connected ports 1-3.
   859 /** Calculates the sum of round-trip-times of connected ports 1-3.
   807  */
   860  */
   808 uint32_t ec_slave_calc_rtt_sum(
   861 uint32_t ec_slave_calc_rtt_sum(
   809         ec_slave_t *slave /**< EtherCAT slave. */
   862         ec_slave_t *slave /**< EtherCAT slave. */
   810         )
   863         )
   811 {
   864 {
   812     uint32_t rtt_sum = 0, rtt;
   865     uint32_t rtt_sum = 0, rtt;
   813     unsigned int i;
   866     unsigned int i = ec_slave_get_next_port(slave,0);
   814     
   867     while (i != 0) {
   815     for (i = 1; i < EC_MAX_PORTS; i++) {
   868         rtt = slave->ports[i].receive_time - slave->ports[ec_slave_get_previous_port(slave,i)].receive_time;
   816         if (slave->ports[i].next_slave) {
   869         rtt_sum += rtt;
   817             rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time;
   870         i = ec_slave_get_next_port(slave,i);
   818             rtt_sum += rtt;
       
   819         }
       
   820     }
   871     }
   821 
   872 
   822     return rtt_sum;
   873     return rtt_sum;
   823 }
   874 }
   824 
   875 
   828  */
   879  */
   829 ec_slave_t *ec_slave_find_next_dc_slave(
   880 ec_slave_t *ec_slave_find_next_dc_slave(
   830         ec_slave_t *slave /**< EtherCAT slave. */
   881         ec_slave_t *slave /**< EtherCAT slave. */
   831         )
   882         )
   832 {
   883 {
       
   884     unsigned int i;
   833     ec_slave_t *dc_slave = NULL;
   885     ec_slave_t *dc_slave = NULL;
   834 
   886 
   835     if (slave->base_dc_supported) {
   887     if (slave->base_dc_supported) {
   836         dc_slave = slave;
   888         dc_slave = slave;
   837     } else {
   889     } else {
   838         unsigned int i;
   890         i = ec_slave_get_next_port(slave,0);
   839 
   891         while (i != 0) {
   840         for (i = 1; i < EC_MAX_PORTS; i++) {
       
   841             ec_slave_t *next = slave->ports[i].next_slave;
   892             ec_slave_t *next = slave->ports[i].next_slave;
   842             if (next) {
   893             if (next) {
   843                 dc_slave = ec_slave_find_next_dc_slave(next);
   894                 dc_slave = ec_slave_find_next_dc_slave(next);
   844                 if (dc_slave)
   895                 if (dc_slave)
   845                     break;
   896                     break;
   846             }
   897             }
       
   898             i = ec_slave_get_next_port(slave,i);
   847         }
   899         }
   848     }
   900     }
   849 
   901 
   850     return dc_slave;
   902     return dc_slave;
   851 }
   903 }
   857 void ec_slave_calc_port_delays(
   909 void ec_slave_calc_port_delays(
   858         ec_slave_t *slave /**< EtherCAT slave. */
   910         ec_slave_t *slave /**< EtherCAT slave. */
   859         )
   911         )
   860 {
   912 {
   861     unsigned int i;
   913     unsigned int i;
   862     ec_slave_t *next, *next_dc;
   914     ec_slave_t *next_dc;
   863     uint32_t rtt, next_rtt_sum;
   915     uint32_t rtt, next_rtt_sum;
   864 
   916 
   865     if (!slave->base_dc_supported)
   917     if (!slave->base_dc_supported)
   866         return;
   918         return;
   867 
   919 
   868     for (i = 1; i < EC_MAX_PORTS; i++) {
   920     i = ec_slave_get_next_port(slave,0);
   869         next = slave->ports[i].next_slave;
   921     while (i != 0) {
   870         if (!next)
   922         next_dc = ec_slave_find_next_dc_slave(slave->ports[i].next_slave);
   871             continue;
   923         if (next_dc) {
   872         next_dc = ec_slave_find_next_dc_slave(next);
   924             rtt = slave->ports[i].receive_time - slave->ports[ec_slave_get_previous_port(slave,i)].receive_time;
   873         if (!next_dc)
   925             next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
   874             continue;
   926 
   875 
   927             slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME
   876         rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time;
   928             next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2;
   877         next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
       
   878 
       
   879         slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME
       
   880         next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2;
       
   881 
   929 
   882 #if 0
   930 #if 0
   883         EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u"
   931             EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u"
   884                 " next_rtt_sum=%u delay=%u\n",
   932                     " next_rtt_sum=%u delay=%u\n",
   885                 slave->ring_position, i, rtt, next_rtt_sum,
   933                     slave->ring_position, i, rtt, next_rtt_sum,
   886                 slave->ports[i].delay_to_next_dc);
   934                     slave->ports[i].delay_to_next_dc);
   887 #endif
   935 #endif
       
   936         }
       
   937         i = ec_slave_get_next_port(slave,i);
   888     }
   938     }
   889 }
   939 }
   890 
   940 
   891 /*****************************************************************************/
   941 /*****************************************************************************/
   892 
   942 
   896         ec_slave_t *slave, /**< Current slave. */
   946         ec_slave_t *slave, /**< Current slave. */
   897         uint32_t *delay /**< Sum of delays. */
   947         uint32_t *delay /**< Sum of delays. */
   898         )
   948         )
   899 {
   949 {
   900     unsigned int i;
   950     unsigned int i;
   901     ec_slave_t *next, *next_dc;
   951     ec_slave_t *next_dc;
   902 
   952 
   903 #if 0
   953 #if 1
   904     EC_SLAVE_DBG(slave, 1, "%u\n", *delay);
   954     EC_SLAVE_DBG(slave, 1, "%u\n", *delay);
   905 #endif
   955 #endif
   906 
   956 
   907     slave->transmission_delay = *delay;
   957     slave->transmission_delay = *delay;
   908 
   958 
   909     for (i = 1; i < EC_MAX_PORTS; i++) {
   959     i = ec_slave_get_next_port(slave,0);
       
   960     while (i != 0) {
   910         ec_slave_port_t *port = &slave->ports[i];
   961         ec_slave_port_t *port = &slave->ports[i];
   911         next = port->next_slave;
   962         next_dc = ec_slave_find_next_dc_slave(port->next_slave);
   912         if (!next)
   963         if (next_dc) {
   913             continue;
   964             *delay = *delay + port->delay_to_next_dc;
   914         next_dc = ec_slave_find_next_dc_slave(next);
       
   915         if (!next_dc)
       
   916             continue;
       
   917 
       
   918         *delay = *delay + port->delay_to_next_dc;
       
   919 #if 0
   965 #if 0
   920         EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay);
   966             EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay);
   921 #endif
   967 #endif
   922         ec_slave_calc_transmission_delays_rec(next_dc, delay);
   968             ec_slave_calc_transmission_delays_rec(next_dc, delay);
       
   969         }
       
   970         i = ec_slave_get_next_port(slave,i);
   923     }
   971     }
   924 
   972 
   925     *delay = *delay + slave->ports[0].delay_to_next_dc;
   973     *delay = *delay + slave->ports[0].delay_to_next_dc;
   926 }
   974 }
   927 
   975