# HG changeset patch # User Florian Pose # Date 1203437398 0 # Node ID 6b1e2b0ed8bc414dfd284fb83386a03300c43eb1 # Parent e0b8cbc8a88d378d69f7ae52ee15e86f807522c7 Cleaned up Pdo mapping and configuration state machines. diff -r e0b8cbc8a88d -r 6b1e2b0ed8bc master/fsm_mapping.c --- a/master/fsm_mapping.c Tue Feb 19 16:03:04 2008 +0000 +++ b/master/fsm_mapping.c Tue Feb 19 16:09:58 2008 +0000 @@ -150,7 +150,7 @@ return; } - fsm->dir = EC_DIR_OUTPUT; + fsm->sync = NULL; ec_fsm_mapping_next_sync(fsm); } @@ -162,31 +162,38 @@ ec_fsm_mapping_t *fsm /**< mapping state machine */ ) { - while (1) { - if (fsm->dir > EC_DIR_INPUT) { - // no more directions to configure mappings for - fsm->state = ec_fsm_mapping_state_end; - return; - } - - if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) { - EC_WARN("next_sync(): No sync manager!\n"); - fsm->dir++; + ec_direction_t dir; + const ec_sync_t *sync; + const ec_pdo_mapping_t *map; + + for (dir = EC_DIR_OUTPUT; dir <= EC_DIR_INPUT; dir++) { + if (!(sync = ec_slave_get_pdo_sync(fsm->slave, dir))) { + EC_WARN("No sync manager for direction %u!\n", dir); continue; } - fsm->mapping = &fsm->slave->config->mapping[fsm->dir]; - - if (ec_pdo_mapping_equal(&fsm->sync->mapping, fsm->mapping)) { - // the mapping for this direction does not have to be altered - fsm->dir++; - continue; + if (fsm->sync) { // there is a last SM + if (sync == fsm->sync) // this is the last SM + fsm->sync = NULL; // take the next one } else { - fsm->dir++; + map = &fsm->slave->config->mapping[dir]; + if (ec_pdo_mapping_equal(&sync->mapping, map)) + continue; + + fsm->sync = sync; + fsm->mapping = map; break; } } + if (!sync) { + if (fsm->slave->master->debug_level) + EC_DBG("Pdo mapping finished for slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_mapping_state_end; + return; + } + if (fsm->slave->master->debug_level) { EC_DBG("Configuring PDO mapping for SM%u of slave %u.\n", fsm->sync->index, fsm->slave->ring_position); @@ -222,6 +229,27 @@ /*****************************************************************************/ +/** Add a Pdo to the mapping. + */ +void ec_fsm_mapping_add_pdo( + ec_fsm_mapping_t *fsm /**< mapping state machine */ + ) +{ + fsm->sdodata.subindex = fsm->pdo_count; + EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); + fsm->sdodata.size = 2; + + if (fsm->slave->master->debug_level) + EC_DBG("Mapping PDO 0x%04X at position %u.\n", + fsm->pdo->index, fsm->sdodata.subindex); + + fsm->state = ec_fsm_mapping_state_add_pdo; + ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); + ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately +} + +/*****************************************************************************/ + /** Set the number of mapped PDOs to zero. */ void ec_fsm_mapping_state_zero_count( @@ -250,17 +278,7 @@ // add first PDO to mapping fsm->pdo_count = 1; - fsm->sdodata.subindex = fsm->pdo_count; - EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); - fsm->sdodata.size = 2; - - if (fsm->slave->master->debug_level) - EC_DBG("Mapping PDO 0x%04X at position %u.\n", - fsm->pdo->index, fsm->sdodata.subindex); - - fsm->state = ec_fsm_mapping_state_add_pdo; - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); - ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + ec_fsm_mapping_add_pdo(fsm); } /*****************************************************************************/ @@ -299,16 +317,7 @@ // add next PDO to mapping fsm->pdo_count++; - fsm->sdodata.subindex = fsm->pdo_count; - EC_WRITE_U16(&fsm->sdo_value, fsm->pdo->index); - fsm->sdodata.size = 2; - - if (fsm->slave->master->debug_level) - EC_DBG("Mapping PDO 0x%04X at position %u.\n", - fsm->pdo->index, fsm->sdodata.subindex); - - ec_fsm_coe_download(fsm->fsm_coe, fsm->slave, &fsm->sdodata); - ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately + ec_fsm_mapping_add_pdo(fsm); } /*****************************************************************************/ diff -r e0b8cbc8a88d -r 6b1e2b0ed8bc master/fsm_mapping.h --- a/master/fsm_mapping.h Tue Feb 19 16:03:04 2008 +0000 +++ b/master/fsm_mapping.h Tue Feb 19 16:09:58 2008 +0000 @@ -54,18 +54,17 @@ */ struct ec_fsm_mapping { - void (*state)(ec_fsm_mapping_t *); /**< state function */ - ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use */ + void (*state)(ec_fsm_mapping_t *); /**< State function. */ + ec_fsm_coe_t *fsm_coe; /**< CoE state machine to use. */ + ec_slave_t *slave; /**< Slave the FSM runs on. */ - ec_slave_t *slave; /**< slave the FSM runs on */ + const ec_sync_t *sync; /**< Current sync manager. */ + const ec_pdo_mapping_t *mapping; /**< Target Pdo mapping. */ + const ec_pdo_t *pdo; /**< Current Pdo. */ - ec_direction_t dir; /**< current PDO direction */ - ec_sync_t *sync; /**< current sync manager */ - const ec_pdo_mapping_t *mapping; /**< Mapping to assign. */ - ec_pdo_t *pdo; /**< current PDO */ - ec_sdo_data_t sdodata; /**< SDO configuration data */ - uint16_t sdo_value; /**< SDO value */ - unsigned int pdo_count; /**< number of mapped PDOs */ + ec_sdo_data_t sdodata; /**< SDO configuration data. */ + uint16_t sdo_value; /**< SDO value. */ + unsigned int pdo_count; /**< Number of mapped Pdos. */ }; /*****************************************************************************/ diff -r e0b8cbc8a88d -r 6b1e2b0ed8bc master/fsm_pdo_config.c --- a/master/fsm_pdo_config.c Tue Feb 19 16:03:04 2008 +0000 +++ b/master/fsm_pdo_config.c Tue Feb 19 16:09:58 2008 +0000 @@ -167,27 +167,28 @@ map = &fsm->slave->config->mapping[dir]; list_for_each_entry(pdo, &map->pdos, list) { - if (fsm->pdo) { - if (pdo == fsm->pdo) - fsm->pdo = NULL; + if (fsm->pdo) { // there was a Pdo configured in the last run + if (pdo == fsm->pdo) // this is the last Pdo + fsm->pdo = NULL; // take the next one } else { if ((mapped_pdo = ec_slave_find_pdo(fsm->slave, pdo->index))) if (ec_pdo_equal_entries(pdo, mapped_pdo)) continue; // Pdo configured correctly fsm->pdo = pdo; - goto configure; + break; } } } - if (fsm->slave->master->debug_level) - EC_DBG("Pdo configuration finished for slave %u.\n", - fsm->slave->ring_position); - fsm->state = ec_fsm_pdo_config_state_end; - return; - -configure: + if (!fsm->pdo) { + if (fsm->slave->master->debug_level) + EC_DBG("Pdo configuration finished for slave %u.\n", + fsm->slave->ring_position); + fsm->state = ec_fsm_pdo_config_state_end; + return; + } + if (fsm->slave->master->debug_level) { EC_DBG("Changing configuration of Pdo 0x%04X of slave %u.\n", fsm->pdo->index, fsm->slave->ring_position); diff -r e0b8cbc8a88d -r 6b1e2b0ed8bc master/fsm_pdo_config.h --- a/master/fsm_pdo_config.h Tue Feb 19 16:03:04 2008 +0000 +++ b/master/fsm_pdo_config.h Tue Feb 19 16:09:58 2008 +0000 @@ -31,10 +31,9 @@ * *****************************************************************************/ -/** - \file - EtherCAT PDO configuration state machine structures. -*/ +/** \file + * EtherCAT PDO configuration state machine structures. + */ /*****************************************************************************/