77 ec_fsm_master_t *fsm, /**< Master state machine. */ |
77 ec_fsm_master_t *fsm, /**< Master state machine. */ |
78 ec_master_t *master, /**< EtherCAT master. */ |
78 ec_master_t *master, /**< EtherCAT master. */ |
79 ec_datagram_t *datagram /**< Datagram object to use. */ |
79 ec_datagram_t *datagram /**< Datagram object to use. */ |
80 ) |
80 ) |
81 { |
81 { |
82 ec_device_index_t dev_idx; |
|
83 |
|
84 fsm->master = master; |
82 fsm->master = master; |
85 fsm->datagram = datagram; |
83 fsm->datagram = datagram; |
86 fsm->state = ec_fsm_master_state_start; |
84 |
87 fsm->idle = 0; |
85 ec_fsm_master_reset(fsm); |
88 fsm->dev_idx = EC_DEVICE_MAIN; |
|
89 for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master); |
|
90 dev_idx++) { |
|
91 fsm->link_state[dev_idx] = 0; |
|
92 fsm->slaves_responding[dev_idx] = 0; |
|
93 fsm->slave_states[dev_idx] = EC_SLAVE_STATE_UNKNOWN; |
|
94 } |
|
95 fsm->rescan_required = 0; |
|
96 |
86 |
97 // init sub-state-machines |
87 // init sub-state-machines |
98 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
88 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
99 ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); |
89 ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); |
100 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
90 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
118 ec_fsm_pdo_clear(&fsm->fsm_pdo); |
108 ec_fsm_pdo_clear(&fsm->fsm_pdo); |
119 ec_fsm_change_clear(&fsm->fsm_change); |
109 ec_fsm_change_clear(&fsm->fsm_change); |
120 ec_fsm_slave_config_clear(&fsm->fsm_slave_config); |
110 ec_fsm_slave_config_clear(&fsm->fsm_slave_config); |
121 ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan); |
111 ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan); |
122 ec_fsm_sii_clear(&fsm->fsm_sii); |
112 ec_fsm_sii_clear(&fsm->fsm_sii); |
|
113 } |
|
114 |
|
115 /*****************************************************************************/ |
|
116 |
|
117 /** Reset state machine. |
|
118 */ |
|
119 void ec_fsm_master_reset( |
|
120 ec_fsm_master_t *fsm /**< Master state machine. */ |
|
121 ) |
|
122 { |
|
123 ec_device_index_t dev_idx; |
|
124 |
|
125 fsm->state = ec_fsm_master_state_start; |
|
126 fsm->idle = 0; |
|
127 fsm->dev_idx = EC_DEVICE_MAIN; |
|
128 |
|
129 for (dev_idx = EC_DEVICE_MAIN; |
|
130 dev_idx < ec_master_num_devices(fsm->master); dev_idx++) { |
|
131 fsm->link_state[dev_idx] = 0; |
|
132 fsm->slaves_responding[dev_idx] = 0; |
|
133 fsm->slave_states[dev_idx] = EC_SLAVE_STATE_UNKNOWN; |
|
134 } |
|
135 |
|
136 fsm->rescan_required = 0; |
123 } |
137 } |
124 |
138 |
125 /*****************************************************************************/ |
139 /*****************************************************************************/ |
126 |
140 |
127 /** Executes the current state of the state machine. |
141 /** Executes the current state of the state machine. |
609 ) |
623 ) |
610 { |
624 { |
611 ec_slave_t *slave = fsm->slave; |
625 ec_slave_t *slave = fsm->slave; |
612 ec_datagram_t *datagram = fsm->datagram; |
626 ec_datagram_t *datagram = fsm->datagram; |
613 |
627 |
614 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
628 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
615 return; |
629 return; |
|
630 } |
616 |
631 |
617 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
632 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
618 EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: "); |
633 EC_SLAVE_ERR(slave, "Failed to receive AL state datagram: "); |
619 ec_datagram_print_state(datagram); |
634 ec_datagram_print_state(datagram); |
620 ec_fsm_master_restart(fsm); |
635 ec_fsm_master_restart(fsm); |
745 ) |
760 ) |
746 { |
761 { |
747 ec_master_t *master = fsm->master; |
762 ec_master_t *master = fsm->master; |
748 ec_datagram_t *datagram = fsm->datagram; |
763 ec_datagram_t *datagram = fsm->datagram; |
749 |
764 |
750 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
765 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
751 return; |
766 return; |
|
767 } |
752 |
768 |
753 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
769 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
754 EC_MASTER_ERR(master, "Failed to receive delay measuring datagram" |
770 EC_MASTER_ERR(master, "Failed to receive delay measuring datagram" |
755 " on %s link: ", ec_device_names[fsm->dev_idx != 0]); |
771 " on %s link: ", ec_device_names[fsm->dev_idx != 0]); |
756 ec_datagram_print_state(datagram); |
772 ec_datagram_print_state(datagram); |