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" |
182 EC_ERR("Failed to receive state datagram from slave %i" |
186 " (datagram state %i)!\n", |
183 " (datagram state %i)!\n", |
204 } |
201 } |
205 |
202 |
206 // repeat writing new state to slave |
203 // repeat writing new state to slave |
207 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
204 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
208 EC_WRITE_U16(datagram->data, fsm->requested_state); |
205 EC_WRITE_U16(datagram->data, fsm->requested_state); |
209 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
210 fsm->retries = EC_FSM_RETRIES; |
206 fsm->retries = EC_FSM_RETRIES; |
211 return; |
207 return; |
212 } |
208 } |
213 |
209 |
214 fsm->take_time = 1; |
210 fsm->take_time = 1; |
215 |
211 |
216 // read AL status from slave |
212 // read AL status from slave |
217 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
213 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
218 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
219 fsm->retries = EC_FSM_RETRIES; |
214 fsm->retries = EC_FSM_RETRIES; |
220 fsm->state = ec_fsm_change_state_status; |
215 fsm->state = ec_fsm_change_state_status; |
221 } |
216 } |
222 |
217 |
223 /*****************************************************************************/ |
218 /*****************************************************************************/ |
230 /**< finite state machine */) |
225 /**< finite state machine */) |
231 { |
226 { |
232 ec_datagram_t *datagram = fsm->datagram; |
227 ec_datagram_t *datagram = fsm->datagram; |
233 ec_slave_t *slave = fsm->slave; |
228 ec_slave_t *slave = fsm->slave; |
234 |
229 |
235 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
230 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
236 ec_master_queue_datagram(fsm->slave->master, datagram); |
231 return; |
237 return; |
|
238 } |
|
239 |
232 |
240 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
233 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
241 fsm->state = ec_fsm_change_state_error; |
234 fsm->state = ec_fsm_change_state_error; |
242 EC_ERR("Failed to receive state checking datagram from slave %i" |
235 EC_ERR("Failed to receive state checking datagram from slave %i" |
243 " (datagram state %i).\n", |
236 " (datagram state %i).\n", |
289 |
282 |
290 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", |
291 req_state, slave->ring_position, cur_state); |
284 req_state, slave->ring_position, cur_state); |
292 // fetch AL status error code |
285 // fetch AL status error code |
293 ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); |
286 ec_datagram_nprd(datagram, slave->station_address, 0x0134, 2); |
294 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
295 fsm->retries = EC_FSM_RETRIES; |
287 fsm->retries = EC_FSM_RETRIES; |
296 fsm->state = ec_fsm_change_state_code; |
288 fsm->state = ec_fsm_change_state_code; |
297 return; |
289 return; |
298 } |
290 } |
299 |
291 |
310 } |
302 } |
311 |
303 |
312 again: |
304 again: |
313 // no timeout yet. check again |
305 // no timeout yet. check again |
314 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
306 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
315 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
316 fsm->retries = EC_FSM_RETRIES; |
307 fsm->retries = EC_FSM_RETRIES; |
317 } |
308 } |
318 |
309 |
319 /*****************************************************************************/ |
310 /*****************************************************************************/ |
320 |
311 |
367 { |
358 { |
368 ec_datagram_t *datagram = fsm->datagram; |
359 ec_datagram_t *datagram = fsm->datagram; |
369 uint32_t code; |
360 uint32_t code; |
370 const ec_code_msg_t *al_msg; |
361 const ec_code_msg_t *al_msg; |
371 |
362 |
372 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
363 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
373 ec_master_queue_datagram(fsm->slave->master, datagram); |
364 return; |
374 return; |
|
375 } |
|
376 |
365 |
377 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
366 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
378 fsm->state = ec_fsm_change_state_error; |
367 fsm->state = ec_fsm_change_state_error; |
379 EC_ERR("Failed to receive AL status code datagram from slave %i" |
368 EC_ERR("Failed to receive AL status code datagram from slave %i" |
380 " (datagram state %i).\n", |
369 " (datagram state %i).\n", |
414 ec_slave_t *slave = fsm->slave; |
403 ec_slave_t *slave = fsm->slave; |
415 ec_datagram_t *datagram = fsm->datagram; |
404 ec_datagram_t *datagram = fsm->datagram; |
416 |
405 |
417 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
406 ec_datagram_npwr(datagram, slave->station_address, 0x0120, 2); |
418 EC_WRITE_U16(datagram->data, slave->current_state); |
407 EC_WRITE_U16(datagram->data, slave->current_state); |
419 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
420 fsm->retries = EC_FSM_RETRIES; |
408 fsm->retries = EC_FSM_RETRIES; |
421 fsm->state = ec_fsm_change_state_ack; |
409 fsm->state = ec_fsm_change_state_ack; |
422 } |
410 } |
423 |
411 |
424 /*****************************************************************************/ |
412 /*****************************************************************************/ |
430 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 */) |
431 { |
419 { |
432 ec_datagram_t *datagram = fsm->datagram; |
420 ec_datagram_t *datagram = fsm->datagram; |
433 ec_slave_t *slave = fsm->slave; |
421 ec_slave_t *slave = fsm->slave; |
434 |
422 |
435 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
423 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
436 ec_master_queue_datagram(fsm->slave->master, datagram); |
424 return; |
437 return; |
|
438 } |
|
439 |
425 |
440 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
426 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
441 fsm->state = ec_fsm_change_state_error; |
427 fsm->state = ec_fsm_change_state_error; |
442 EC_ERR("Failed to receive state ack datagram for slave %i" |
428 EC_ERR("Failed to receive state ack datagram for slave %i" |
443 " (datagram state %i).\n", |
429 " (datagram state %i).\n", |
454 |
440 |
455 fsm->take_time = 1; |
441 fsm->take_time = 1; |
456 |
442 |
457 // read new AL status |
443 // read new AL status |
458 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
444 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
459 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
460 fsm->retries = EC_FSM_RETRIES; |
445 fsm->retries = EC_FSM_RETRIES; |
461 fsm->state = ec_fsm_change_state_check_ack; |
446 fsm->state = ec_fsm_change_state_check_ack; |
462 } |
447 } |
463 |
448 |
464 /*****************************************************************************/ |
449 /*****************************************************************************/ |
471 /**< finite state machine */) |
456 /**< finite state machine */) |
472 { |
457 { |
473 ec_datagram_t *datagram = fsm->datagram; |
458 ec_datagram_t *datagram = fsm->datagram; |
474 ec_slave_t *slave = fsm->slave; |
459 ec_slave_t *slave = fsm->slave; |
475 |
460 |
476 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
461 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
477 ec_master_queue_datagram(fsm->slave->master, datagram); |
462 return; |
478 return; |
|
479 } |
|
480 |
463 |
481 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
464 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
482 fsm->state = ec_fsm_change_state_error; |
465 fsm->state = ec_fsm_change_state_error; |
483 EC_ERR("Failed to receive state ack check datagram from slave %i" |
466 EC_ERR("Failed to receive state ack check datagram from slave %i" |
484 " (datagram state %i).\n", |
467 " (datagram state %i).\n", |
524 return; |
507 return; |
525 } |
508 } |
526 |
509 |
527 // reread new AL status |
510 // reread new AL status |
528 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
511 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
529 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
530 fsm->retries = EC_FSM_RETRIES; |
512 fsm->retries = EC_FSM_RETRIES; |
531 } |
513 } |
532 |
514 |
533 /*****************************************************************************/ |
515 /*****************************************************************************/ |
534 |
516 |