133 /****************************************************************************** |
133 /****************************************************************************** |
134 * state functions |
134 * state functions |
135 *****************************************************************************/ |
135 *****************************************************************************/ |
136 |
136 |
137 /** |
137 /** |
138 * Start reading mapping. |
138 * Start reading Pdo assignment. |
139 */ |
139 */ |
140 |
140 |
141 void ec_fsm_coe_map_state_start( |
141 void ec_fsm_coe_map_state_start( |
142 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
142 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
143 ) |
143 ) |
144 { |
144 { |
145 // read mapping for first direction |
145 // read Pdo assignment for first direction |
146 fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT |
146 fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT |
147 ec_fsm_coe_map_action_next_dir(fsm); |
147 ec_fsm_coe_map_action_next_dir(fsm); |
148 } |
148 } |
149 |
149 |
150 /*****************************************************************************/ |
150 /*****************************************************************************/ |
151 |
151 |
152 /** |
152 /** |
153 * Read mapping of next direction manager. |
153 * Read Pdo assignment of next direction manager. |
154 */ |
154 */ |
155 |
155 |
156 void ec_fsm_coe_map_action_next_dir( |
156 void ec_fsm_coe_map_action_next_dir( |
157 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
157 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
158 ) |
158 ) |
174 } |
174 } |
175 |
175 |
176 fsm->sync_sdo_index = 0x1C10 + fsm->sync->index; |
176 fsm->sync_sdo_index = 0x1C10 + fsm->sync->index; |
177 |
177 |
178 if (slave->master->debug_level) |
178 if (slave->master->debug_level) |
179 EC_DBG("Reading Pdo mapping of sync manager %u of slave %u.\n", |
179 EC_DBG("Reading Pdo assignment of sync manager %u of slave %u.\n", |
180 fsm->sync->index, slave->ring_position); |
180 fsm->sync->index, slave->ring_position); |
181 |
181 |
182 ec_pdo_mapping_clear_pdos(&fsm->mapping); |
182 ec_pdo_list_clear_pdos(&fsm->pdos); |
183 |
183 |
184 ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0); |
184 ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0); |
185 ecrt_sdo_request_read(&fsm->request); |
185 ecrt_sdo_request_read(&fsm->request); |
186 fsm->state = ec_fsm_coe_map_state_pdo_count; |
186 fsm->state = ec_fsm_coe_map_state_pdo_count; |
187 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
187 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
188 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
188 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
189 return; |
189 return; |
190 } |
190 } |
191 |
191 |
192 if (slave->master->debug_level) |
192 if (slave->master->debug_level) |
193 EC_DBG("Reading of Pdo mapping finished for slave %u.\n", |
193 EC_DBG("Reading of Pdo assignment finished for slave %u.\n", |
194 slave->ring_position); |
194 slave->ring_position); |
195 |
195 |
196 fsm->state = ec_fsm_coe_map_state_end; |
196 fsm->state = ec_fsm_coe_map_state_end; |
197 } |
197 } |
198 |
198 |
199 /*****************************************************************************/ |
199 /*****************************************************************************/ |
200 |
200 |
201 /** |
201 /** |
202 * Count mapped Pdos. |
202 * Count assigned Pdos. |
203 */ |
203 */ |
204 |
204 |
205 void ec_fsm_coe_map_state_pdo_count( |
205 void ec_fsm_coe_map_state_pdo_count( |
206 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
206 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
207 ) |
207 ) |
216 } |
216 } |
217 |
217 |
218 fsm->sync_subindices = EC_READ_U8(fsm->request.data); |
218 fsm->sync_subindices = EC_READ_U8(fsm->request.data); |
219 |
219 |
220 if (fsm->slave->master->debug_level) |
220 if (fsm->slave->master->debug_level) |
221 EC_DBG(" %u Pdos mapped.\n", fsm->sync_subindices); |
221 EC_DBG(" %u Pdos assigned.\n", fsm->sync_subindices); |
222 |
222 |
223 // read first Pdo |
223 // read first Pdo |
224 fsm->sync_subindex = 1; |
224 fsm->sync_subindex = 1; |
225 ec_fsm_coe_map_action_next_pdo(fsm); |
225 ec_fsm_coe_map_action_next_pdo(fsm); |
226 } |
226 } |
243 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
243 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
244 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
244 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
245 return; |
245 return; |
246 } |
246 } |
247 |
247 |
248 // finished reading Pdo mapping/configuration |
248 // finished reading Pdo assignment/mapping |
249 |
249 |
250 if (ec_pdo_mapping_copy(&fsm->sync->mapping, &fsm->mapping)) { |
250 if (ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos)) { |
251 fsm->state = ec_fsm_coe_map_state_error; |
251 fsm->state = ec_fsm_coe_map_state_error; |
252 return; |
252 return; |
253 } |
253 } |
254 |
254 |
255 fsm->sync->mapping_source = EC_SYNC_MAPPING_COE; |
255 fsm->sync->assign_source = EC_ASSIGN_COE; |
256 ec_pdo_mapping_clear_pdos(&fsm->mapping); |
256 ec_pdo_list_clear_pdos(&fsm->pdos); |
257 |
257 |
258 // next direction |
258 // next direction |
259 ec_fsm_coe_map_action_next_dir(fsm); |
259 ec_fsm_coe_map_action_next_dir(fsm); |
260 } |
260 } |
261 |
261 |
270 ) |
270 ) |
271 { |
271 { |
272 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
272 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
273 |
273 |
274 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
274 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
275 EC_ERR("Failed to read mapped Pdo index from slave %u.\n", |
275 EC_ERR("Failed to read assigned Pdo index from slave %u.\n", |
276 fsm->slave->ring_position); |
276 fsm->slave->ring_position); |
277 fsm->state = ec_fsm_coe_map_state_error; |
277 fsm->state = ec_fsm_coe_map_state_error; |
278 return; |
278 return; |
279 } |
279 } |
280 |
280 |
290 fsm->pdo->dir = ec_sync_direction(fsm->sync); |
290 fsm->pdo->dir = ec_sync_direction(fsm->sync); |
291 |
291 |
292 if (fsm->slave->master->debug_level) |
292 if (fsm->slave->master->debug_level) |
293 EC_DBG(" Pdo 0x%04X.\n", fsm->pdo->index); |
293 EC_DBG(" Pdo 0x%04X.\n", fsm->pdo->index); |
294 |
294 |
295 list_add_tail(&fsm->pdo->list, &fsm->mapping.pdos); |
295 list_add_tail(&fsm->pdo->list, &fsm->pdos.list); |
296 |
296 |
297 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
297 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
298 ecrt_sdo_request_read(&fsm->request); |
298 ecrt_sdo_request_read(&fsm->request); |
299 fsm->state = ec_fsm_coe_map_state_pdo_entry_count; |
299 fsm->state = ec_fsm_coe_map_state_pdo_entry_count; |
300 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
300 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
365 ) |
365 ) |
366 { |
366 { |
367 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
367 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
368 |
368 |
369 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
369 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
370 EC_ERR("Failed to read index of mapped Pdo entry from slave %u.\n", |
370 EC_ERR("Failed to read mapped Pdo entry from slave %u.\n", |
371 fsm->slave->ring_position); |
371 fsm->slave->ring_position); |
372 fsm->state = ec_fsm_coe_map_state_error; |
372 fsm->state = ec_fsm_coe_map_state_error; |
373 return; |
373 return; |
374 } |
374 } |
375 |
375 |