equal
deleted
inserted
replaced
42 #include "master.h" |
42 #include "master.h" |
43 #include "mailbox.h" |
43 #include "mailbox.h" |
44 #include "sdo_request.h" |
44 #include "sdo_request.h" |
45 #include "fsm_coe_map.h" |
45 #include "fsm_coe_map.h" |
46 |
46 |
47 /** Retry reading the number of assigned Pdos if this time is not elapsed. |
|
48 */ |
|
49 #define EC_FSM_COE_MAP_TIMEOUT 5 |
|
50 |
|
51 /*****************************************************************************/ |
47 /*****************************************************************************/ |
52 |
48 |
53 void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *); |
49 void ec_fsm_coe_map_state_start(ec_fsm_coe_map_t *); |
54 void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *); |
50 void ec_fsm_coe_map_state_pdo_count(ec_fsm_coe_map_t *); |
55 void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *); |
51 void ec_fsm_coe_map_state_pdo(ec_fsm_coe_map_t *); |
184 fsm->sync->index, slave->ring_position); |
180 fsm->sync->index, slave->ring_position); |
185 |
181 |
186 ec_pdo_list_clear_pdos(&fsm->pdos); |
182 ec_pdo_list_clear_pdos(&fsm->pdos); |
187 |
183 |
188 ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0); |
184 ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, 0); |
|
185 fsm->request.response_timeout = 10000; |
189 ecrt_sdo_request_read(&fsm->request); |
186 ecrt_sdo_request_read(&fsm->request); |
190 fsm->jiffies_start = jiffies; |
|
191 fsm->state = ec_fsm_coe_map_state_pdo_count; |
187 fsm->state = ec_fsm_coe_map_state_pdo_count; |
192 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
188 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
193 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
189 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
194 return; |
190 return; |
195 } |
191 } |
212 ) |
208 ) |
213 { |
209 { |
214 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
210 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
215 |
211 |
216 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
212 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
217 if (jiffies - fsm->jiffies_start < HZ * EC_FSM_COE_MAP_TIMEOUT) { |
|
218 if (fsm->slave->master->debug_level) |
|
219 EC_DBG("Failed to read number of assigned Pdos from " |
|
220 "slave %u. Retrying...\n", fsm->slave->ring_position); |
|
221 ecrt_sdo_request_read(&fsm->request); |
|
222 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
|
223 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
|
224 return; |
|
225 } |
|
226 EC_ERR("Failed to read number of assigned Pdos from slave %u.\n", |
213 EC_ERR("Failed to read number of assigned Pdos from slave %u.\n", |
227 fsm->slave->ring_position); |
214 fsm->slave->ring_position); |
228 fsm->state = ec_fsm_coe_map_state_error; |
215 fsm->state = ec_fsm_coe_map_state_error; |
229 return; |
216 return; |
230 } |
217 } |
250 ) |
237 ) |
251 { |
238 { |
252 if (fsm->sync_subindex <= fsm->sync_subindices) { |
239 if (fsm->sync_subindex <= fsm->sync_subindices) { |
253 ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, |
240 ec_sdo_request_address(&fsm->request, fsm->sync_sdo_index, |
254 fsm->sync_subindex); |
241 fsm->sync_subindex); |
|
242 fsm->request.response_timeout = 0; |
255 ecrt_sdo_request_read(&fsm->request); |
243 ecrt_sdo_request_read(&fsm->request); |
256 fsm->state = ec_fsm_coe_map_state_pdo; |
244 fsm->state = ec_fsm_coe_map_state_pdo; |
257 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
245 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
258 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
246 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
259 return; |
247 return; |
307 EC_DBG(" Pdo 0x%04X.\n", fsm->pdo->index); |
295 EC_DBG(" Pdo 0x%04X.\n", fsm->pdo->index); |
308 |
296 |
309 list_add_tail(&fsm->pdo->list, &fsm->pdos.list); |
297 list_add_tail(&fsm->pdo->list, &fsm->pdos.list); |
310 |
298 |
311 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
299 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
|
300 fsm->request.response_timeout = 0; |
312 ecrt_sdo_request_read(&fsm->request); |
301 ecrt_sdo_request_read(&fsm->request); |
313 fsm->state = ec_fsm_coe_map_state_pdo_entry_count; |
302 fsm->state = ec_fsm_coe_map_state_pdo_entry_count; |
314 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
303 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
315 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
304 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
316 } |
305 } |
354 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
343 ec_fsm_coe_map_t *fsm /**< finite state machine */ |
355 ) |
344 ) |
356 { |
345 { |
357 if (fsm->pdo_subindex <= fsm->pdo_subindices) { |
346 if (fsm->pdo_subindex <= fsm->pdo_subindices) { |
358 ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex); |
347 ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->pdo_subindex); |
|
348 fsm->request.response_timeout = 0; |
359 ecrt_sdo_request_read(&fsm->request); |
349 ecrt_sdo_request_read(&fsm->request); |
360 fsm->state = ec_fsm_coe_map_state_pdo_entry; |
350 fsm->state = ec_fsm_coe_map_state_pdo_entry; |
361 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
351 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
362 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
352 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
363 return; |
353 return; |