51 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
51 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
52 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
53 void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *); |
53 void ec_fsm_slave_config_state_preop(ec_fsm_slave_config_t *); |
54 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
54 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
55 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
56 void ec_fsm_slave_config_state_pdo_assign(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_pdo_mapping(ec_fsm_slave_config_t *); |
|
58 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 *); |
59 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 *); |
60 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 *); |
61 |
60 |
62 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
61 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
73 |
72 |
74 /** Constructor. |
73 /** Constructor. |
75 */ |
74 */ |
76 void ec_fsm_slave_config_init( |
75 void ec_fsm_slave_config_init( |
77 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
76 ec_fsm_slave_config_t *fsm, /**< slave state machine */ |
78 ec_datagram_t *datagram /**< datagram structure to use */ |
77 ec_datagram_t *datagram, /**< datagram structure to use */ |
|
78 ec_fsm_change_t *fsm_change, /**< State change state machine to use. */ |
|
79 ec_fsm_coe_t *fsm_coe, /**< CoE state machine to use. */ |
|
80 ec_fsm_pdo_t *fsm_pdo /**< Pdo configuration state machine to use. */ |
79 ) |
81 ) |
80 { |
82 { |
81 fsm->datagram = datagram; |
83 fsm->datagram = datagram; |
82 |
84 fsm->fsm_change = fsm_change; |
83 // init sub state machines |
85 fsm->fsm_coe = fsm_coe; |
84 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
86 fsm->fsm_pdo = fsm_pdo; |
85 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
|
86 ec_fsm_pdo_assign_init(&fsm->fsm_pdo_assign, &fsm->fsm_coe); |
|
87 ec_fsm_pdo_mapping_init(&fsm->fsm_pdo_mapping, &fsm->fsm_coe); |
|
88 } |
87 } |
89 |
88 |
90 /*****************************************************************************/ |
89 /*****************************************************************************/ |
91 |
90 |
92 /** Destructor. |
91 /** Destructor. |
93 */ |
92 */ |
94 void ec_fsm_slave_config_clear( |
93 void ec_fsm_slave_config_clear( |
95 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
94 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
96 ) |
95 ) |
97 { |
96 { |
98 // clear sub state machines |
|
99 ec_fsm_change_clear(&fsm->fsm_change); |
|
100 ec_fsm_coe_clear(&fsm->fsm_coe); |
|
101 ec_fsm_pdo_assign_clear(&fsm->fsm_pdo_assign); |
|
102 ec_fsm_pdo_mapping_clear(&fsm->fsm_pdo_mapping); |
|
103 } |
97 } |
104 |
98 |
105 /*****************************************************************************/ |
99 /*****************************************************************************/ |
106 |
100 |
107 /** Start slave configuration state machine. |
101 /** Start slave configuration state machine. |
179 |
173 |
180 // configuration will be done immediately; therefore reset the |
174 // configuration will be done immediately; therefore reset the |
181 // force flag |
175 // force flag |
182 fsm->slave->force_config = 0; |
176 fsm->slave->force_config = 0; |
183 |
177 |
184 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); |
178 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_INIT); |
185 ec_fsm_change_exec(&fsm->fsm_change); |
179 ec_fsm_change_exec(fsm->fsm_change); |
186 fsm->state = ec_fsm_slave_config_state_init; |
180 fsm->state = ec_fsm_slave_config_state_init; |
187 } |
181 } |
188 |
182 |
189 /*****************************************************************************/ |
183 /*****************************************************************************/ |
190 |
184 |
196 { |
190 { |
197 ec_master_t *master = fsm->slave->master; |
191 ec_master_t *master = fsm->slave->master; |
198 ec_slave_t *slave = fsm->slave; |
192 ec_slave_t *slave = fsm->slave; |
199 ec_datagram_t *datagram = fsm->datagram; |
193 ec_datagram_t *datagram = fsm->datagram; |
200 |
194 |
201 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
195 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
202 |
196 |
203 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
197 if (!ec_fsm_change_success(fsm->fsm_change)) { |
204 if (!fsm->fsm_change.spontaneous_change) |
198 if (!fsm->fsm_change->spontaneous_change) |
205 slave->error_flag = 1; |
199 slave->error_flag = 1; |
206 fsm->state = ec_fsm_slave_config_state_error; |
200 fsm->state = ec_fsm_slave_config_state_error; |
207 return; |
201 return; |
208 } |
202 } |
209 |
203 |
383 void ec_fsm_slave_config_enter_preop( |
377 void ec_fsm_slave_config_enter_preop( |
384 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
378 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
385 ) |
379 ) |
386 { |
380 { |
387 fsm->state = ec_fsm_slave_config_state_preop; |
381 fsm->state = ec_fsm_slave_config_state_preop; |
388 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
382 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
389 ec_fsm_change_exec(&fsm->fsm_change); // execute immediately |
383 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
390 } |
384 } |
391 |
385 |
392 /*****************************************************************************/ |
386 /*****************************************************************************/ |
393 |
387 |
394 /** Slave configuration state: PREOP. |
388 /** Slave configuration state: PREOP. |
398 ) |
392 ) |
399 { |
393 { |
400 ec_slave_t *slave = fsm->slave; |
394 ec_slave_t *slave = fsm->slave; |
401 ec_master_t *master = fsm->slave->master; |
395 ec_master_t *master = fsm->slave->master; |
402 |
396 |
403 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
397 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
404 |
398 |
405 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
399 if (!ec_fsm_change_success(fsm->fsm_change)) { |
406 if (!fsm->fsm_change.spontaneous_change) |
400 if (!fsm->fsm_change->spontaneous_change) |
407 slave->error_flag = 1; |
401 slave->error_flag = 1; |
408 fsm->state = ec_fsm_slave_config_state_error; |
402 fsm->state = ec_fsm_slave_config_state_error; |
409 return; |
403 return; |
410 } |
404 } |
411 |
405 |
453 // start Sdo configuration |
447 // start Sdo configuration |
454 fsm->state = ec_fsm_slave_config_state_sdo_conf; |
448 fsm->state = ec_fsm_slave_config_state_sdo_conf; |
455 fsm->request = list_entry(fsm->slave->config->sdo_configs.next, |
449 fsm->request = list_entry(fsm->slave->config->sdo_configs.next, |
456 ec_sdo_request_t, list); |
450 ec_sdo_request_t, list); |
457 ecrt_sdo_request_write(fsm->request); |
451 ecrt_sdo_request_write(fsm->request); |
458 ec_fsm_coe_transfer(&fsm->fsm_coe, fsm->slave, fsm->request); |
452 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, fsm->request); |
459 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
453 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
460 } |
454 } |
461 |
455 |
462 /*****************************************************************************/ |
456 /*****************************************************************************/ |
463 |
457 |
464 /** Slave configuration state: SDO_CONF. |
458 /** Slave configuration state: SDO_CONF. |
465 */ |
459 */ |
466 void ec_fsm_slave_config_state_sdo_conf( |
460 void ec_fsm_slave_config_state_sdo_conf( |
467 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
461 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
468 ) |
462 ) |
469 { |
463 { |
470 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
464 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
471 |
465 |
472 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
466 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
473 EC_ERR("Sdo configuration failed for slave %u.\n", |
467 EC_ERR("Sdo configuration failed for slave %u.\n", |
474 fsm->slave->ring_position); |
468 fsm->slave->ring_position); |
475 fsm->slave->error_flag = 1; |
469 fsm->slave->error_flag = 1; |
476 fsm->state = ec_fsm_slave_config_state_error; |
470 fsm->state = ec_fsm_slave_config_state_error; |
477 return; |
471 return; |
480 // Another Sdo to configure? |
474 // Another Sdo to configure? |
481 if (fsm->request->list.next != &fsm->slave->config->sdo_configs) { |
475 if (fsm->request->list.next != &fsm->slave->config->sdo_configs) { |
482 fsm->request = list_entry(fsm->request->list.next, ec_sdo_request_t, |
476 fsm->request = list_entry(fsm->request->list.next, ec_sdo_request_t, |
483 list); |
477 list); |
484 ecrt_sdo_request_write(fsm->request); |
478 ecrt_sdo_request_write(fsm->request); |
485 ec_fsm_coe_transfer(&fsm->fsm_coe, fsm->slave, fsm->request); |
479 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, fsm->request); |
486 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
480 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
487 return; |
481 return; |
488 } |
482 } |
489 |
483 |
490 // All Sdos are now configured. |
484 // All Sdos are now configured. |
491 ec_fsm_slave_config_enter_pdo_sync(fsm); |
485 ec_fsm_slave_config_enter_pdo_sync(fsm); |
569 slave->ring_position); |
563 slave->ring_position); |
570 ec_datagram_print_wc_error(datagram); |
564 ec_datagram_print_wc_error(datagram); |
571 return; |
565 return; |
572 } |
566 } |
573 |
567 |
574 // Start configuring Pdo mapping |
568 // Start configuring Pdos |
575 ec_fsm_pdo_mapping_start(&fsm->fsm_pdo_mapping, fsm->slave); |
569 ec_fsm_pdo_start_configuration(fsm->fsm_pdo, fsm->slave); |
576 fsm->state = ec_fsm_slave_config_state_pdo_mapping; |
570 fsm->state = ec_fsm_slave_config_state_pdo_conf; |
577 fsm->state(fsm); // execute immediately |
571 fsm->state(fsm); // execute immediately |
578 } |
572 } |
579 |
573 |
580 /*****************************************************************************/ |
574 /*****************************************************************************/ |
581 |
575 |
582 /** Slave configuration state: PDO_MAPPING. |
576 /** Slave configuration state: PDO_CONF. |
583 */ |
577 */ |
584 void ec_fsm_slave_config_state_pdo_mapping( |
578 void ec_fsm_slave_config_state_pdo_conf( |
585 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
579 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
586 ) |
580 ) |
587 { |
581 { |
588 if (ec_fsm_pdo_mapping_exec(&fsm->fsm_pdo_mapping)) return; |
582 if (ec_fsm_pdo_exec(fsm->fsm_pdo)) |
589 |
583 return; |
590 if (!ec_fsm_pdo_mapping_success(&fsm->fsm_pdo_mapping)) { |
584 |
591 EC_WARN("Configuration of Pdo mappings failed on slave %u.\n", |
585 if (!ec_fsm_pdo_success(fsm->fsm_pdo)) { |
592 fsm->slave->ring_position); |
586 EC_WARN("Pdo configuration failed on slave %u.\n", |
593 } |
|
594 |
|
595 // start applying Pdo assignments |
|
596 ec_fsm_pdo_assign_start(&fsm->fsm_pdo_assign, fsm->slave); |
|
597 fsm->state = ec_fsm_slave_config_state_pdo_assign; |
|
598 fsm->state(fsm); // execute immediately |
|
599 } |
|
600 |
|
601 /*****************************************************************************/ |
|
602 |
|
603 /** Slave configuration state: PDO_ASSIGN. |
|
604 */ |
|
605 void ec_fsm_slave_config_state_pdo_assign( |
|
606 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
607 ) |
|
608 { |
|
609 if (ec_fsm_pdo_assign_exec(&fsm->fsm_pdo_assign)) return; |
|
610 |
|
611 if (!ec_fsm_pdo_assign_success(&fsm->fsm_pdo_assign)) { |
|
612 EC_WARN("Configuration of Pdo assignments failed on slave %u.\n", |
|
613 fsm->slave->ring_position); |
587 fsm->slave->ring_position); |
614 } |
588 } |
615 |
589 |
616 ec_fsm_slave_config_enter_fmmu(fsm); |
590 ec_fsm_slave_config_enter_fmmu(fsm); |
617 } |
591 } |
706 void ec_fsm_slave_config_enter_safeop( |
680 void ec_fsm_slave_config_enter_safeop( |
707 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
681 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
708 ) |
682 ) |
709 { |
683 { |
710 fsm->state = ec_fsm_slave_config_state_safeop; |
684 fsm->state = ec_fsm_slave_config_state_safeop; |
711 ec_fsm_change_start(&fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAFEOP); |
685 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_SAFEOP); |
712 ec_fsm_change_exec(&fsm->fsm_change); // execute immediately |
686 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
713 } |
687 } |
714 |
688 |
715 /*****************************************************************************/ |
689 /*****************************************************************************/ |
716 |
690 |
717 /** Slave configuration state: SAFEOP. |
691 /** Slave configuration state: SAFEOP. |
721 ) |
695 ) |
722 { |
696 { |
723 ec_master_t *master = fsm->slave->master; |
697 ec_master_t *master = fsm->slave->master; |
724 ec_slave_t *slave = fsm->slave; |
698 ec_slave_t *slave = fsm->slave; |
725 |
699 |
726 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
700 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
727 |
701 |
728 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
702 if (!ec_fsm_change_success(fsm->fsm_change)) { |
729 if (!fsm->fsm_change.spontaneous_change) |
703 if (!fsm->fsm_change->spontaneous_change) |
730 fsm->slave->error_flag = 1; |
704 fsm->slave->error_flag = 1; |
731 fsm->state = ec_fsm_slave_config_state_error; |
705 fsm->state = ec_fsm_slave_config_state_error; |
732 return; |
706 return; |
733 } |
707 } |
734 |
708 |
747 return; |
721 return; |
748 } |
722 } |
749 |
723 |
750 // set state to OP |
724 // set state to OP |
751 fsm->state = ec_fsm_slave_config_state_op; |
725 fsm->state = ec_fsm_slave_config_state_op; |
752 ec_fsm_change_start(&fsm->fsm_change, slave, EC_SLAVE_STATE_OP); |
726 ec_fsm_change_start(fsm->fsm_change, slave, EC_SLAVE_STATE_OP); |
753 ec_fsm_change_exec(&fsm->fsm_change); // execute immediately |
727 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
754 } |
728 } |
755 |
729 |
756 /*****************************************************************************/ |
730 /*****************************************************************************/ |
757 |
731 |
758 /** Slave configuration state: OP |
732 /** Slave configuration state: OP |
762 ) |
736 ) |
763 { |
737 { |
764 ec_master_t *master = fsm->slave->master; |
738 ec_master_t *master = fsm->slave->master; |
765 ec_slave_t *slave = fsm->slave; |
739 ec_slave_t *slave = fsm->slave; |
766 |
740 |
767 if (ec_fsm_change_exec(&fsm->fsm_change)) return; |
741 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
768 |
742 |
769 if (!ec_fsm_change_success(&fsm->fsm_change)) { |
743 if (!ec_fsm_change_success(fsm->fsm_change)) { |
770 if (!fsm->fsm_change.spontaneous_change) |
744 if (!fsm->fsm_change->spontaneous_change) |
771 slave->error_flag = 1; |
745 slave->error_flag = 1; |
772 fsm->state = ec_fsm_slave_config_state_error; |
746 fsm->state = ec_fsm_slave_config_state_error; |
773 return; |
747 return; |
774 } |
748 } |
775 |
749 |