From b328ae794dde70ab83e49cc676876b9f686944ad Mon Sep 17 00:00:00 2001 From: r2k-in-the-vortex <30666740+r2k-in-the-vortex@users.noreply.github.com> Date: Thu, 22 Feb 2024 19:47:44 +0200 Subject: [PATCH] Multientrydev (#7) * refactored to enable multiple entires per PDO registration * subindex fixed * fix tx subindex * entry / pdo index fixes --------- Co-authored-by: Rainer Kordmaa --- .vscode/launch.json | 2 +- .vscode/settings.json | 3 +- src/ecat_handler/ecat_handler.c | 149 +++++++++++++++++--------- src/ecat_handler/ecat_handler.h | 36 +++++-- src/xml_conf_parser/xml_conf_parser.c | 44 +++++--- 5 files changed, 161 insertions(+), 73 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 0669e90..baa3214 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -14,7 +14,7 @@ "cwd": "${fileDirname}", "environment": [], "externalConsole": false, - "miDebuggerPath": "/home/pi/gdb.sh", + //"miDebuggerPath": "/home/pi/gdb.sh", "MIMode": "gdb", "setupCommands": [ { diff --git a/.vscode/settings.json b/.vscode/settings.json index d9fff41..fb093b7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,7 +3,8 @@ "ecrt.h": "c", "log.h": "c", "ecat_handler.h": "c", - "xml_conf_parser.h": "c" + "xml_conf_parser.h": "c", + "algorithm": "c" }, "cmake.cmakePath": "cmake", "cmake.sourceDirectory": "${workspaceFolder}/.", diff --git a/src/ecat_handler/ecat_handler.c b/src/ecat_handler/ecat_handler.c index 47fd4af..a2af907 100644 --- a/src/ecat_handler/ecat_handler.c +++ b/src/ecat_handler/ecat_handler.c @@ -85,7 +85,7 @@ static uint8_t *domain1_pd = NULL; /****************************************************************************/ -int RegisterRxInDomain(SlaveConfig *slave, ec_pdo_entry_info_t *entry, EcatPdo *pdo){ +int RegisterRxInDomain(SlaveConfig *slave, ec_pdo_entry_info_t *entry, EcatPdo *pdo, char *entryname){ domain1_regs[domain1_regs_idx].alias = slave->alias; domain1_regs[domain1_regs_idx].position = slave->position; domain1_regs[domain1_regs_idx].vendor_id = slave->vendor_id; @@ -99,14 +99,24 @@ int RegisterRxInDomain(SlaveConfig *slave, ec_pdo_entry_info_t *entry, EcatPdo * RxPdoRegistry[RxPdoRegistry_idx].slavename = slave->name; RxPdoRegistry[RxPdoRegistry_idx].slavetype = slave->type; RxPdoRegistry[RxPdoRegistry_idx].bitlength = entry->bit_length; - RxPdoRegistry[RxPdoRegistry_idx].pdoname = pdo->name; + + char *str = (char*)malloc((255 + 1) * sizeof(char)); + + strcpy(str, pdo->name); + strcat(str, "_"); + strcat(str, entryname); + RxPdoRegistry[RxPdoRegistry_idx].pdoname = (char*)malloc((strlen(str) + 1) * sizeof(char)); + strcpy(RxPdoRegistry[RxPdoRegistry_idx].pdoname, str); + + free(str); + RxPdoRegistry[RxPdoRegistry_idx].pdoidx = RxPdoRegistry_idx; domain1_regs_idx++; RxPdoRegistry_idx++; return 0; } -int RegisterTxInDomain(SlaveConfig *slave, ec_pdo_entry_info_t *entry, EcatPdo *pdo){ +int RegisterTxInDomain(SlaveConfig *slave, ec_pdo_entry_info_t *entry, EcatPdo *pdo, char *entryname){ domain1_regs[domain1_regs_idx].alias = slave->alias; domain1_regs[domain1_regs_idx].position = slave->position; domain1_regs[domain1_regs_idx].vendor_id = slave->vendor_id; @@ -120,7 +130,16 @@ int RegisterTxInDomain(SlaveConfig *slave, ec_pdo_entry_info_t *entry, EcatPdo * TxPdoRegistry[TxPdoRegistry_idx].slavename = slave->name; TxPdoRegistry[TxPdoRegistry_idx].slavetype = slave->type; TxPdoRegistry[TxPdoRegistry_idx].bitlength = entry->bit_length; - TxPdoRegistry[TxPdoRegistry_idx].pdoname = pdo->name; + + char *str = (char*)malloc((255 + 1) * sizeof(char)); + + strcpy(str, pdo->name); + strcat(str, "_"); + strcat(str, entryname); + TxPdoRegistry[TxPdoRegistry_idx].pdoname = (char*)malloc((strlen(str) + 1) * sizeof(char)); + strcpy(TxPdoRegistry[TxPdoRegistry_idx].pdoname, str); + + free(str); TxPdoRegistry[TxPdoRegistry_idx].pdoidx = TxPdoRegistry_idx; domain1_regs_idx++; @@ -128,6 +147,22 @@ int RegisterTxInDomain(SlaveConfig *slave, ec_pdo_entry_info_t *entry, EcatPdo * return 0; } +int countRxRegisters(EcatConfig *configin){ + int count = 0; + for (int i = 0; i < config->slave_count; i++) + for (int j = 0; j < config->slavesConfig[i].RxPdo_count; j++) + count += config->slavesConfig[i].RxPDO[j].entrycount; + return count; +} + +int countTxRegisters(EcatConfig *configin){ + int count = 0; + for (int i = 0; i < config->slave_count; i++) + for (int j = 0; j < config->slavesConfig[i].TxPdo_count; j++) + count += config->slavesConfig[i].TxPDO[j].entrycount; + return count; +} + int ConfigureSlave(EcatConfig *config, SlaveConfig *slave, ec_slave_config_t *sc){ log_trace("Slave %s %i|%i %i 0x%08X", slave->type, slave->alias, slave->position, slave->vendor_id, slave->product_code); if (!config->config_only_flag){ @@ -142,13 +177,13 @@ int ConfigureSlave(EcatConfig *config, SlaveConfig *slave, ec_slave_config_t *sc ec_sync_info_t *ss = (ec_sync_info_t*) malloc((slave->sm_count + 1) * sizeof(ec_sync_info_t)); slave_syncs[slave->position] = ss; - ec_pdo_entry_info_t *rxe = (ec_pdo_entry_info_t*) malloc(slave->RxPdo_count * sizeof(ec_pdo_entry_info_t)); + ec_pdo_entry_info_t *rxe = (ec_pdo_entry_info_t*) malloc(slave->RxEntry_count * sizeof(ec_pdo_entry_info_t)); rx_entries[slave->position] = rxe; ec_pdo_info_t *rxp = (ec_pdo_info_t*) malloc(slave->RxPdo_count * sizeof(ec_pdo_info_t)); rx_pdos[slave->position] = rxp; - ec_pdo_entry_info_t *txe = (ec_pdo_entry_info_t*) malloc(slave->TxPdo_count * sizeof(ec_pdo_entry_info_t)); + ec_pdo_entry_info_t *txe = (ec_pdo_entry_info_t*) malloc(slave->TxEntry_count * sizeof(ec_pdo_entry_info_t)); tx_entries[slave->position] = txe; ec_pdo_info_t *txp = (ec_pdo_info_t*) malloc(slave->TxPdo_count * sizeof(ec_pdo_info_t)); @@ -163,16 +198,15 @@ int ConfigureSlave(EcatConfig *config, SlaveConfig *slave, ec_slave_config_t *sc ss[i].watchdog_mode = EC_WD_DEFAULT; // set default } + int rxentryidx = 0; // rx pdos - for (int i = 0;i < slave->RxPdo_count;i++){ - EcatPdo rxpdo = slave->RxPDO[i]; - rxe[i].subindex = i + 1; - rxe[i].index = rxpdo.entryindex; - rxe[i].bit_length = rxpdo.bitlen; - rxp[i].entries = &rxe[i]; - rxp[i].index = rxpdo.index; - rxp[i].n_entries = 1; + for (int rxpdoidx = 0;rxpdoidx < slave->RxPdo_count;rxpdoidx++){ + EcatPdo rxpdo = slave->RxPDO[rxpdoidx]; + rxp[rxpdoidx].entries = &rxe[rxentryidx]; + rxp[rxpdoidx].index = rxpdo.index; + rxp[rxpdoidx].n_entries = rxpdo.entrycount; ss[rxpdo.sm].dir = EC_DIR_OUTPUT; + /* if for example 22:52:44 Sync master 0 EC_DIR_OUTPUT @@ -184,45 +218,63 @@ int ConfigureSlave(EcatConfig *config, SlaveConfig *slave, ec_slave_config_t *sc if one entry sm0, another sm1 and then again sm0, then problem */ - if (ss[rxpdo.sm].n_pdos == 0)ss[rxpdo.sm].pdos = rxp + i; + if (ss[rxpdo.sm].n_pdos == 0)ss[rxpdo.sm].pdos = rxp + rxpdoidx; ss[rxpdo.sm].n_pdos++; - if(RegisterRxInDomain(slave, &rxe[i], &rxpdo)){ - log_error("Failed to register RxPDO in domain"); - return -1; + + for (int entryidx = 0; entryidx < rxpdo.entrycount; entryidx++){ + rxe[rxentryidx].subindex = rxpdo.entries[entryidx].subindex; + rxe[rxentryidx].index = rxpdo.entries[entryidx].entryindex; + rxe[rxentryidx].bit_length = rxpdo.entries[entryidx].bitlen; + + if(RegisterRxInDomain(slave, &rxe[rxentryidx], &rxpdo, rxpdo.entries[entryidx].entryname)){ + log_error("Failed to register RxPDO in domain"); + return -1; + } + rxentryidx++; } } + int txentryidx = 0; // tx pdos - for (int i = 0;i < slave->TxPdo_count;i++){ - EcatPdo txpdo = slave->TxPDO[i]; - txe[i].subindex = i + 1; - txe[i].index = txpdo.entryindex; - txe[i].bit_length = txpdo.bitlen; - txp[i].entries = &txe[i]; - txp[i].index = txpdo.index; - txp[i].n_entries = 1; + for (int txpdoidx = 0;txpdoidx < slave->TxPdo_count;txpdoidx++){ + EcatPdo txpdo = slave->TxPDO[txpdoidx]; + txp[txpdoidx].entries = &txe[txentryidx]; + txp[txpdoidx].index = txpdo.index; + txp[txpdoidx].n_entries = txpdo.entrycount; ss[txpdo.sm].dir = EC_DIR_INPUT; - if (ss[txpdo.sm].n_pdos == 0)ss[txpdo.sm].pdos = txp + i; + + + if (ss[txpdo.sm].n_pdos == 0)ss[txpdo.sm].pdos = txp + txpdoidx; ss[txpdo.sm].n_pdos++; - if(RegisterTxInDomain(slave, &txe[i], &txpdo)){ - log_error("Failed to register TxPDO in domain"); - return -1; + + for (int entryidx = 0; entryidx < txpdo.entrycount; entryidx++){ + txe[txentryidx].subindex = txpdo.entries[entryidx].subindex; + txe[txentryidx].index = txpdo.entries[entryidx].entryindex; + txe[txentryidx].bit_length = txpdo.entries[entryidx].bitlen; + + if(RegisterTxInDomain(slave, &txe[txentryidx], &txpdo, txpdo.entries[entryidx].entryname)){ + log_error("Failed to register TxPDO in domain"); + return -1; + } + txentryidx++; } } - for (int i = 0; i < slave->sm_count;i++){ + for (int smidx = 0; smidx < slave->sm_count;smidx++){ char *dir = "[error invalid direction]"; - if (ss[i].dir == EC_DIR_INPUT)dir = "EC_DIR_INPUT"; - if (ss[i].dir == EC_DIR_OUTPUT)dir = "EC_DIR_OUTPUT"; - log_trace("Sync master %i %s", i, dir); - for (int j = 0; j < ss[i].n_pdos; j++){ - log_trace("PDO index %i/%i 0x%4X entryindex 0x%4X subindex %i bitlen %i", - j + 1, - ss[i].n_pdos, - ss[i].pdos[j].index, - ss[i].pdos[j].entries[0].index, - ss[i].pdos[j].entries[0].subindex, - ss[i].pdos[j].entries[0].bit_length); + if (ss[smidx].dir == EC_DIR_INPUT)dir = "EC_DIR_INPUT"; + if (ss[smidx].dir == EC_DIR_OUTPUT)dir = "EC_DIR_OUTPUT"; + log_trace("Sync master %i %s", smidx, dir); + for (int pdoidx = 0; pdoidx < ss[smidx].n_pdos; pdoidx++){ + for (int entryidx = 0; entryidx < ss[smidx].pdos[pdoidx].n_entries; entryidx++){ + log_trace("PDO index %i/%i 0x%4X entryindex 0x%4X subindex %i bitlen %i", + pdoidx + 1, + ss[smidx].n_pdos, + ss[smidx].pdos[pdoidx].index, + ss[smidx].pdos[pdoidx].entries[entryidx].index, + ss[smidx].pdos[pdoidx].entries[entryidx].subindex, + ss[smidx].pdos[pdoidx].entries[entryidx].bit_length); + } } } @@ -341,12 +393,9 @@ int EtherCATinit(EcatConfig *configin, logger_callback logger){ log_trace("Domain created"); // count registers - rxregistry_count = 0; - txregistry_count = 0; - for (int i = 0; i < config->slave_count; i++){ - rxregistry_count += config->slavesConfig[i].RxPdo_count; - txregistry_count += config->slavesConfig[i].TxPdo_count; - } + rxregistry_count = countRxRegisters(config); + txregistry_count = countTxRegisters(config); + domain1_regs = (ec_pdo_entry_reg_t*) malloc((rxregistry_count + txregistry_count) * sizeof(ec_pdo_entry_reg_t)); //static PdoRegistryEntry *TxPdoRegistry = NULL; RxPdoRegistry = (PdoRegistryEntry*) malloc(rxregistry_count * sizeof(PdoRegistryEntry)); @@ -650,7 +699,11 @@ void terminate_handler(){ free(tx_pdos[i]); } free(domain1_regs); + for(int i = 0; i < RxPdoRegistry_idx; i++) + free(RxPdoRegistry[i].pdoname); free(RxPdoRegistry); + for(int i = 0; i < TxPdoRegistry_idx; i++) + free(TxPdoRegistry[i].pdoname); free(TxPdoRegistry); free(sc_slaves); free(sc_salves_states); diff --git a/src/ecat_handler/ecat_handler.h b/src/ecat_handler/ecat_handler.h index 19aa13c..2d745d4 100644 --- a/src/ecat_handler/ecat_handler.h +++ b/src/ecat_handler/ecat_handler.h @@ -35,26 +35,38 @@ struct _EcatSm{ }; /** - * EcatPdo: + * EcatPdoEntry: * * Device recieve, PLC output */ -typedef struct _EcatPdo EcatPdo; -typedef EcatPdo *EcatPdoPtr; -struct _EcatPdo{ - char *pdotype; // RxPdo or TxPdo - uint16_t sm; // sm index - char *fixed; // fixed attr - char *mandatory; // mandatory attr - uint16_t index; // pdo index - char *name; // pdo name +typedef struct _EcatPdoEntry EcatPdoEntry; +typedef EcatPdoEntry *EcatPdoEntryPtr; +struct _EcatPdoEntry{ uint16_t entryindex; // entry index - char *subindex; // entry subindex + uint8_t subindex; // entry subindex uint8_t bitlen; // bit length, 1, 8, 16, 32 char *entryname; // entryname, arbritary, usually Input/Output, but can be descriptive char *datatype; // datatype, BOOL for bitlen 1, others... }; +/** + * EcatPdo: + * + * Device recieve, PLC output + */ +typedef struct _EcatPdo EcatPdo; +typedef EcatPdo *EcatPdoPtr; +struct _EcatPdo{ + char *pdotype; // RxPdo or TxPdo + uint16_t sm; // sm index + char *fixed; // fixed attr + char *mandatory; // mandatory attr + uint16_t index; // pdo index + char *name; // pdo name + uint16_t entrycount; // how many entries + struct _EcatPdoEntry *entries; // entires +}; + /** * Ethercat slave config data: * Slaves are addressed with the \a alias and \a position parameters. @@ -78,8 +90,10 @@ struct _SlaveConfig{ uint32_t sm_count; // number of specified Sync managers struct _EcatSm *Sm; // Sm collection uint32_t RxPdo_count; // number of specified RxPDO + uint32_t RxEntry_count; // number of specified Rx entries struct _EcatPdo *RxPDO; // RxPDO collection uint32_t TxPdo_count; // number of specified TxPDO + uint32_t TxEntry_count; // number of specified Tx entries struct _EcatPdo *TxPDO; // TxPDO collection }; diff --git a/src/xml_conf_parser/xml_conf_parser.c b/src/xml_conf_parser/xml_conf_parser.c index 99a8a5b..418dbf4 100644 --- a/src/xml_conf_parser/xml_conf_parser.c +++ b/src/xml_conf_parser/xml_conf_parser.c @@ -164,15 +164,27 @@ int ParsePdo(xmlNode *node, EcatPdo *pdo){ char *index = getNodeTextContent(getSingularnodeNamed(node->children, "Index")); pdo->index = (uint16_t)hexstrtoint32(index); pdo->name = strmcpy(getNodeTextContent(getSingularnodeNamed(node->children, "Name"))); - entry = getSingularnodeNamed(node->children, "Entry"); - char *entryindex = getNodeTextContent(getSingularnodeNamed(entry->children, "Index")); - pdo->entryindex = (uint16_t)hexstrtoint32(entryindex); - pdo->subindex = strmcpy(getNodeTextContent(getSingularnodeNamed(entry->children, "SubIndex"))); - char *bitlen = getNodeTextContent(getSingularnodeNamed(entry->children, "BitLen")); - pdo->bitlen = (uint8_t)strtol(bitlen, &ptr, 10); - pdo->entryname = strmcpy(getNodeTextContent(getSingularnodeNamed(entry->children, "Name"))); - pdo->datatype = strmcpy(getNodeTextContent(getSingularnodeNamed(entry->children, "DataType"))); - log_trace("%s %i %s %s 0x%X %s 0x%X %s %i %s %s", pdo->pdotype, pdo->sm, pdo->fixed, pdo->mandatory, pdo->index, pdo->name, pdo->entryindex, pdo->subindex, pdo->bitlen, pdo->entryname, pdo->datatype); + + pdo->entrycount = countNodesNamed(node->children, "Entry"); + pdo->entries = (EcatPdoEntry*) malloc(pdo->entrycount * sizeof(EcatPdoEntry)); + + log_trace("%s %i %s %s 0x%X %s", pdo->pdotype, pdo->sm, pdo->fixed, pdo->mandatory, pdo->index, pdo->name); + entry = getNextNodeNamed(node->children, "Entry"); + for (int i = 0; ientrycount; i++){ + EcatPdoEntry *pdoentry = &pdo->entries[i]; + + char *entryindex = getNodeTextContent(getSingularnodeNamed(entry->children, "Index")); + + pdoentry->entryindex = (uint16_t)hexstrtoint32(entryindex); + char *subindex = getNodeTextContent(getSingularnodeNamed(entry->children, "SubIndex")); + pdoentry->subindex = (uint8_t)strtol(subindex, &ptr, 10); + char *bitlen = getNodeTextContent(getSingularnodeNamed(entry->children, "BitLen")); + pdoentry->bitlen = (uint8_t)strtol(bitlen, &ptr, 10); + pdoentry->entryname = strmcpy(getNodeTextContent(getSingularnodeNamed(entry->children, "Name"))); + pdoentry->datatype = strmcpy(getNodeTextContent(getSingularnodeNamed(entry->children, "DataType"))); + entry = getNextNodeNamed(entry->next, "Entry"); + log_trace("%s 0x%X %i %i %s", pdoentry->entryname, pdoentry->entryindex, pdoentry->subindex, pdoentry->bitlen, pdoentry->datatype); + } return 0; } @@ -215,6 +227,8 @@ int ParseDescriptions(xmlNode *descriptions, SlaveConfig *config){ config->RxPdo_count = countNodesNamed(device->children, "RxPdo"); config->TxPdo_count = countNodesNamed(device->children, "TxPdo"); + config->RxEntry_count = 0; + config->TxEntry_count = 0; log_trace("RxPdos %i | TxPdos %i", config->RxPdo_count, config->TxPdo_count); EcatPdo *RxPdos, *TxPdos; @@ -228,12 +242,14 @@ int ParseDescriptions(xmlNode *descriptions, SlaveConfig *config){ for (int i = 0; i< config->RxPdo_count;i++){ start = getNextNodeNamed(start, "RxPdo"); ParsePdo(start, &RxPdos[i]); + config->RxEntry_count += RxPdos[i].entrycount; start = start->next; } start = device->children; for (int i = 0; i< config->TxPdo_count;i++){ start = getNextNodeNamed(start, "TxPdo"); ParsePdo(start, &TxPdos[i]); + config->TxEntry_count += TxPdos[i].entrycount; start = start->next; } @@ -321,14 +337,18 @@ int parse_xml_config(char *filename, EcatConfig *newconfig){ return 0; } +void terminate_entry(EcatPdoEntry *entry){ + free(entry->datatype); + free(entry->entryname); +} + void terminate_pdo(EcatPdo *pdo){ - free(pdo->datatype); - free(pdo->entryname); + for (int i = 0; i < pdo->entrycount; i++) terminate_entry(&pdo->entries[i]); + free(pdo->entries); free(pdo->fixed); free(pdo->mandatory); free(pdo->name); free(pdo->pdotype); - free(pdo->subindex); } void terminate_slaveconfig(SlaveConfig *s){