156 fsm->old_state = fsm->slave->current_state; |
156 fsm->old_state = fsm->slave->current_state; |
157 |
157 |
158 // write new state to slave |
158 // write new state to slave |
159 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
159 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
160 EC_WRITE_U16(datagram->data, fsm->requested_state); |
160 EC_WRITE_U16(datagram->data, fsm->requested_state); |
161 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
162 fsm->retries = EC_FSM_RETRIES; |
161 fsm->retries = EC_FSM_RETRIES; |
163 fsm->state = ec_fsm_change_state_check; |
162 fsm->state = ec_fsm_change_state_check; |
164 } |
163 } |
165 |
164 |
166 /*****************************************************************************/ |
165 /*****************************************************************************/ |
173 /**< finite state machine */) |
172 /**< finite state machine */) |
174 { |
173 { |
175 ec_datagram_t *datagram = fsm->datagram; |
174 ec_datagram_t *datagram = fsm->datagram; |
176 ec_slave_t *slave = fsm->slave; |
175 ec_slave_t *slave = fsm->slave; |
177 |
176 |
178 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
177 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
179 ec_master_queue_datagram(fsm->slave->master, datagram); |
178 return; |
180 return; |
|
181 } |
|
182 |
179 |
183 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
180 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
184 fsm->state = ec_fsm_change_state_error; |
181 fsm->state = ec_fsm_change_state_error; |
185 EC_ERR("Failed to receive state datagram from slave %i!\n", |
182 EC_ERR("Failed to receive state datagram from slave %i" |
186 fsm->slave->ring_position); |
183 " (datagram state %i)!\n", |
|
184 fsm->slave->ring_position, datagram->state); |
187 return; |
185 return; |
188 } |
186 } |
189 |
187 |
190 if (fsm->take_time) { |
188 if (fsm->take_time) { |
191 fsm->take_time = 0; |
189 fsm->take_time = 0; |
203 } |
201 } |
204 |
202 |
205 // repeat writing new state to slave |
203 // repeat writing new state to slave |
206 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
204 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
207 EC_WRITE_U16(datagram->data, fsm->requested_state); |
205 EC_WRITE_U16(datagram->data, fsm->requested_state); |
208 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
209 fsm->retries = EC_FSM_RETRIES; |
206 fsm->retries = EC_FSM_RETRIES; |
210 return; |
207 return; |
211 } |
208 } |
212 |
209 |
213 fsm->take_time = 1; |
210 fsm->take_time = 1; |
214 |
211 |
215 // read AL status from slave |
212 // read AL status from slave |
216 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
213 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
217 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
218 fsm->retries = EC_FSM_RETRIES; |
214 fsm->retries = EC_FSM_RETRIES; |
219 fsm->state = ec_fsm_change_state_status; |
215 fsm->state = ec_fsm_change_state_status; |
220 } |
216 } |
221 |
217 |
222 /*****************************************************************************/ |
218 /*****************************************************************************/ |
229 /**< finite state machine */) |
225 /**< finite state machine */) |
230 { |
226 { |
231 ec_datagram_t *datagram = fsm->datagram; |
227 ec_datagram_t *datagram = fsm->datagram; |
232 ec_slave_t *slave = fsm->slave; |
228 ec_slave_t *slave = fsm->slave; |
233 |
229 |
234 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
230 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
235 ec_master_queue_datagram(fsm->slave->master, datagram); |
231 return; |
236 return; |
|
237 } |
|
238 |
232 |
239 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
233 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
240 fsm->state = ec_fsm_change_state_error; |
234 fsm->state = ec_fsm_change_state_error; |
241 EC_ERR("Failed to receive state checking datagram from slave %i.\n", |
235 EC_ERR("Failed to receive state checking datagram from slave %i" |
242 slave->ring_position); |
236 " (datagram state %i).\n", |
|
237 slave->ring_position, datagram->state); |
243 return; |
238 return; |
244 } |
239 } |
245 |
240 |
246 if (datagram->working_counter != 1) { |
241 if (datagram->working_counter != 1) { |
247 char req_state[EC_STATE_STRING_SIZE]; |
242 char req_state[EC_STATE_STRING_SIZE]; |
287 |
282 |
288 EC_ERR("Failed to set %s state, slave %i refused state change (%s).\n", |
283 EC_ERR("Failed to set %s state, slave %i refused state change (%s).\n", |
289 req_state, slave->ring_position, cur_state); |
284 req_state, slave->ring_position, cur_state); |
290 // fetch AL status error code |
285 // fetch AL status error code |
291 ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); |
286 ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); |
292 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
293 fsm->retries = EC_FSM_RETRIES; |
287 fsm->retries = EC_FSM_RETRIES; |
294 fsm->state = ec_fsm_change_state_code; |
288 fsm->state = ec_fsm_change_state_code; |
295 return; |
289 return; |
296 } |
290 } |
297 |
291 |
308 } |
302 } |
309 |
303 |
310 again: |
304 again: |
311 // no timeout yet. check again |
305 // no timeout yet. check again |
312 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
306 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
313 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
314 fsm->retries = EC_FSM_RETRIES; |
307 fsm->retries = EC_FSM_RETRIES; |
315 } |
308 } |
316 |
309 |
317 /*****************************************************************************/ |
310 /*****************************************************************************/ |
318 |
311 |
365 { |
358 { |
366 ec_datagram_t *datagram = fsm->datagram; |
359 ec_datagram_t *datagram = fsm->datagram; |
367 uint32_t code; |
360 uint32_t code; |
368 const ec_code_msg_t *al_msg; |
361 const ec_code_msg_t *al_msg; |
369 |
362 |
370 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
363 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
371 ec_master_queue_datagram(fsm->slave->master, datagram); |
364 return; |
372 return; |
|
373 } |
|
374 |
365 |
375 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
366 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
376 fsm->state = ec_fsm_change_state_error; |
367 fsm->state = ec_fsm_change_state_error; |
377 EC_ERR("Failed to receive AL status code datagram from slave %i.\n", |
368 EC_ERR("Failed to receive AL status code datagram from slave %i" |
378 fsm->slave->ring_position); |
369 " (datagram state %i).\n", |
|
370 fsm->slave->ring_position, datagram->state); |
379 return; |
371 return; |
380 } |
372 } |
381 |
373 |
382 if (datagram->working_counter != 1) { |
374 if (datagram->working_counter != 1) { |
383 EC_WARN("Reception of AL status code datagram failed.\n"); |
375 EC_WARN("Reception of AL status code datagram failed.\n"); |
411 ec_slave_t *slave = fsm->slave; |
403 ec_slave_t *slave = fsm->slave; |
412 ec_datagram_t *datagram = fsm->datagram; |
404 ec_datagram_t *datagram = fsm->datagram; |
413 |
405 |
414 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
406 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
415 EC_WRITE_U16(datagram->data, slave->current_state); |
407 EC_WRITE_U16(datagram->data, slave->current_state); |
416 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
417 fsm->retries = EC_FSM_RETRIES; |
408 fsm->retries = EC_FSM_RETRIES; |
418 fsm->state = ec_fsm_change_state_ack; |
409 fsm->state = ec_fsm_change_state_ack; |
419 } |
410 } |
420 |
411 |
421 /*****************************************************************************/ |
412 /*****************************************************************************/ |
427 void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */) |
418 void ec_fsm_change_state_ack(ec_fsm_change_t *fsm /**< finite state machine */) |
428 { |
419 { |
429 ec_datagram_t *datagram = fsm->datagram; |
420 ec_datagram_t *datagram = fsm->datagram; |
430 ec_slave_t *slave = fsm->slave; |
421 ec_slave_t *slave = fsm->slave; |
431 |
422 |
432 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
423 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
433 ec_master_queue_datagram(fsm->slave->master, datagram); |
424 return; |
434 return; |
|
435 } |
|
436 |
425 |
437 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
426 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
438 fsm->state = ec_fsm_change_state_error; |
427 fsm->state = ec_fsm_change_state_error; |
439 EC_ERR("Failed to receive state ack datagram for slave %i.\n", |
428 EC_ERR("Failed to receive state ack datagram for slave %i" |
440 slave->ring_position); |
429 " (datagram state %i).\n", |
|
430 slave->ring_position, datagram->state); |
441 return; |
431 return; |
442 } |
432 } |
443 |
433 |
444 if (datagram->working_counter != 1) { |
434 if (datagram->working_counter != 1) { |
445 fsm->state = ec_fsm_change_state_error; |
435 fsm->state = ec_fsm_change_state_error; |
450 |
440 |
451 fsm->take_time = 1; |
441 fsm->take_time = 1; |
452 |
442 |
453 // read new AL status |
443 // read new AL status |
454 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
444 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
455 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
456 fsm->retries = EC_FSM_RETRIES; |
445 fsm->retries = EC_FSM_RETRIES; |
457 fsm->state = ec_fsm_change_state_check_ack; |
446 fsm->state = ec_fsm_change_state_check_ack; |
458 } |
447 } |
459 |
448 |
460 /*****************************************************************************/ |
449 /*****************************************************************************/ |
467 /**< finite state machine */) |
456 /**< finite state machine */) |
468 { |
457 { |
469 ec_datagram_t *datagram = fsm->datagram; |
458 ec_datagram_t *datagram = fsm->datagram; |
470 ec_slave_t *slave = fsm->slave; |
459 ec_slave_t *slave = fsm->slave; |
471 |
460 |
472 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
461 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
473 ec_master_queue_datagram(fsm->slave->master, datagram); |
462 return; |
474 return; |
|
475 } |
|
476 |
463 |
477 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
464 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
478 fsm->state = ec_fsm_change_state_error; |
465 fsm->state = ec_fsm_change_state_error; |
479 EC_ERR("Failed to receive state ack check datagram from slave %i.\n", |
466 EC_ERR("Failed to receive state ack check datagram from slave %i" |
480 slave->ring_position); |
467 " (datagram state %i).\n", |
|
468 slave->ring_position, datagram->state); |
481 return; |
469 return; |
482 } |
470 } |
483 |
471 |
484 if (datagram->working_counter != 1) { |
472 if (datagram->working_counter != 1) { |
485 fsm->state = ec_fsm_change_state_error; |
473 fsm->state = ec_fsm_change_state_error; |
519 return; |
507 return; |
520 } |
508 } |
521 |
509 |
522 // reread new AL status |
510 // reread new AL status |
523 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
511 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
524 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
525 fsm->retries = EC_FSM_RETRIES; |
512 fsm->retries = EC_FSM_RETRIES; |
526 } |
513 } |
527 |
514 |
528 /*****************************************************************************/ |
515 /*****************************************************************************/ |
529 |
516 |