equal
deleted
inserted
replaced
56 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
56 void ec_fsm_slave_config_state_pdo_conf(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
57 void ec_fsm_slave_config_state_fmmu(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
58 void ec_fsm_slave_config_state_safeop(ec_fsm_slave_config_t *); |
59 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
59 void ec_fsm_slave_config_state_op(ec_fsm_slave_config_t *); |
60 |
60 |
|
61 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
62 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
62 void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *); |
63 void ec_fsm_slave_config_enter_preop(ec_fsm_slave_config_t *); |
63 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
64 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_enter_pdo_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_enter_safeop(ec_fsm_slave_config_t *); |
68 |
69 |
69 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_state_end(ec_fsm_slave_config_t *); |
70 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *); |
71 void ec_fsm_slave_config_state_error(ec_fsm_slave_config_t *); |
71 |
72 |
|
73 void ec_fsm_slave_config_reconfigure(ec_fsm_slave_config_t *); |
|
74 |
72 /*****************************************************************************/ |
75 /*****************************************************************************/ |
73 |
76 |
74 /** Constructor. |
77 /** Constructor. |
75 */ |
78 */ |
76 void ec_fsm_slave_config_init( |
79 void ec_fsm_slave_config_init( |
79 ec_fsm_change_t *fsm_change, /**< State change state machine to use. */ |
82 ec_fsm_change_t *fsm_change, /**< State change state machine to use. */ |
80 ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */ |
83 ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */ |
81 ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration state machine to use. */ |
84 ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration state machine to use. */ |
82 ) |
85 ) |
83 { |
86 { |
|
87 ec_sdo_request_init(&fsm->request_copy); |
|
88 |
84 fsm->datagram = datagram; |
89 fsm->datagram = datagram; |
85 fsm->fsm_change = fsm_change; |
90 fsm->fsm_change = fsm_change; |
86 fsm->fsm_coe = fsm_coe; |
91 fsm->fsm_coe = fsm_coe; |
87 fsm->fsm_pdo = fsm_pdo; |
92 fsm->fsm_pdo = fsm_pdo; |
88 } |
93 } |
93 */ |
98 */ |
94 void ec_fsm_slave_config_clear( |
99 void ec_fsm_slave_config_clear( |
95 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
100 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
96 ) |
101 ) |
97 { |
102 { |
|
103 ec_sdo_request_clear(&fsm->request_copy); |
98 } |
104 } |
99 |
105 |
100 /*****************************************************************************/ |
106 /*****************************************************************************/ |
101 |
107 |
102 /** Start slave configuration state machine. |
108 /** Start slave configuration state machine. |
170 { |
176 { |
171 if (fsm->slave->master->debug_level) { |
177 if (fsm->slave->master->debug_level) { |
172 EC_DBG("Configuring slave %u...\n", fsm->slave->ring_position); |
178 EC_DBG("Configuring slave %u...\n", fsm->slave->ring_position); |
173 } |
179 } |
174 |
180 |
|
181 ec_fsm_slave_config_enter_init(fsm); |
|
182 } |
|
183 |
|
184 /*****************************************************************************/ |
|
185 |
|
186 /** Start state change to INIT. |
|
187 */ |
|
188 void ec_fsm_slave_config_enter_init( |
|
189 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
190 ) |
|
191 { |
175 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); |
192 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); |
176 ec_fsm_change_exec(fsm->fsm_change); |
193 ec_fsm_change_exec(fsm->fsm_change); |
177 fsm->state = ec_fsm_slave_config_state_init; |
194 fsm->state = ec_fsm_slave_config_state_init; |
178 } |
195 } |
179 |
196 |
442 |
459 |
443 // start Sdo configuration |
460 // start Sdo configuration |
444 fsm->state = ec_fsm_slave_config_state_sdo_conf; |
461 fsm->state = ec_fsm_slave_config_state_sdo_conf; |
445 fsm->request = list_entry(fsm->slave->config->sdo_configs.next, |
462 fsm->request = list_entry(fsm->slave->config->sdo_configs.next, |
446 ec_sdo_request_t, list); |
463 ec_sdo_request_t, list); |
447 ecrt_sdo_request_write(fsm->request); |
464 ec_sdo_request_copy(&fsm->request_copy, fsm->request); |
448 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, fsm->request); |
465 ecrt_sdo_request_write(&fsm->request_copy); |
|
466 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); |
449 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
467 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
450 } |
468 } |
451 |
469 |
452 /*****************************************************************************/ |
470 /*****************************************************************************/ |
453 |
471 |
465 fsm->slave->error_flag = 1; |
483 fsm->slave->error_flag = 1; |
466 fsm->state = ec_fsm_slave_config_state_error; |
484 fsm->state = ec_fsm_slave_config_state_error; |
467 return; |
485 return; |
468 } |
486 } |
469 |
487 |
|
488 if (!fsm->slave->config) { // config removed in the meantime |
|
489 ec_fsm_slave_config_reconfigure(fsm); |
|
490 return; |
|
491 } |
|
492 |
470 // Another Sdo to configure? |
493 // Another Sdo to configure? |
471 if (fsm->request->list.next != &fsm->slave->config->sdo_configs) { |
494 if (fsm->request->list.next != &fsm->slave->config->sdo_configs) { |
472 fsm->request = list_entry(fsm->request->list.next, ec_sdo_request_t, |
495 fsm->request = list_entry(fsm->request->list.next, |
473 list); |
496 ec_sdo_request_t, list); |
474 ecrt_sdo_request_write(fsm->request); |
497 ec_sdo_request_copy(&fsm->request_copy, fsm->request); |
475 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, fsm->request); |
498 ecrt_sdo_request_write(&fsm->request_copy); |
|
499 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request_copy); |
476 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
500 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
477 return; |
501 return; |
478 } |
502 } |
479 |
503 |
480 // All Sdos are now configured. |
504 // All Sdos are now configured. |
503 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
527 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
504 ) |
528 ) |
505 { |
529 { |
506 if (ec_fsm_pdo_exec(fsm->fsm_pdo)) |
530 if (ec_fsm_pdo_exec(fsm->fsm_pdo)) |
507 return; |
531 return; |
|
532 |
|
533 if (!fsm->slave->config) { // config removed in the meantime |
|
534 ec_fsm_slave_config_reconfigure(fsm); |
|
535 return; |
|
536 } |
508 |
537 |
509 if (!ec_fsm_pdo_success(fsm->fsm_pdo)) { |
538 if (!ec_fsm_pdo_success(fsm->fsm_pdo)) { |
510 EC_WARN("Pdo configuration failed on slave %u.\n", |
539 EC_WARN("Pdo configuration failed on slave %u.\n", |
511 fsm->slave->ring_position); |
540 fsm->slave->ring_position); |
512 } |
541 } |
589 slave->error_flag = 1; |
618 slave->error_flag = 1; |
590 fsm->state = ec_fsm_slave_config_state_error; |
619 fsm->state = ec_fsm_slave_config_state_error; |
591 EC_ERR("Failed to set process data sync managers of slave %u: ", |
620 EC_ERR("Failed to set process data sync managers of slave %u: ", |
592 slave->ring_position); |
621 slave->ring_position); |
593 ec_datagram_print_wc_error(datagram); |
622 ec_datagram_print_wc_error(datagram); |
|
623 return; |
|
624 } |
|
625 |
|
626 if (!fsm->slave->config) { // config removed in the meantime |
|
627 ec_fsm_slave_config_reconfigure(fsm); |
594 return; |
628 return; |
595 } |
629 } |
596 |
630 |
597 ec_fsm_slave_config_enter_fmmu(fsm); |
631 ec_fsm_slave_config_enter_fmmu(fsm); |
598 } |
632 } |
762 } |
796 } |
763 |
797 |
764 fsm->state = ec_fsm_slave_config_state_end; // successful |
798 fsm->state = ec_fsm_slave_config_state_end; // successful |
765 } |
799 } |
766 |
800 |
|
801 /*****************************************************************************/ |
|
802 |
|
803 /** Reconfigure the slave starting at INIT. |
|
804 */ |
|
805 void ec_fsm_slave_config_reconfigure( |
|
806 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
807 ) |
|
808 { |
|
809 if (fsm->slave->master->debug_level) { |
|
810 EC_DBG("Slave configuration for slave %u detached during " |
|
811 "configuration. Reconfiguring.", fsm->slave->ring_position); |
|
812 } |
|
813 |
|
814 ec_fsm_slave_config_enter_init(fsm); // reconfigure |
|
815 } |
|
816 |
767 /****************************************************************************** |
817 /****************************************************************************** |
768 * Common state functions |
818 * Common state functions |
769 *****************************************************************************/ |
819 *****************************************************************************/ |
770 |
820 |
771 /** State: ERROR. |
821 /** State: ERROR. |