Skip to content

Commit

Permalink
Multientrydev (#7)
Browse files Browse the repository at this point in the history 
* refactored to enable multiple entires per PDO registration

* subindex fixed

* fix tx subindex

* entry / pdo index fixes

---------

Co-authored-by: Rainer Kordmaa <[email protected]>
  • Loading branch information
@r2k-in-the-vortex
r2k-in-the-vortex and Rainer Kordmaa authored Feb 22, 2024
1 parent ae67684 commit b328ae7
Show file tree
Hide file tree
Showing 5 changed files with 161 additions and 73 deletions.
2 changes: 1 addition & 1 deletion .vscode/launch.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
"cwd": "${fileDirname}",
"environment": [],
"externalConsole": false,
"miDebuggerPath": "/home/pi/gdb.sh",
//"miDebuggerPath": "/home/pi/gdb.sh",
"MIMode": "gdb",
"setupCommands": [
{
Expand Down
3 changes: 2 additions & 1 deletion .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}/.",
Expand Down
149 changes: 101 additions & 48 deletions src/ecat_handler/ecat_handler.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -120,14 +130,39 @@ 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++;
TxPdoRegistry_idx++;
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){
Expand All @@ -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));
Expand All @@ -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
Expand All @@ -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);
}
}
}

Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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);
Expand Down
36 changes: 25 additions & 11 deletions src/ecat_handler/ecat_handler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
};

Expand Down
44 changes: 32 additions & 12 deletions src/xml_conf_parser/xml_conf_parser.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; i<pdo->entrycount; 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;
}

Expand Down Expand Up @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -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){
Expand Down

0 comments on commit b328ae7

Please sign in to comment.