42 #include "master.h" |
42 #include "master.h" |
43 #include "fsm_change.h" |
43 #include "fsm_change.h" |
44 |
44 |
45 /*****************************************************************************/ |
45 /*****************************************************************************/ |
46 |
46 |
47 void ec_fsm_change_start(ec_fsm_change_t *); |
47 void ec_fsm_change_state_start(ec_fsm_change_t *); |
48 void ec_fsm_change_check(ec_fsm_change_t *); |
48 void ec_fsm_change_state_check(ec_fsm_change_t *); |
49 void ec_fsm_change_status(ec_fsm_change_t *); |
49 void ec_fsm_change_state_status(ec_fsm_change_t *); |
50 void ec_fsm_change_code(ec_fsm_change_t *); |
50 void ec_fsm_change_state_code(ec_fsm_change_t *); |
51 void ec_fsm_change_ack(ec_fsm_change_t *); |
51 void ec_fsm_change_state_start_ack(ec_fsm_change_t *); |
52 void ec_fsm_change_check_ack(ec_fsm_change_t *); |
52 void ec_fsm_change_state_ack(ec_fsm_change_t *); |
53 void ec_fsm_change_end(ec_fsm_change_t *); |
53 void ec_fsm_change_state_check_ack(ec_fsm_change_t *); |
54 void ec_fsm_change_error(ec_fsm_change_t *); |
54 void ec_fsm_change_state_end(ec_fsm_change_t *); |
|
55 void ec_fsm_change_state_error(ec_fsm_change_t *); |
55 |
56 |
56 /*****************************************************************************/ |
57 /*****************************************************************************/ |
57 |
58 |
58 /** |
59 /** |
59 Constructor. |
60 Constructor. |
78 } |
79 } |
79 |
80 |
80 /*****************************************************************************/ |
81 /*****************************************************************************/ |
81 |
82 |
82 /** |
83 /** |
83 Resets the state machine. |
84 Starts the change state machine. |
84 */ |
85 */ |
85 |
86 |
86 void ec_fsm_change(ec_fsm_change_t *fsm, /**< finite state machine */ |
87 void ec_fsm_change_start(ec_fsm_change_t *fsm, /**< finite state machine */ |
87 ec_slave_t *slave, /**< EtherCAT slave */ |
88 ec_slave_t *slave, /**< EtherCAT slave */ |
88 ec_slave_state_t state /**< requested state */ |
89 ec_slave_state_t state /**< requested state */ |
89 ) |
90 ) |
90 { |
91 { |
|
92 fsm->mode = EC_FSM_CHANGE_MODE_FULL; |
91 fsm->slave = slave; |
93 fsm->slave = slave; |
92 fsm->requested_state = state; |
94 fsm->requested_state = state; |
93 fsm->state = ec_fsm_change_start; |
95 fsm->state = ec_fsm_change_state_start; |
|
96 } |
|
97 |
|
98 /*****************************************************************************/ |
|
99 |
|
100 /** |
|
101 Starts the change state machine to only acknowlegde a slave's state. |
|
102 */ |
|
103 |
|
104 void ec_fsm_change_ack(ec_fsm_change_t *fsm, /**< finite state machine */ |
|
105 ec_slave_t *slave /**< EtherCAT slave */ |
|
106 ) |
|
107 { |
|
108 fsm->mode = EC_FSM_CHANGE_MODE_ACK_ONLY; |
|
109 fsm->slave = slave; |
|
110 fsm->requested_state = EC_SLAVE_STATE_UNKNOWN; |
|
111 fsm->state = ec_fsm_change_state_start_ack; |
94 } |
112 } |
95 |
113 |
96 /*****************************************************************************/ |
114 /*****************************************************************************/ |
97 |
115 |
98 /** |
116 /** |
115 \return non-zero if successful. |
133 \return non-zero if successful. |
116 */ |
134 */ |
117 |
135 |
118 int ec_fsm_change_success(ec_fsm_change_t *fsm /**< Finite state machine */) |
136 int ec_fsm_change_success(ec_fsm_change_t *fsm /**< Finite state machine */) |
119 { |
137 { |
120 return fsm->state == ec_fsm_change_end; |
138 return fsm->state == ec_fsm_change_state_end; |
121 } |
139 } |
122 |
140 |
123 /****************************************************************************** |
141 /****************************************************************************** |
124 * state change state machine |
142 * state change state machine |
125 *****************************************************************************/ |
143 *****************************************************************************/ |
126 |
144 |
127 /** |
145 /** |
128 Change state: START. |
146 Change state: START. |
129 */ |
147 */ |
130 |
148 |
131 void ec_fsm_change_start(ec_fsm_change_t *fsm /**< finite state machine */) |
149 void ec_fsm_change_state_start(ec_fsm_change_t *fsm |
|
150 /**< finite state machine */) |
132 { |
151 { |
133 ec_datagram_t *datagram = fsm->datagram; |
152 ec_datagram_t *datagram = fsm->datagram; |
134 ec_slave_t *slave = fsm->slave; |
153 ec_slave_t *slave = fsm->slave; |
135 |
154 |
136 fsm->take_time = 1; |
155 fsm->take_time = 1; |
|
156 fsm->old_state = fsm->slave->current_state; |
137 |
157 |
138 // write new state to slave |
158 // write new state to slave |
139 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
159 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
140 EC_WRITE_U16(datagram->data, fsm->requested_state); |
160 EC_WRITE_U16(datagram->data, fsm->requested_state); |
141 ec_master_queue_datagram(fsm->slave->master, datagram); |
161 ec_master_queue_datagram(fsm->slave->master, datagram); |
142 fsm->state = ec_fsm_change_check; |
162 fsm->state = ec_fsm_change_state_check; |
143 } |
163 } |
144 |
164 |
145 /*****************************************************************************/ |
165 /*****************************************************************************/ |
146 |
166 |
147 /** |
167 /** |
148 Change state: CHECK. |
168 Change state: CHECK. |
149 */ |
169 */ |
150 |
170 |
151 void ec_fsm_change_check(ec_fsm_change_t *fsm /**< finite state machine */) |
171 void ec_fsm_change_state_check(ec_fsm_change_t *fsm |
|
172 /**< finite state machine */) |
152 { |
173 { |
153 ec_datagram_t *datagram = fsm->datagram; |
174 ec_datagram_t *datagram = fsm->datagram; |
154 ec_slave_t *slave = fsm->slave; |
175 ec_slave_t *slave = fsm->slave; |
155 |
176 |
156 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
177 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
157 fsm->state = ec_fsm_change_error; |
178 fsm->state = ec_fsm_change_state_error; |
158 EC_ERR("Failed to send state datagram to slave %i!\n", |
179 EC_ERR("Failed to send state datagram to slave %i!\n", |
159 fsm->slave->ring_position); |
180 fsm->slave->ring_position); |
160 return; |
181 return; |
161 } |
182 } |
162 |
183 |
167 |
188 |
168 if (datagram->working_counter != 1) { |
189 if (datagram->working_counter != 1) { |
169 if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) { |
190 if (datagram->jiffies_received - fsm->jiffies_start >= 3 * HZ) { |
170 char state_str[EC_STATE_STRING_SIZE]; |
191 char state_str[EC_STATE_STRING_SIZE]; |
171 ec_state_string(fsm->requested_state, state_str); |
192 ec_state_string(fsm->requested_state, state_str); |
172 fsm->state = ec_fsm_change_error; |
193 fsm->state = ec_fsm_change_state_error; |
173 EC_ERR("Failed to set state %s on slave %i: Slave did not" |
194 EC_ERR("Failed to set state %s on slave %i: Slave did not" |
174 " respond.\n", state_str, fsm->slave->ring_position); |
195 " respond.\n", state_str, fsm->slave->ring_position); |
175 return; |
196 return; |
176 } |
197 } |
177 |
198 |
185 fsm->take_time = 1; |
206 fsm->take_time = 1; |
186 |
207 |
187 // read AL status from slave |
208 // read AL status from slave |
188 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
209 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
189 ec_master_queue_datagram(fsm->slave->master, datagram); |
210 ec_master_queue_datagram(fsm->slave->master, datagram); |
190 fsm->state = ec_fsm_change_status; |
211 fsm->state = ec_fsm_change_state_status; |
191 } |
212 } |
192 |
213 |
193 /*****************************************************************************/ |
214 /*****************************************************************************/ |
194 |
215 |
195 /** |
216 /** |
196 Change state: STATUS. |
217 Change state: STATUS. |
197 */ |
218 */ |
198 |
219 |
199 void ec_fsm_change_status(ec_fsm_change_t *fsm /**< finite state machine */) |
220 void ec_fsm_change_state_status(ec_fsm_change_t *fsm |
|
221 /**< finite state machine */) |
200 { |
222 { |
201 ec_datagram_t *datagram = fsm->datagram; |
223 ec_datagram_t *datagram = fsm->datagram; |
202 ec_slave_t *slave = fsm->slave; |
224 ec_slave_t *slave = fsm->slave; |
203 |
225 |
204 if (datagram->state != EC_DATAGRAM_RECEIVED |
226 if (datagram->state != EC_DATAGRAM_RECEIVED |
205 || datagram->working_counter != 1) { |
227 || datagram->working_counter != 1) { |
206 fsm->state = ec_fsm_change_error; |
228 fsm->state = ec_fsm_change_state_error; |
207 EC_ERR("Failed to check state 0x%02X on slave %i.\n", |
229 EC_ERR("Failed to check state 0x%02X on slave %i.\n", |
208 fsm->requested_state, slave->ring_position); |
230 fsm->requested_state, slave->ring_position); |
209 return; |
231 return; |
210 } |
232 } |
211 |
233 |
216 |
238 |
217 slave->current_state = EC_READ_U8(datagram->data); |
239 slave->current_state = EC_READ_U8(datagram->data); |
218 |
240 |
219 if (slave->current_state == fsm->requested_state) { |
241 if (slave->current_state == fsm->requested_state) { |
220 // state has been set successfully |
242 // state has been set successfully |
221 fsm->state = ec_fsm_change_end; |
243 fsm->state = ec_fsm_change_state_end; |
222 return; |
244 return; |
223 } |
245 } |
224 |
246 |
225 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
247 if (slave->current_state != fsm->old_state) { // state changed |
226 // state change error |
|
227 char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE]; |
248 char req_state[EC_STATE_STRING_SIZE], cur_state[EC_STATE_STRING_SIZE]; |
|
249 |
|
250 slave->error_flag = 1; |
228 ec_state_string(fsm->requested_state, req_state); |
251 ec_state_string(fsm->requested_state, req_state); |
229 ec_state_string(slave->current_state, cur_state); |
252 ec_state_string(slave->current_state, cur_state); |
230 EC_ERR("Failed to set %s state, slave %i refused state change (%s).\n", |
253 |
231 req_state, slave->ring_position, cur_state); |
254 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
232 // fetch AL status error code |
255 // state change error |
233 ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); |
256 EC_ERR("Failed to set %s state," |
234 ec_master_queue_datagram(fsm->slave->master, datagram); |
257 " slave %i refused state change (%s).\n", |
235 fsm->state = ec_fsm_change_code; |
258 req_state, slave->ring_position, cur_state); |
236 return; |
259 // fetch AL status error code |
237 } |
260 ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); |
|
261 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
262 fsm->state = ec_fsm_change_state_code; |
|
263 return; |
|
264 } |
|
265 |
|
266 // state change to unrequested state |
|
267 EC_ERR("Slave %i changed to unrequested state %s!\n", |
|
268 slave->ring_position, cur_state); |
|
269 fsm->state = ec_fsm_change_state_error; |
|
270 return; |
|
271 } |
|
272 |
|
273 // still old state |
238 |
274 |
239 if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s |
275 if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s |
240 // timeout while checking |
276 // timeout while checking |
241 char state_str[EC_STATE_STRING_SIZE]; |
277 char state_str[EC_STATE_STRING_SIZE]; |
242 ec_state_string(fsm->requested_state, state_str); |
278 ec_state_string(fsm->requested_state, state_str); |
243 fsm->state = ec_fsm_change_error; |
279 fsm->state = ec_fsm_change_state_error; |
244 EC_ERR("Timeout while setting state %s on slave %i.\n", |
280 EC_ERR("Timeout while setting state %s on slave %i.\n", |
245 state_str, slave->ring_position); |
281 state_str, slave->ring_position); |
246 return; |
282 return; |
247 } |
283 } |
248 |
284 |
249 // still old state: check again |
285 // check again |
250 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
286 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
251 ec_master_queue_datagram(fsm->slave->master, datagram); |
287 ec_master_queue_datagram(fsm->slave->master, datagram); |
252 } |
288 } |
253 |
289 |
254 /*****************************************************************************/ |
290 /*****************************************************************************/ |
320 EC_ERR("Unknown AL status code 0x%04X.\n", code); |
356 EC_ERR("Unknown AL status code 0x%04X.\n", code); |
321 } |
357 } |
322 } |
358 } |
323 |
359 |
324 // acknowledge "old" slave state |
360 // acknowledge "old" slave state |
|
361 ec_fsm_change_state_start_ack(fsm); // execute immediately |
|
362 } |
|
363 |
|
364 /*****************************************************************************/ |
|
365 |
|
366 /** |
|
367 Change state: START ACK. |
|
368 */ |
|
369 |
|
370 void ec_fsm_change_state_start_ack(ec_fsm_change_t *fsm |
|
371 /**< finite state machine */) |
|
372 { |
|
373 ec_slave_t *slave = fsm->slave; |
|
374 ec_datagram_t *datagram = fsm->datagram; |
|
375 |
325 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
376 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
326 EC_WRITE_U16(datagram->data, slave->current_state); |
377 EC_WRITE_U16(datagram->data, slave->current_state); |
327 ec_master_queue_datagram(fsm->slave->master, datagram); |
378 ec_master_queue_datagram(fsm->slave->master, datagram); |
328 fsm->state = ec_fsm_change_ack; |
379 fsm->state = ec_fsm_change_state_ack; |
329 } |
380 } |
330 |
381 |
331 /*****************************************************************************/ |
382 /*****************************************************************************/ |
332 |
383 |
333 /** |
384 /** |
334 Change state: ACK. |
385 Change state: ACK. |
335 */ |
386 */ |
336 |
387 |
337 void ec_fsm_change_ack(ec_fsm_change_t *fsm /**< finite state machine */) |
388 void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */) |
338 { |
389 { |
339 ec_datagram_t *datagram = fsm->datagram; |
390 ec_datagram_t *datagram = fsm->datagram; |
340 ec_slave_t *slave = fsm->slave; |
391 ec_slave_t *slave = fsm->slave; |
341 |
392 |
342 if (datagram->state != EC_DATAGRAM_RECEIVED |
393 if (datagram->state != EC_DATAGRAM_RECEIVED |
343 || datagram->working_counter != 1) { |
394 || datagram->working_counter != 1) { |
344 fsm->state = ec_fsm_change_error; |
395 fsm->state = ec_fsm_change_state_error; |
345 EC_ERR("Reception of state ack datagram failed.\n"); |
396 EC_ERR("Reception of state ack datagram failed.\n"); |
346 return; |
397 return; |
347 } |
398 } |
348 |
399 |
349 fsm->take_time = 1; |
400 fsm->take_time = 1; |
350 |
401 |
351 // read new AL status |
402 // read new AL status |
352 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
403 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
353 ec_master_queue_datagram(fsm->slave->master, datagram); |
404 ec_master_queue_datagram(fsm->slave->master, datagram); |
354 fsm->state = ec_fsm_change_check_ack; |
405 fsm->state = ec_fsm_change_state_check_ack; |
355 } |
406 } |
356 |
407 |
357 /*****************************************************************************/ |
408 /*****************************************************************************/ |
358 |
409 |
359 /** |
410 /** |
360 Change state: CHECK ACK. |
411 Change state: CHECK ACK. |
361 */ |
412 */ |
362 |
413 |
363 void ec_fsm_change_check_ack(ec_fsm_change_t *fsm /**< finite state machine */) |
414 void ec_fsm_change_state_check_ack(ec_fsm_change_t *fsm |
|
415 /**< finite state machine */) |
364 { |
416 { |
365 ec_datagram_t *datagram = fsm->datagram; |
417 ec_datagram_t *datagram = fsm->datagram; |
366 ec_slave_t *slave = fsm->slave; |
418 ec_slave_t *slave = fsm->slave; |
367 |
419 |
368 if (datagram->state != EC_DATAGRAM_RECEIVED |
420 if (datagram->state != EC_DATAGRAM_RECEIVED |
369 || datagram->working_counter != 1) { |
421 || datagram->working_counter != 1) { |
370 fsm->state = ec_fsm_change_error; |
422 fsm->state = ec_fsm_change_state_error; |
371 EC_ERR("Reception of state ack check datagram failed.\n"); |
423 EC_ERR("Reception of state ack check datagram failed.\n"); |
372 return; |
424 return; |
373 } |
425 } |
374 |
426 |
375 if (fsm->take_time) { |
427 if (fsm->take_time) { |
380 slave->current_state = EC_READ_U8(datagram->data); |
432 slave->current_state = EC_READ_U8(datagram->data); |
381 |
433 |
382 if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) { |
434 if (!(slave->current_state & EC_SLAVE_STATE_ACK_ERR)) { |
383 char state_str[EC_STATE_STRING_SIZE]; |
435 char state_str[EC_STATE_STRING_SIZE]; |
384 ec_state_string(slave->current_state, state_str); |
436 ec_state_string(slave->current_state, state_str); |
385 fsm->state = ec_fsm_change_error; |
437 if (fsm->mode == EC_FSM_CHANGE_MODE_FULL) { |
|
438 fsm->state = ec_fsm_change_state_error; |
|
439 } |
|
440 else { // EC_FSM_CHANGE_MODE_ACK_ONLY |
|
441 fsm->state = ec_fsm_change_state_end; |
|
442 } |
386 EC_INFO("Acknowledged state %s on slave %i.\n", |
443 EC_INFO("Acknowledged state %s on slave %i.\n", |
387 state_str, slave->ring_position); |
444 state_str, slave->ring_position); |
388 return; |
445 return; |
389 } |
446 } |
390 |
447 |
391 if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s |
448 if (datagram->jiffies_received - fsm->jiffies_start >= HZ) { // 1s |
392 // timeout while checking |
449 // timeout while checking |
393 char state_str[EC_STATE_STRING_SIZE]; |
450 char state_str[EC_STATE_STRING_SIZE]; |
394 ec_state_string(slave->current_state, state_str); |
451 ec_state_string(slave->current_state, state_str); |
395 fsm->state = ec_fsm_change_error; |
452 fsm->state = ec_fsm_change_state_error; |
396 EC_ERR("Timeout while acknowledging state %s on slave %i.\n", |
453 EC_ERR("Timeout while acknowledging state %s on slave %i.\n", |
397 state_str, slave->ring_position); |
454 state_str, slave->ring_position); |
398 return; |
455 return; |
399 } |
456 } |
400 |
457 |