60 void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *); |
59 void ec_fsm_slave_conf_state_clear_fmmus(ec_fsm_slave_t *); |
61 void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *); |
60 void ec_fsm_slave_conf_state_mbox_sync(ec_fsm_slave_t *); |
62 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *); |
61 void ec_fsm_slave_conf_state_preop(ec_fsm_slave_t *); |
63 void ec_fsm_slave_conf_state_sdo_conf(ec_fsm_slave_t *); |
62 void ec_fsm_slave_conf_state_sdo_conf(ec_fsm_slave_t *); |
64 void ec_fsm_slave_conf_state_mapping(ec_fsm_slave_t *); |
63 void ec_fsm_slave_conf_state_mapping(ec_fsm_slave_t *); |
|
64 void ec_fsm_slave_conf_state_pdo_conf(ec_fsm_slave_t *); |
65 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *); |
65 void ec_fsm_slave_conf_state_pdo_sync(ec_fsm_slave_t *); |
66 void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *); |
66 void ec_fsm_slave_conf_state_fmmu(ec_fsm_slave_t *); |
67 void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *); |
67 void ec_fsm_slave_conf_state_saveop(ec_fsm_slave_t *); |
68 void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *); |
68 void ec_fsm_slave_conf_state_op(ec_fsm_slave_t *); |
69 |
69 |
70 void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *); |
70 void ec_fsm_slave_conf_enter_mbox_sync(ec_fsm_slave_t *); |
71 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *); |
71 void ec_fsm_slave_conf_enter_preop(ec_fsm_slave_t *); |
|
72 void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *); |
|
73 void ec_fsm_slave_conf_enter_mapping(ec_fsm_slave_t *); |
72 void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *); |
74 void ec_fsm_slave_conf_enter_pdo_sync(ec_fsm_slave_t *); |
73 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *); |
75 void ec_fsm_slave_conf_enter_fmmu(ec_fsm_slave_t *); |
74 void ec_fsm_slave_conf_enter_sdo_conf(ec_fsm_slave_t *); |
|
75 void ec_fsm_slave_conf_enter_mapping(ec_fsm_slave_t *); |
|
76 void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *); |
76 void ec_fsm_slave_conf_enter_saveop(ec_fsm_slave_t *); |
77 |
77 |
78 void ec_fsm_slave_state_end(ec_fsm_slave_t *); |
78 void ec_fsm_slave_state_end(ec_fsm_slave_t *); |
79 void ec_fsm_slave_state_error(ec_fsm_slave_t *); |
79 void ec_fsm_slave_state_error(ec_fsm_slave_t *); |
80 |
80 |
81 /*****************************************************************************/ |
81 /*****************************************************************************/ |
82 |
82 |
83 /** |
83 /** Constructor. |
84 Constructor. |
84 */ |
85 */ |
|
86 |
|
87 void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */ |
85 void ec_fsm_slave_init(ec_fsm_slave_t *fsm, /**< slave state machine */ |
88 ec_datagram_t *datagram /**< datagram structure to use */ |
86 ec_datagram_t *datagram /**< datagram structure to use */ |
89 ) |
87 ) |
90 { |
88 { |
91 fsm->datagram = datagram; |
89 fsm->datagram = datagram; |
93 // init sub state machines |
91 // init sub state machines |
94 ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); |
92 ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); |
95 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
93 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
96 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
94 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
97 ec_fsm_mapping_init(&fsm->fsm_map, &fsm->fsm_coe); |
95 ec_fsm_mapping_init(&fsm->fsm_map, &fsm->fsm_coe); |
98 } |
96 ec_fsm_pdo_config_init(&fsm->fsm_pdo, &fsm->fsm_coe); |
99 |
97 } |
100 /*****************************************************************************/ |
98 |
101 |
99 /*****************************************************************************/ |
102 /** |
100 |
103 Destructor. |
101 /** Destructor. |
104 */ |
102 */ |
105 |
|
106 void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */) |
103 void ec_fsm_slave_clear(ec_fsm_slave_t *fsm /**< slave state machine */) |
107 { |
104 { |
108 // clear sub state machines |
105 // clear sub state machines |
109 ec_fsm_sii_clear(&fsm->fsm_sii); |
106 ec_fsm_sii_clear(&fsm->fsm_sii); |
110 ec_fsm_change_clear(&fsm->fsm_change); |
107 ec_fsm_change_clear(&fsm->fsm_change); |
111 ec_fsm_coe_clear(&fsm->fsm_coe); |
108 ec_fsm_coe_clear(&fsm->fsm_coe); |
112 ec_fsm_mapping_clear(&fsm->fsm_map); |
109 ec_fsm_mapping_clear(&fsm->fsm_map); |
|
110 ec_fsm_pdo_config_clear(&fsm->fsm_pdo); |
113 } |
111 } |
114 |
112 |
115 /*****************************************************************************/ |
113 /*****************************************************************************/ |
116 |
114 |
117 /** |
115 /** |
882 EC_DBG("Slave %u is not configured.\n", slave->ring_position); |
880 EC_DBG("Slave %u is not configured.\n", slave->ring_position); |
883 ec_fsm_slave_conf_enter_saveop(fsm); |
881 ec_fsm_slave_conf_enter_saveop(fsm); |
884 return; |
882 return; |
885 } |
883 } |
886 |
884 |
887 // No CoE configuration to be applied? FIXME |
885 // No CoE configuration to be applied? |
888 if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration |
886 if (list_empty(&slave->config->sdo_configs)) { // skip SDO configuration |
889 ec_fsm_slave_conf_enter_mapping(fsm); |
887 ec_fsm_slave_conf_enter_mapping(fsm); |
890 return; |
888 return; |
891 } |
889 } |
892 |
890 |
939 |
937 |
940 void ec_fsm_slave_conf_enter_mapping( |
938 void ec_fsm_slave_conf_enter_mapping( |
941 ec_fsm_slave_t *fsm /**< slave state machine */ |
939 ec_fsm_slave_t *fsm /**< slave state machine */ |
942 ) |
940 ) |
943 { |
941 { |
944 ec_slave_t *slave = fsm->slave; |
|
945 |
|
946 if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) { |
|
947 // Slave does not support CoE: no configuration of PDO mapping. |
|
948 ec_fsm_slave_conf_enter_pdo_sync(fsm); |
|
949 return; |
|
950 } |
|
951 |
|
952 // start configuring PDO mapping |
942 // start configuring PDO mapping |
953 fsm->state = ec_fsm_slave_conf_state_mapping; |
943 fsm->state = ec_fsm_slave_conf_state_mapping; |
954 ec_fsm_mapping_start(&fsm->fsm_map, fsm->slave); |
944 ec_fsm_mapping_start(&fsm->fsm_map, fsm->slave); |
955 ec_fsm_mapping_exec(&fsm->fsm_map); // execute immediately |
945 ec_fsm_mapping_exec(&fsm->fsm_map); // execute immediately |
956 } |
946 } |
967 { |
957 { |
968 if (ec_fsm_mapping_exec(&fsm->fsm_map)) return; |
958 if (ec_fsm_mapping_exec(&fsm->fsm_map)) return; |
969 |
959 |
970 if (!ec_fsm_mapping_success(&fsm->fsm_map)) { |
960 if (!ec_fsm_mapping_success(&fsm->fsm_map)) { |
971 EC_ERR("PDO mapping configuration failed for slave %u.\n", |
961 EC_ERR("PDO mapping configuration failed for slave %u.\n", |
|
962 fsm->slave->ring_position); |
|
963 fsm->slave->error_flag = 1; |
|
964 fsm->state = ec_fsm_slave_state_error; |
|
965 return; |
|
966 } |
|
967 |
|
968 // Start Pdo configuration |
|
969 fsm->state = ec_fsm_slave_conf_state_pdo_conf; |
|
970 ec_fsm_pdo_config_start(&fsm->fsm_pdo, fsm->slave); |
|
971 ec_fsm_pdo_config_exec(&fsm->fsm_pdo); // execute immediately |
|
972 } |
|
973 |
|
974 /*****************************************************************************/ |
|
975 |
|
976 /** |
|
977 Slave configuration state: PDO_CONF. |
|
978 */ |
|
979 |
|
980 void ec_fsm_slave_conf_state_pdo_conf( |
|
981 ec_fsm_slave_t *fsm /**< slave state machine */ |
|
982 ) |
|
983 { |
|
984 if (ec_fsm_pdo_config_exec(&fsm->fsm_pdo)) return; |
|
985 |
|
986 if (!ec_fsm_pdo_config_success(&fsm->fsm_pdo)) { |
|
987 EC_ERR("Pdo configuration failed for slave %u.\n", |
972 fsm->slave->ring_position); |
988 fsm->slave->ring_position); |
973 fsm->slave->error_flag = 1; |
989 fsm->slave->error_flag = 1; |
974 fsm->state = ec_fsm_slave_state_error; |
990 fsm->state = ec_fsm_slave_state_error; |
975 return; |
991 return; |
976 } |
992 } |