From 5d786d134b4214cd86e55c4f411b112dd97a8de6 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh-essen.com>
Date: Tue, 19 Feb 2008 16:09:58 +0000
Subject: [PATCH] Cleaned up Pdo mapping and configuration state machines.
---
master/fsm_mapping.c | 85 +++++++++++++++++++++++------------------
master/fsm_mapping.h | 19 +++++----
master/fsm_pdo_config.c | 21 +++++-----
master/fsm_pdo_config.h | 7 ++--
4 files changed, 70 insertions(+), 62 deletions(-)
diff --git a/master/fsm_mapping.c b/master/fsm_mapping.c
index d140947d..d493bae7 100644
--- a/master/fsm_mapping.c
+++ b/master/fsm_mapping.c
@@ -150,7 +150,7 @@ void ec_fsm_mapping_state_start(
return;
}
- fsm->dir = EC_DIR_OUTPUT;
+ fsm->sync = NULL;
ec_fsm_mapping_next_sync(fsm);
}
@@ -162,31 +162,38 @@ void ec_fsm_mapping_next_sync(
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;
- }
+ ec_direction_t dir;
+ const ec_sync_t *sync;
+ const ec_pdo_mapping_t *map;
- if (!(fsm->sync = ec_slave_get_pdo_sync(fsm->slave, fsm->dir))) {
- EC_WARN("next_sync(): No sync manager!\n");
- fsm->dir++;
+ 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 @@ ec_pdo_t *ec_fsm_mapping_next_pdo(
/*****************************************************************************/
+/** 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 @@ void ec_fsm_mapping_state_zero_count(
// 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 @@ void ec_fsm_mapping_state_add_pdo(
// 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 --git a/master/fsm_mapping.h b/master/fsm_mapping.h
index 74b41e4e..0bc3ced0 100644
--- a/master/fsm_mapping.h
+++ b/master/fsm_mapping.h
@@ -54,18 +54,17 @@ typedef struct ec_fsm_mapping ec_fsm_mapping_t; /**< \see ec_fsm_mapping */
*/
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 --git a/master/fsm_pdo_config.c b/master/fsm_pdo_config.c
index 850dc312..409fdb35 100644
--- a/master/fsm_pdo_config.c
+++ b/master/fsm_pdo_config.c
@@ -167,27 +167,28 @@ void ec_fsm_pdo_config_next_pdo(
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;
+ 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;
+ }
-configure:
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 --git a/master/fsm_pdo_config.h b/master/fsm_pdo_config.h
index 01f854a4..cae790fa 100644
--- a/master/fsm_pdo_config.h
+++ b/master/fsm_pdo_config.h
@@ -31,10 +31,9 @@
*
*****************************************************************************/
-/**
- \file
- EtherCAT PDO configuration state machine structures.
-*/
+/** \file
+ * EtherCAT PDO configuration state machine structures.
+ */
/*****************************************************************************/
--
GitLab