63 ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */ |
63 ec_fsm_pdo_mapping_t *fsm, /**< mapping state machine */ |
64 ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */ |
64 ec_fsm_coe_t *fsm_coe /**< CoE state machine to use */ |
65 ) |
65 ) |
66 { |
66 { |
67 fsm->fsm_coe = fsm_coe; |
67 fsm->fsm_coe = fsm_coe; |
68 fsm->sdodata.data = (uint8_t *) &fsm->sdo_value; |
68 ec_sdo_request_init(&fsm->request); |
69 } |
69 } |
70 |
70 |
71 /*****************************************************************************/ |
71 /*****************************************************************************/ |
72 |
72 |
73 /** Destructor. |
73 /** Destructor. |
74 */ |
74 */ |
75 void ec_fsm_pdo_mapping_clear( |
75 void ec_fsm_pdo_mapping_clear( |
76 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
76 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
77 ) |
77 ) |
78 { |
78 { |
|
79 ec_sdo_request_clear(&fsm->request); |
79 } |
80 } |
80 |
81 |
81 /*****************************************************************************/ |
82 /*****************************************************************************/ |
82 |
83 |
83 /** Start Pdo mapping configuration state machine. |
84 /** Start Pdo mapping configuration state machine. |
193 if (fsm->slave->master->debug_level) { |
194 if (fsm->slave->master->debug_level) { |
194 EC_DBG("Changing Pdo mapping for SM%u of slave %u.\n", |
195 EC_DBG("Changing Pdo mapping for SM%u of slave %u.\n", |
195 fsm->sync->index, fsm->slave->ring_position); |
196 fsm->sync->index, fsm->slave->ring_position); |
196 } |
197 } |
197 |
198 |
|
199 if (ec_sdo_request_alloc(&fsm->request, 2)) { |
|
200 fsm->state = ec_fsm_pdo_mapping_state_error; |
|
201 return; |
|
202 } |
|
203 |
198 // set mapped Pdo count to zero |
204 // set mapped Pdo count to zero |
199 fsm->sdodata.index = 0x1C10 + fsm->sync->index; |
205 EC_WRITE_U8(&fsm->request.data, 0); // zero Pdos mapped |
200 fsm->sdodata.subindex = 0; // mapped Pdo count |
206 fsm->request.data_size = 1; |
201 EC_WRITE_U8(&fsm->sdo_value, 0); // zero Pdos mapped |
207 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0); |
202 fsm->sdodata.size = 1; |
208 ec_sdo_request_write(&fsm->request); |
203 if (fsm->slave->master->debug_level) |
209 if (fsm->slave->master->debug_level) |
204 EC_DBG("Setting Pdo count to zero for SM%u.\n", fsm->sync->index); |
210 EC_DBG("Setting Pdo count to zero for SM%u.\n", fsm->sync->index); |
205 |
211 |
206 fsm->state = ec_fsm_pdo_mapping_state_zero_count; |
212 fsm->state = ec_fsm_pdo_mapping_state_zero_count; |
207 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
213 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); |
208 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
214 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
209 return; |
215 return; |
210 } |
216 } |
211 |
217 |
212 if (fsm->slave->master->debug_level) |
218 if (fsm->slave->master->debug_level) |
236 */ |
242 */ |
237 void ec_fsm_pdo_mapping_add_pdo( |
243 void ec_fsm_pdo_mapping_add_pdo( |
238 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
244 ec_fsm_pdo_mapping_t *fsm /**< mapping state machine */ |
239 ) |
245 ) |
240 { |
246 { |
241 fsm->sdodata.subindex = fsm->pdo_count; |
247 EC_WRITE_U16(&fsm->request.data, fsm->pdo->index); |
242 EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); |
248 fsm->request.data_size = 2; |
243 fsm->sdodata.size = 2; |
249 ec_sdo_request_address(&fsm->request, |
244 |
250 0x1C10 + fsm->sync->index, fsm->pdo_count); |
|
251 ec_sdo_request_write(&fsm->request); |
245 if (fsm->slave->master->debug_level) |
252 if (fsm->slave->master->debug_level) |
246 EC_DBG("Mapping Pdo 0x%04X at position %u.\n", |
253 EC_DBG("Mapping Pdo 0x%04X at position %u.\n", |
247 fsm->pdo->index, fsm->sdodata.subindex); |
254 fsm->pdo->index, fsm->pdo_count); |
248 |
255 |
249 fsm->state = ec_fsm_pdo_mapping_state_add_pdo; |
256 fsm->state = ec_fsm_pdo_mapping_state_add_pdo; |
250 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
257 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); |
251 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
258 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
252 } |
259 } |
253 |
260 |
254 /*****************************************************************************/ |
261 /*****************************************************************************/ |
255 |
262 |
302 } |
309 } |
303 |
310 |
304 // find next Pdo |
311 // find next Pdo |
305 if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) { |
312 if (!(fsm->pdo = ec_fsm_pdo_mapping_next_pdo(fsm, &fsm->pdo->list))) { |
306 // no more Pdos to map. write Pdo count |
313 // no more Pdos to map. write Pdo count |
307 fsm->sdodata.subindex = 0; |
314 EC_WRITE_U8(&fsm->request.data, fsm->pdo_count); |
308 EC_WRITE_U8(&fsm->sdo_value, fsm->pdo_count); |
315 fsm->request.data_size = 1; |
309 fsm->sdodata.size = 1; |
316 ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync->index, 0); |
310 |
317 ec_sdo_request_write(&fsm->request); |
311 if (fsm->slave->master->debug_level) |
318 if (fsm->slave->master->debug_level) |
312 EC_DBG("Setting number of mapped Pdos to %u.\n", |
319 EC_DBG("Setting number of mapped Pdos to %u.\n", |
313 fsm->pdo_count); |
320 fsm->pdo_count); |
314 |
321 |
315 fsm->state = ec_fsm_pdo_mapping_state_pdo_count; |
322 fsm->state = ec_fsm_pdo_mapping_state_pdo_count; |
316 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
323 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); |
317 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
324 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
318 return; |
325 return; |
319 } |
326 } |
320 |
327 |
321 // add next Pdo to mapping |
328 // add next Pdo to mapping |