63 ec_fsm_pdo_config_t *fsm, /**< pdo_config state machine */ |
63 ec_fsm_pdo_config_t *fsm, /**< pdo_config 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_config_clear( |
75 void ec_fsm_pdo_config_clear( |
76 ec_fsm_pdo_config_t *fsm /**< pdo_config state machine */ |
76 ec_fsm_pdo_config_t *fsm /**< pdo_config state machine */ |
77 ) |
77 ) |
78 { |
78 { |
|
79 ec_sdo_request_clear(&fsm->request); |
79 } |
80 } |
80 |
81 |
81 /*****************************************************************************/ |
82 /*****************************************************************************/ |
82 |
83 |
83 /** Start Pdo configuration state machine. |
84 /** Start Pdo configuration state machine. |
202 if (fsm->slave->master->debug_level) { |
203 if (fsm->slave->master->debug_level) { |
203 EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n", |
204 EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n", |
204 fsm->pdo->index, fsm->slave->ring_position); |
205 fsm->pdo->index, fsm->slave->ring_position); |
205 } |
206 } |
206 |
207 |
|
208 if (ec_sdo_request_alloc(&fsm->request, 4)) { |
|
209 fsm->state = ec_fsm_pdo_config_state_error; |
|
210 return; |
|
211 } |
|
212 |
207 // set mapped Pdo count to zero |
213 // set mapped Pdo count to zero |
208 fsm->sdodata.index = fsm->pdo->index; |
214 EC_WRITE_U8(&fsm->request.data, 0); |
209 fsm->sdodata.subindex = 0; // number of configured entries |
215 fsm->request.data_size = 1; |
210 EC_WRITE_U8(&fsm->sdo_value, 0); |
216 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
211 fsm->sdodata.size = 1; |
217 ec_sdo_request_write(&fsm->request); |
212 if (fsm->slave->master->debug_level) |
218 if (fsm->slave->master->debug_level) |
213 EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n", |
219 EC_DBG("Setting entry count to zero for Pdo 0x%04X.\n", |
214 fsm->pdo->index); |
220 fsm->pdo->index); |
215 |
221 |
216 fsm->state = ec_fsm_pdo_config_state_zero_count; |
222 fsm->state = ec_fsm_pdo_config_state_zero_count; |
217 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
223 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); |
218 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
224 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
219 } |
225 } |
220 |
226 |
221 /*****************************************************************************/ |
227 /*****************************************************************************/ |
222 |
228 |
241 ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine. */ |
247 ec_fsm_pdo_config_t *fsm /**< Pdo configuration state machine. */ |
242 ) |
248 ) |
243 { |
249 { |
244 uint32_t value; |
250 uint32_t value; |
245 |
251 |
246 fsm->sdodata.subindex = fsm->entry_count; |
|
247 value = fsm->entry->index << 16 |
252 value = fsm->entry->index << 16 |
248 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
253 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
249 EC_WRITE_U32(&fsm->sdo_value, value); |
254 EC_WRITE_U32(&fsm->request.data, value); |
250 fsm->sdodata.size = 4; |
255 fsm->request.data_size = 4; |
251 |
256 ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->entry_count); |
|
257 ec_sdo_request_write(&fsm->request); |
252 if (fsm->slave->master->debug_level) |
258 if (fsm->slave->master->debug_level) |
253 EC_DBG("Configuring Pdo entry %08X at position %u.\n", |
259 EC_DBG("Configuring Pdo entry %08X at position %u.\n", |
254 value, fsm->entry_count); |
260 value, fsm->entry_count); |
255 |
261 |
256 fsm->state = ec_fsm_pdo_config_state_add_entry; |
262 fsm->state = ec_fsm_pdo_config_state_add_entry; |
257 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
263 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); |
258 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
264 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
259 } |
265 } |
260 |
266 |
261 /*****************************************************************************/ |
267 /*****************************************************************************/ |
262 |
268 |
309 } |
315 } |
310 |
316 |
311 // find next entry |
317 // find next entry |
312 if (!(fsm->entry = ec_fsm_pdo_config_next_entry(fsm, &fsm->entry->list))) { |
318 if (!(fsm->entry = ec_fsm_pdo_config_next_entry(fsm, &fsm->entry->list))) { |
313 // No more entries to add. Write entry count. |
319 // No more entries to add. Write entry count. |
314 fsm->sdodata.subindex = 0; |
320 EC_WRITE_U8(&fsm->request.data, fsm->entry_count); |
315 EC_WRITE_U8(&fsm->sdo_value, fsm->entry_count); |
321 fsm->request.data_size = 1; |
316 fsm->sdodata.size = 1; |
322 ec_sdo_request_address(&fsm->request, fsm->pdo->index, 0); |
317 |
323 ec_sdo_request_write(&fsm->request); |
318 if (fsm->slave->master->debug_level) |
324 if (fsm->slave->master->debug_level) |
319 EC_DBG("Setting number of Pdo entries to %u.\n", |
325 EC_DBG("Setting number of Pdo entries to %u.\n", |
320 fsm->entry_count); |
326 fsm->entry_count); |
321 |
327 |
322 fsm->state = ec_fsm_pdo_config_state_entry_count; |
328 fsm->state = ec_fsm_pdo_config_state_entry_count; |
323 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); |
329 ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->request); |
324 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
330 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
325 return; |
331 return; |
326 } |
332 } |
327 |
333 |
328 // add next entry |
334 // add next entry |