159 */ |
159 */ |
160 void ec_fsm_pdo_mapping_next_pdo( |
160 void ec_fsm_pdo_mapping_next_pdo( |
161 ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */ |
161 ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */ |
162 ) |
162 ) |
163 { |
163 { |
164 ec_direction_t dir; |
164 uint8_t sync_index; |
165 const ec_pdo_list_t *pdos; |
165 const ec_pdo_list_t *pdos; |
166 const ec_pdo_t *pdo, *assigned_pdo; |
166 const ec_pdo_t *pdo, *assigned_pdo; |
167 |
167 |
168 for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) { |
168 for (sync_index = 0; sync_index < EC_MAX_SYNCS; sync_index++) { |
169 pdos = &fsm->slave->config->pdos[dir]; |
169 pdos = &fsm->slave->config->sync_configs[sync_index].pdos; |
170 |
170 |
171 list_for_each_entry(pdo, &pdos->list, list) { |
171 list_for_each_entry(pdo, &pdos->list, list) { |
172 if (fsm->pdo) { // there was a Pdo mapping changed in the last run |
172 if (fsm->pdo) { // there was a Pdo mapping changed in the last run |
173 if (pdo == fsm->pdo) // this is the previously configured Pdo |
173 if (pdo == fsm->pdo) // this is the previously configured Pdo |
174 fsm->pdo = NULL; // take the next one |
174 fsm->pdo = NULL; // take the next one |
249 ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */ |
249 ec_fsm_pdo_mapping_t *fsm /**< Pdo mapping state machine. */ |
250 ) |
250 ) |
251 { |
251 { |
252 uint32_t value; |
252 uint32_t value; |
253 |
253 |
|
254 if (fsm->slave->master->debug_level) |
|
255 EC_DBG("Mapping Pdo entry 0x%04X:%02X (%u bit) at position %u.\n", |
|
256 fsm->entry->index, fsm->entry->subindex, |
|
257 fsm->entry->bit_length, fsm->entry_count); |
|
258 |
254 value = fsm->entry->index << 16 |
259 value = fsm->entry->index << 16 |
255 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
260 | fsm->entry->subindex << 8 | fsm->entry->bit_length; |
256 EC_WRITE_U32(fsm->request.data, value); |
261 EC_WRITE_U32(fsm->request.data, value); |
257 fsm->request.data_size = 4; |
262 fsm->request.data_size = 4; |
258 ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->entry_count); |
263 ec_sdo_request_address(&fsm->request, fsm->pdo->index, fsm->entry_count); |
259 ecrt_sdo_request_write(&fsm->request); |
264 ecrt_sdo_request_write(&fsm->request); |
260 if (fsm->slave->master->debug_level) |
|
261 EC_DBG("Configuring Pdo entry %08X at position %u.\n", |
|
262 value, fsm->entry_count); |
|
263 |
265 |
264 fsm->state = ec_fsm_pdo_mapping_state_add_entry; |
266 fsm->state = ec_fsm_pdo_mapping_state_add_entry; |
265 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
267 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
266 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
268 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
267 } |
269 } |