commit 0fb7af31c0826596e2e32dd116cba22504773c63 Author: Harald Wolff Date: Wed Dec 16 09:29:40 2020 +0100 Alpha Commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..542b474 --- /dev/null +++ b/.gitignore @@ -0,0 +1,46 @@ +# Autosave files +*~ + +# build +[Oo]bj/ +[Bb]in/ +packages/ +TestResults/ + +# globs +Makefile.in +*.DS_Store +*.sln.cache +*.suo +*.cache +*.pidb +*.userprefs +*.usertasks +config.log +config.make +config.status +aclocal.m4 +install-sh +autom4te.cache/ +*.user +*.tar.gz +tarballs/ +test-results/ +Thumbs.db +.vs/ + +# Mac bundle stuff +*.dmg +*.app + +# resharper +*_Resharper.* +*.Resharper + +# dotCover +*.dotCover + +*.log +*.log.old +.vscode +.build \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..24e77dd --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "contrib/SOEM"] + path = contrib/SOEM + url = https://git.l--n.de/ln-dotnet/SOEM.git diff --git a/build.ln b/build.ln new file mode 100644 index 0000000..0cf44ab --- /dev/null +++ b/build.ln @@ -0,0 +1,24 @@ +{ + "templates": [ + "dotnet" + ], + "env": { + "NUGET_SOURCE": "https://nexus.niclas-thobaben.de/repository/l--n.de/", + "CONFIGURATION": "Release" + }, + "stages": [ + { + "name": "prepare", + "commands": [ + "dotnet prepare */*.csproj" + ] + }, + { + "name": "native-build", + "priority": 200, + "commands": [ + "sh make -f contrib.soem.make all" + ] + } + ] +} \ No newline at end of file diff --git a/contrib.soem.make b/contrib.soem.make new file mode 100644 index 0000000..db8ba26 --- /dev/null +++ b/contrib.soem.make @@ -0,0 +1,28 @@ +CFLAGS=-fPIC -Icontrib/SOEM/soem -Icontrib/SOEM/osal -Icontrib/SOEM/osal/linux -Icontrib/SOEM/oshw/linux + +SRC_ECMBIND=$(wildcard libecmbind/*.c) +OBJ_ECMBIND=$(SRC_ECMBIND:libecmbind/%.c=.build/ecmbind/%.o) + + +all: DIR libecmbind.so + +clean: + rm -Rf .build/soem .build/ecmbind .build/soem-unpack + +DIR: + mkdir -p .build/ecmbind .build/soem .build/soem-unpack + +.build/soem/libsoem.a: contrib/SOEM/CMakeLists.txt + rm -Rf .build/soem/* + cd .build/soem; CFLAGS=-fPIC cmake ../../contrib/SOEM; make + cd .build/soem-unpack; ar x ../soem/libsoem.a + +libecmbind.so: $(OBJ_ECMBIND) .build/soem/libsoem.a + gcc -shared -o ln.ethercat/lib/libecmbind.so $(OBJ_ECMBIND) $(wildcard .build/soem-unpack/*.o) -lpthread + + + + + +.build/ecmbind/%.o: libecmbind/%.c + gcc -c -o $@ $< $(CFLAGS) diff --git a/contrib/SOEM b/contrib/SOEM new file mode 160000 index 0000000..342ca86 --- /dev/null +++ b/contrib/SOEM @@ -0,0 +1 @@ +Subproject commit 342ca8632c3a495ea9700cc2ea189ca20c12c3e2 diff --git a/libecmbind/ecd_map.c b/libecmbind/ecd_map.c new file mode 100644 index 0000000..195a2e0 --- /dev/null +++ b/libecmbind/ecd_map.c @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include + +#include "ecmbind.h" + + +extern char ecd_iomap[4096]; +extern int ecd_iomap_size; + +ecd_pdo_entry_t ecd_pdo_map[1024]; +int ecd_pdo_map_length; + + + + +/** Read PDO assign structure */ +int ecd_read_pdoassign(uint16 slave, uint16 PDOassign, int mapoffset, int bitoffset) +{ + ec_ODlistt ODlist; + ec_OElistt OElist; + + uint16 idxloop, nidx, subidxloop, rdat, idx, subidx; + uint8 subcnt; + int wkc, bsize = 0, rdl; + int32 rdat2; + uint8 bitlen, obj_subidx; + uint16 obj_idx; + int abs_offset, abs_bit; + + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign subindex 0 ( = number of PDO's) */ + wkc = ec_SDOread(slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + rdat = etohs(rdat); + /* positive result from slave ? */ + if ((wkc > 0) && (rdat > 0)) + { + /* number of available sub indexes */ + nidx = rdat; + bsize = 0; + /* read all PDO's */ + for (idxloop = 1; idxloop <= nidx; idxloop++) + { + rdl = sizeof(rdat); rdat = 0; + /* read PDO assign */ + wkc = ec_SDOread(slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM); + /* result is index of PDO */ + idx = etohs(rdat); + if (idx > 0) + { + rdl = sizeof(subcnt); subcnt = 0; + /* read number of subindexes of PDO */ + wkc = ec_SDOread(slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM); + subidx = subcnt; + /* for each subindex */ + for (subidxloop = 1; subidxloop <= subidx; subidxloop++) + { + rdl = sizeof(rdat2); rdat2 = 0; + /* read SDO that is mapped in PDO */ + wkc = ec_SDOread(slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM); + rdat2 = etohl(rdat2); + /* extract bitlength of SDO */ + bitlen = LO_BYTE(rdat2); + bsize += bitlen; + obj_idx = (uint16)(rdat2 >> 16); + obj_subidx = (uint8)((rdat2 >> 8) & 0x000000ff); + abs_offset = mapoffset + (bitoffset / 8); + abs_bit = bitoffset % 8; + ODlist.Slave = slave; + ODlist.Index[0] = obj_idx; + OElist.Entries = 0; + wkc = 0; + /* read object entry from dictionary if not a filler (0x0000:0x00) */ + if(obj_idx || obj_subidx) + wkc = ec_readOEsingle(0, obj_subidx, &ODlist, &OElist); + //printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen); + + ecd_pdo_map[ecd_pdo_map_length] = (ecd_pdo_entry_t){ + slave, + obj_idx, + obj_subidx, + abs_offset, + abs_bit, + bitlen, + 0, + "" + }; + + if((wkc > 0) && OElist.Entries) + { + ecd_pdo_map[ecd_pdo_map_length].type = OElist.DataType[obj_subidx]; + strncpy(ecd_pdo_map[ecd_pdo_map_length].name, OElist.Name[obj_subidx], 32); + + //printf(" %-12s %s\n", dtype2string(OElist.DataType[obj_subidx]), OElist.Name[obj_subidx]); + } +/* + printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X) %s\n", + ecd_pdo_map[ecd_pdo_map_length].addr_offset, + ecd_pdo_map[ecd_pdo_map_length].addr_bit, + ecd_pdo_map[ecd_pdo_map_length].index, + ecd_pdo_map[ecd_pdo_map_length].subindex, + ecd_pdo_map[ecd_pdo_map_length].bitlength, + ecd_pdo_map[ecd_pdo_map_length].name + + ); +*/ + ecd_pdo_map_length++; + + bitoffset += bitlen; + }; + }; + }; + }; + /* return total found bitlength (PDO) */ + return bsize; +} + + +int ecd_read_pdo_map(int slave) +{ + int wkc, rdl; + int retVal = -1; + uint8 nSM, iSM, tSM; + int Tsize, outputs_bo, inputs_bo; + uint8 SMt_bug_add; + + //printf("PDO mapping according to CoE :\n"); + SMt_bug_add = 0; + outputs_bo = 0; + inputs_bo = 0; + rdl = sizeof(nSM); nSM = 0; + /* read SyncManager Communication Type object count */ + wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM); + /* positive result from slave ? */ + if ((wkc > 0) && (nSM > 2)) + { + /* make nSM equal to number of defined SM */ + nSM--; + /* limit to maximum number of SM defined, if true the slave can't be configured */ + if (nSM > EC_MAXSM) + nSM = EC_MAXSM; + /* iterate for every SM type defined */ + for (iSM = 2 ; iSM <= nSM ; iSM++) + { + rdl = sizeof(tSM); tSM = 0; + /* read SyncManager Communication Type */ + wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM); + if (wkc > 0) + { + if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave! + { + SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4 + //printf("Activated SM type workaround, possible incorrect mapping.\n"); + } + if(tSM) + tSM += SMt_bug_add; // only add if SMt > 0 + + if (tSM == 3) // outputs + { + /* read the assign RXPDO */ + //printf(" SM%1d outputs\n addr b index: sub bitl data_type name\n", iSM); + Tsize = ecd_read_pdoassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].outputs - (uint8 *)&ecd_iomap[0]), outputs_bo ); + outputs_bo += Tsize; + } + if (tSM == 4) // inputs + { + /* read the assign TXPDO */ + //printf(" SM%1d inputs\n addr b index: sub bitl data_type name\n", iSM); + Tsize = ecd_read_pdoassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].inputs - (uint8 *)&ecd_iomap[0]), inputs_bo ); + inputs_bo += Tsize; + } + } + } + } + + /* found some I/O bits ? */ + if ((outputs_bo > 0) || (inputs_bo > 0)) + retVal = 0; + return retVal; +} + +int ecd_extract_map_value(int offset,int firstbit,int bitlength,char *buffer) +{ + int n; + int bytelength = bitlength ? (7 + firstbit + bitlength) / 8 : 0; + + memcpy(buffer, &ecd_iomap[offset], bytelength); +/* + fprintf(stderr, "IM: "); + print_hex(buffer, bytelength); +*/ + buffer[bytelength] = 0; + + if (firstbit) + { + for (n=0;n> firstbit) & (0xFFu >> firstbit) | (buffer[n+1] << (8-firstbit))); + } + buffer[n] = ((buffer[n] >> firstbit) & (0xFF >> firstbit)); + } + +/* fprintf(stderr, "\nFIN: "); + print_hex(buffer, bytelength); + fprintf(stderr, "\n"); + fflush(stderr); +*/ + return (bitlength); +} + +int ecd_insert_map_value(int offset,int firstbit,int bitlength,char *buffer) +{ + int n; + int bytelength = bitlength ? (7 + firstbit + bitlength) / 8 : 0; + + char* im = malloc(bytelength); + if (!im) + return -1; + + im[bytelength-1] = 0x00; + memcpy(im, buffer, bitlength / 8); + +/* fprintf(stderr, "IM: "); + print_hex(im, bytelength); +*/ + if (firstbit) + { + for (n=bytelength-1;n > 0;n++) + { + im[n-1] = ((im[n] << firstbit) | (im[n-1] >> (8-firstbit))); + } + im[n] = ((im[n] >> firstbit) & (0xFF >> firstbit)); + } +/* + fprintf(stderr, "\nFIN: "); + print_hex(im, bytelength); + fprintf(stderr, "\n"); + fflush(stderr); +*/ + if (firstbit) + { +/* fprintf(stderr, "FIXME: unaligned bits not supported yet!\n"); + fflush(stderr); +*/ + } else { +/* fprintf(stderr, "Inserting at %d = ", offset); + print_hex(im, bytelength); +*/ + memcpy(&ecd_iomap[offset], im, bytelength); + } + + return (bitlength); +} + + + diff --git a/libecmbind/ecmbind.h b/libecmbind/ecmbind.h new file mode 100644 index 0000000..77a8087 --- /dev/null +++ b/libecmbind/ecmbind.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +extern int TIMEOUT_PROCESSDATA; + +typedef struct +{ + int slave; + short index; + char subindex; + int addr_offset; + int addr_bit; + int bitlength; + int type; + char name[64]; +} ecd_pdo_entry_t; + +typedef struct { + int slave; + uint16_t index; + uint16_t datatype; + uint8_t objectcode; + uint8_t maxsub; + + char name[128]; +} dto_servicedescriptor_t; + +typedef void (*cb_enum_indeces_t)(int slave, int index); +typedef void (*cb_enum_sdo_descriptors_t)(int slave, int index, uint16_t dataType, uint16_t objectCode, int sub, char *name); +typedef void (*cb_enum_pdo)(uint16_t slave, uint16_t index, char subindex, int addr_offset, int addr_bit, int bitlength); + +extern ecd_pdo_entry_t ecd_pdo_map[1024]; +extern int ecd_pdo_map_length; + + +int ecd_read_pdo_map(int slave); + diff --git a/libecmbind/libecmbind.c b/libecmbind/libecmbind.c new file mode 100644 index 0000000..2ca46a2 --- /dev/null +++ b/libecmbind/libecmbind.c @@ -0,0 +1,220 @@ +#include +#include +#include + +#include "ecmbind.h" + +int TIMEOUT_PROCESSDATA = 2000; + +char ecd_iomap[4096]; +int ecd_iomap_size; +int ecd_expected_wkc_size; + +int ecmbind_version(char *versionString) +{ + strcpy(versionString, "ecmbind/0.1.0"); + return 0x00010000; +} + +int ecmbind_initialize(char *ifname) +{ + return ec_init(ifname); +} + +int ecmbind_config_init() +{ + ec_config_init(FALSE); + return ec_slavecount; +} + +void* ecmbind_get_iomap() { return ecd_iomap; } +int ecmbind_get_expected_wkc_size() { return ecd_expected_wkc_size; } + +int ecmbind_config_map() +{ + ecd_pdo_map_length = 0; + + ecd_iomap_size = ec_config_map(&ecd_iomap); + //ec_configdc(); + ecd_expected_wkc_size = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + + for (int slave=1;slave<=ec_slavecount;slave++) + ecd_read_pdo_map(slave); + + return ecd_iomap_size; +} + +uint16 ecmbind_read_state() +{ + return ec_readstate(); +} + +uint16 ecmbind_get_slave_state(int slave) +{ + return ec_slave[slave].state; +} + + +int ecmbind_write_slave_state(int slave, uint16 state) +{ + ec_slave[slave].state = state; + return ec_writestate(slave); +} + +uint16 ecmbind_request_state(int slave, uint16 reqState, int timeout) +{ + return ec_statecheck(slave, reqState, timeout * 1000); +} + +int ecmbind_processdata() +{ + ec_send_processdata(); + int wkc = ec_receive_processdata(EC_TIMEOUTRET); + return wkc; +} + +int ecmbind_processdata2(char *managed_iomap, int size) +{ + if (size != ecd_iomap_size) + return -1; + + memcpy( ecd_iomap, managed_iomap, ecd_iomap_size); + + ec_send_processdata(); + int wkc = ec_receive_processdata(EC_TIMEOUTRET); + + memcpy( managed_iomap, ecd_iomap, ecd_iomap_size); + + return wkc; +} + +int ecmbind_recover() +{ + int slave = 0; + ec_group[0].docheckstate = FALSE; + ec_readstate(); + for (slave = 1; slave <= ec_slavecount; slave++) + { + if ((ec_slave[slave].group == 0) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) + { + ec_group[0].docheckstate = TRUE; + if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) + { + printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); + ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); + ec_writestate(slave); + } + else if(ec_slave[slave].state == EC_STATE_SAFE_OP) + { + printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); + ec_slave[slave].state = EC_STATE_OPERATIONAL; + ec_writestate(slave); + } + else if(ec_slave[slave].state > EC_STATE_NONE) + { + if (ec_reconfig_slave(slave, TIMEOUT_PROCESSDATA)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d reconfigured\n",slave); + } + } + else if(!ec_slave[slave].islost) + { + /* re-check state */ + ec_statecheck(slave, EC_STATE_OPERATIONAL, TIMEOUT_PROCESSDATA); + if (ec_slave[slave].state == EC_STATE_NONE) + { + ec_slave[slave].islost = TRUE; + printf("ERROR : slave %d lost\n",slave); + } + } + } + + if (ec_slave[slave].islost) + { + if(ec_slave[slave].state == EC_STATE_NONE) + { + if (ec_recover_slave(slave, TIMEOUT_PROCESSDATA)) + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d recovered\n",slave); + } + } + else + { + ec_slave[slave].islost = FALSE; + printf("MESSAGE : slave %d found\n",slave); + } + } + } + + if(!ec_group[0].docheckstate) + { + printf("OK : all slaves resumed OPERATIONAL.\n"); + ecd_expected_wkc_size = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; + return ecd_expected_wkc_size; + } + return -1; +} + +int ecmbind_get_pdo_entries_length() { return ecd_pdo_map_length; } +int ecmbind_get_pdo_entries(ecd_pdo_entry_t* table, int length) +{ + if (length < ecd_pdo_map_length) + return -1; + + int tableSize = (sizeof(ecd_pdo_entry_t)* ecd_pdo_map_length); + memcpy(table, ecd_pdo_map, tableSize); + + return ecd_pdo_map_length; +} + +int ecmbind_pdo_read(int slave, int index, int subindex, char* buffer, int size) +{ + return -1; +} +int ecmbind_pdo_write(int slave, int index, int subindex, char* buffer, int size) +{ + return -1; +} + +int ecmbind_sdo_read(int slave, int index, int subindex, char* buffer, int size) +{ + int wkc = ec_SDOread(slave, index, subindex, FALSE, &size, buffer, 100000); + if (wkc <= 0) + return -1; + return size; +} +int ecmbind_sdo_write(int slave, int index, int subindex, char* buffer, int size) +{ + return -1; +} + +/* +typedef void (*cb_enum_pdo)(int slave, int index, int subindex, int addr_offset, int addr_bit, int bitlength); +*/ +int ecmbind_pdo_enumerate(cb_enum_pdo cb) +{ + printf("libecmbind: ecmbind_pdo_enumerate(): enumerating %d PDOs\n", ecd_pdo_map_length); + for (int n=0;n ecd_iomap_size) + return -1; + memcpy(buffer, &ecd_iomap[offset], length); + return length; +} +int ecmbind_iomap_set(int offset, char* buffer, int length) +{ + if (offset + length > ecd_iomap_size) + return -1; + + memcpy(&ecd_iomap[offset], buffer, length); +} + + diff --git a/libecmbind/libecmbind_descriptors.c b/libecmbind/libecmbind_descriptors.c new file mode 100644 index 0000000..34a0ca7 --- /dev/null +++ b/libecmbind/libecmbind_descriptors.c @@ -0,0 +1,295 @@ +#include +#include +#include + +#include +#include "ecmbind.h" + +/** SDO service structure */ +PACKED_BEGIN +typedef struct PACKED +{ + ec_mbxheadert MbxHeader; + uint16 CANOpen; + uint8 Opcode; + uint8 Reserved; + uint16 Fragments; + union + { + uint8 bdata[0x200]; /* variants for easy data access */ + uint16 wdata[0x100]; + uint32 ldata[0x80]; + }; +} ec_SDOservicet; +PACKED_END + + + +static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode) +{ + ec_errort Ec; + + memset(&Ec, 0, sizeof(Ec)); + Ec.Slave = Slave; + Ec.Index = Index; + Ec.SubIdx = SubIdx; + *(context->ecaterror) = TRUE; + Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR; + Ec.AbortCode = AbortCode; + ecx_pusherror(context, &Ec); +} + + +int ecmbind_enumerate_servicedescriptors(int Slave, cb_enum_indeces_t cb) +{ + ecx_contextt *context = &ecx_context; + ec_SDOservicet *SDOp, *aSDOp; + ec_mbxbuft MbxIn, MbxOut; + int wkc; + uint16 x, n, i, sp, offset; + boolean stop; + uint8 cnt; + boolean First; + + if (cb == NULL) + return -1; + + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x0008); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_ODLIST_REQ; /* get object description list request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(0x01); /* all objects */ + /* send get object description list request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + x = 0; + sp = 0; + First = TRUE; + offset = 1; /* offset to skip info header in first frame, otherwise set to 0 */ + do + { + stop = TRUE; /* assume this is last iteration */ + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + /* response should be CoE and "get object description list response" */ + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES)) + { + if (First) + { + /* extract number of indexes from mailbox data size */ + n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2; + } + else + { + /* extract number of indexes from mailbox data size */ + n = (etohs(aSDOp->MbxHeader.length) - 6) / 2; + } + + /* extract indexes one by one */ + for (i = 0; i < n; i++) + cb(Slave, etohs(aSDOp->wdata[i + offset])); + + sp += n; + /* check if more fragments will follow */ + if (aSDOp->Fragments > 0) + { + stop = FALSE; + } + First = FALSE; + offset = 0; + } + /* got unexpected response from slave */ + else + { + if ((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0])); + stop = TRUE; + } + else + { + ecx_packeterror(context, Slave, 0, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + x += 20; + } + } + x++; + } + while ((x <= 128) && !stop); + } + return wkc; +} + +int ecmbind_read_objectdescription(int Slave, int index, cb_enum_sdo_descriptors_t cb) +{ + ecx_contextt *context = &ecx_context; + ec_SDOservicet *SDOp, *aSDOp; + int wkc; + uint16 n; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + char temp[128]; + + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x0008); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt); + context->slavelist[Slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_OD_REQ; /* get object description request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(index); /* Data of Index */ + /* send get object description request to slave */ + wkc = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, Slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES)) + { + n = (etohs(aSDOp->MbxHeader.length) - 12); /* length of string(name of object) */ + if (n > sizeof(temp)-1) + n = sizeof(temp)-1; + strncpy(temp , (char *)&aSDOp->bdata[6], n); + temp[n] = 0x00; + + cb(Slave, index, etohs(aSDOp->wdata[1]), aSDOp->bdata[5], aSDOp->bdata[4], temp); + } + /* got unexpected response from slave */ + else + { + if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ + { + ecx_SDOinfoerror(context, Slave, index, 0, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, Slave, index, 0, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} + +int ecmbind_read_objectdescription_entry(uint16_t slave, uint16_t index, uint16_t sub, cb_enum_sdo_descriptors_t cb) +{ + ecx_contextt *context = &ecx_context; + ec_SDOservicet *SDOp, *aSDOp; + int wkc; + int16 n; + ec_mbxbuft MbxIn, MbxOut; + uint8 cnt; + char temp[128]; + + wkc = 0; + ec_clearmbx(&MbxIn); + /* clear pending out mailbox in slave if available. Timeout is set to 0 */ + wkc = ecx_mbxreceive(context, slave, &MbxIn, 0); + ec_clearmbx(&MbxOut); + aSDOp = (ec_SDOservicet*)&MbxIn; + SDOp = (ec_SDOservicet*)&MbxOut; + SDOp->MbxHeader.length = htoes(0x000a); + SDOp->MbxHeader.address = htoes(0x0000); + SDOp->MbxHeader.priority = 0x00; + /* Get new mailbox counter value */ + cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); + context->slavelist[slave].mbx_cnt = cnt; + SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */ + SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */ + SDOp->Opcode = ECT_GET_OE_REQ; /* get object entry description request */ + SDOp->Reserved = 0; + SDOp->Fragments = 0; /* fragments left */ + SDOp->wdata[0] = htoes(index); /* index */ + SDOp->bdata[2] = sub; /* Subindex */ + SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */ + /* send get object entry description request to slave */ + wkc = ecx_mbxsend(context, slave, &MbxOut, EC_TIMEOUTTXM); + /* mailbox placed in slave ? */ + if (wkc > 0) + { + ec_clearmbx(&MbxIn); + /* read slave response */ + wkc = ecx_mbxreceive(context, slave, &MbxIn, EC_TIMEOUTRXM); + /* got response ? */ + if (wkc > 0) + { + if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && + ((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES)) + { + n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */ + if (n > sizeof(temp)-1) + n = sizeof(temp)-1; + + if (n < 0 ) + n = 0; + + strncpy(temp , (char *)&aSDOp->wdata[5], n); + temp[n] = 0x00; /* string terminator */ + + cb(slave, index, (ushort)etohs(aSDOp->wdata[2]), 0, sub, temp); + +/* + pOElist->ValueInfo[sub] = aSDOp->bdata[3]; + pOElist->DataType[sub] = etohs(aSDOp->wdata[2]); + pOElist->BitLength[sub] = etohs(aSDOp->wdata[3]); + pOElist->ObjAccess[sub] = etohs(aSDOp->wdata[4]); +*/ + } + + /* got unexpected response from slave */ + else + { + if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */ + { + ecx_SDOinfoerror(context, slave, index, sub, etohl(aSDOp->ldata[0])); + } + else + { + ecx_packeterror(context, slave, index, sub, 1); /* Unexpected frame returned */ + } + wkc = 0; + } + } + } + + return wkc; +} diff --git a/ln.ethercat.service/EthercatService.cs b/ln.ethercat.service/EthercatService.cs new file mode 100644 index 0000000..9f17db9 --- /dev/null +++ b/ln.ethercat.service/EthercatService.cs @@ -0,0 +1,61 @@ + + +using System.Threading; +using ln.ethercat.service.api.v1; +using ln.http; +using ln.http.router; +using ln.logging; +using ln.type; + +namespace ln.ethercat.service +{ + public class EthercatService + { + + public ECMaster ECMaster { get; } + + + HTTPServer httpServer; + LoggingRouter httpLoggingRouter; + SimpleRouter httpRouter; + + EthercatApiController apiController; + + + public EthercatService(string interfaceName) + { + ECMaster = new ECMaster(interfaceName); + + Initialize(); + } + + void Initialize() + { + httpRouter = new SimpleRouter(); + httpLoggingRouter = new LoggingRouter(httpRouter); + + apiController = new EthercatApiController(ECMaster); + httpRouter.AddSimpleRoute("/api/v1/*", apiController); + + httpServer = new HTTPServer(httpLoggingRouter); + httpServer.AddEndpoint(new Endpoint(IPv6.ANY, 7676)); + + } + + + public void Start() + { + httpServer.Start(); + + ECMaster.Start(); + + while (ECMaster.MasterState != ECMasterState.RUNNING) + Thread.Sleep(100); + + Logging.Log(LogLevel.INFO,"ECMaster is ready (ExpectedWorkCounter={0})", ECMaster.ExpectedWorkCounter); + } + + + + } +} \ No newline at end of file diff --git a/ln.ethercat.service/Program.cs b/ln.ethercat.service/Program.cs new file mode 100644 index 0000000..b787dab --- /dev/null +++ b/ln.ethercat.service/Program.cs @@ -0,0 +1,42 @@ +using System; +using System.Text; +using System.Threading; +using ln.logging; +using ln.type; + +namespace ln.ethercat.service +{ + class Program + { + static void Main(string[] args) + { + Logging.Log(LogLevel.INFO, ".NET EtherCAT service host"); + + StringBuilder versionString = new StringBuilder(1024); + ECMBind.ecmbind_version(versionString); + + Logging.Log(LogLevel.INFO, "ECMBind version: {0}", versionString.ToString()); + + EthercatService ethercatService = new EthercatService(args[0]); + ethercatService.Start(); + +/* + while (true) + { + Thread.Sleep(100); + for (int n=1;n <= ecMaster.CountSlaves;n++) + { + //Logging.Log(LogLevel.DEBUG, "Slave {0} is in state {1}", n, ecMaster.ReadSlaveState(n)); + if (ecMaster.ReadSDO(new DOAddr(n, 0x1000),out byte[] typeBytes)) + { + Logging.Log(LogLevel.DEBUG, "Slave {0} has type {1}", n, typeBytes.ToHexString()); + } else { + Logging.Log(LogLevel.DEBUG, "ReadSDO() failed"); + } + } + } + +*/ + } + } +} diff --git a/ln.ethercat.service/api/v1/ControllerApiController.cs b/ln.ethercat.service/api/v1/ControllerApiController.cs new file mode 100644 index 0000000..7b038b3 --- /dev/null +++ b/ln.ethercat.service/api/v1/ControllerApiController.cs @@ -0,0 +1,15 @@ + + +using ln.http.api; + +namespace ln.ethercat.service.api.v1 +{ + + public class ControllerApiController : WebApiController + { + + + + } + +} \ No newline at end of file diff --git a/ln.ethercat.service/api/v1/EthercatApiController.cs b/ln.ethercat.service/api/v1/EthercatApiController.cs new file mode 100644 index 0000000..66b1ad7 --- /dev/null +++ b/ln.ethercat.service/api/v1/EthercatApiController.cs @@ -0,0 +1,104 @@ +using System.Linq; +using System.Runtime.CompilerServices; +using ln.http; +using ln.http.api; +using ln.http.api.attributes; +using ln.json; +using ln.json.mapping; +using ln.logging; + +namespace ln.ethercat.service.api.v1 +{ + + public class EthercatApiController : WebApiController + { + ECMaster ECMaster; + public EthercatApiController(ECMaster ecMaster) + { + ECMaster = ecMaster; + } + + [GET("/slaves")] + public int GetSlaveCount() => ECMaster.CountSlaves; + + [GET("/slaves/:slave/sdo/:index/:subindex")] + [GET("/slaves/:slave/sdo/:index")] + public HttpResponse ReadSDO(int slave,int index,int subindex = 0) + { + if (ECMaster.GetSDO(new SDOAddr(slave, index, subindex), out SDO sdo)) + { + Logging.Log(LogLevel.DEBUG, "SDO updated: {0}", sdo); + return HttpResponse + .OK() + .Content(JSONMapper.DefaultMapper.ToJson(sdo)) + .ContentType("application/json") + ; + } + return HttpResponse.RequestTimeout().Content("master could not read from slave"); + } + + [GET("/slaves/:slave/sdo")] + public SDO[] GetServiceDescriptors(int slave) + { + return ECMaster.GetSDOs(slave).ToArray(); + } + + [GET("/slaves/:slave/state")] + public HttpResponse GetEthercatState(int slave) => HttpResponse.OK().Content(ECMaster.ReadSlaveState(slave).ToString()); + + [POST("/slaves/:slave/state")] + public HttpResponse RequestEthercatState(int slave, ECSlaveState state) + { + if (ECMaster.RequestSlaveState(slave, state) > 0) + return HttpResponse.NoContent(); + return HttpResponse.GatewayTimeout(); + } + + + [GET("/master/state")] + public HttpResponse GetFullState() + { + JSONArray slaveStates = new JSONArray(); + + for (int slave=1; slave <= ECMaster.CountSlaves; slave++) + { + slaveStates.Add(new JSONObject() + .Add("id", new JSONNumber(slave)) + .Add("state", new JSONString(ECMaster.ReadSlaveState(slave).ToString())) + ); + } + + JSONObject fullState = new JSONObject() + .Add("bus_state", new JSONString(ECMaster.EthercatState.ToString())) + .Add("slaves", slaveStates) + ; + + return HttpResponse.OK().Content(fullState); + } + + [GET("/master/pdomap")] + public PDO[] GetPDOMap() => ECMaster.GetPDOMap(); + + [GET("/master/pdos")] + public SDO[] GetPDOs() => ECMaster.GetPDOMap().Select((pdo)=>pdo.SDO).ToArray(); + + [POST("/master/action")] + public HttpResponse PostMasterAction(string action) + { + switch (action) + { + case "stop": + ECMaster.Stop(); + return HttpResponse.NoContent(); + case "start": + ECMaster.Start(); + return HttpResponse.NoContent(); + default: + return HttpResponse.BadRequest().Content(string.Format("{0} is no valid action", action)); + } + } + + + } + +} \ No newline at end of file diff --git a/ln.ethercat.service/ln.ethercat.service.csproj b/ln.ethercat.service/ln.ethercat.service.csproj new file mode 100644 index 0000000..14892f2 --- /dev/null +++ b/ln.ethercat.service/ln.ethercat.service.csproj @@ -0,0 +1,18 @@ + + + + Exe + netcoreapp3.1 + + + + + + + + + + + + + diff --git a/ln.ethercat.sln b/ln.ethercat.sln new file mode 100644 index 0000000..d37ec3a --- /dev/null +++ b/ln.ethercat.sln @@ -0,0 +1,62 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 15 +VisualStudioVersion = 15.0.26124.0 +MinimumVisualStudioVersion = 15.0.26124.0 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ln.ethercat", "ln.ethercat\ln.ethercat.csproj", "{CA065CFA-9690-4276-8234-A82912ABF7FD}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ln.ethercat.tests", "ln.ethercat.tests\ln.ethercat.tests.csproj", "{3E509EF8-48F8-4C25-8E9A-B5BDE025A422}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ln.ethercat.service", "ln.ethercat.service\ln.ethercat.service.csproj", "{6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Debug|Any CPU.Build.0 = Debug|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Debug|x64.ActiveCfg = Debug|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Debug|x64.Build.0 = Debug|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Debug|x86.ActiveCfg = Debug|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Debug|x86.Build.0 = Debug|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Release|Any CPU.ActiveCfg = Release|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Release|Any CPU.Build.0 = Release|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Release|x64.ActiveCfg = Release|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Release|x64.Build.0 = Release|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Release|x86.ActiveCfg = Release|Any CPU + {CA065CFA-9690-4276-8234-A82912ABF7FD}.Release|x86.Build.0 = Release|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Debug|Any CPU.Build.0 = Debug|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Debug|x64.ActiveCfg = Debug|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Debug|x64.Build.0 = Debug|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Debug|x86.ActiveCfg = Debug|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Debug|x86.Build.0 = Debug|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Release|Any CPU.ActiveCfg = Release|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Release|Any CPU.Build.0 = Release|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Release|x64.ActiveCfg = Release|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Release|x64.Build.0 = Release|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Release|x86.ActiveCfg = Release|Any CPU + {3E509EF8-48F8-4C25-8E9A-B5BDE025A422}.Release|x86.Build.0 = Release|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Debug|Any CPU.Build.0 = Debug|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Debug|x64.ActiveCfg = Debug|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Debug|x64.Build.0 = Debug|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Debug|x86.ActiveCfg = Debug|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Debug|x86.Build.0 = Debug|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Release|Any CPU.ActiveCfg = Release|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Release|Any CPU.Build.0 = Release|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Release|x64.ActiveCfg = Release|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Release|x64.Build.0 = Release|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Release|x86.ActiveCfg = Release|Any CPU + {6F8D4B47-2ECB-416A-85DB-E84B6BC295B1}.Release|x86.Build.0 = Release|Any CPU + EndGlobalSection +EndGlobal diff --git a/ln.ethercat.tests/UnitTest1.cs b/ln.ethercat.tests/UnitTest1.cs new file mode 100644 index 0000000..b8aff6e --- /dev/null +++ b/ln.ethercat.tests/UnitTest1.cs @@ -0,0 +1,24 @@ +using System; +using System.Reflection; +using System.Text; +using NUnit.Framework; + +namespace ln.ethercat.tests +{ + public class Tests + { + [SetUp] + public void Setup() + { + } + + [Test] + public void Test1() + { + StringBuilder versionString = new StringBuilder(1024); + Assert.AreEqual(0x00010000, ECMBind.ecmbind_version(versionString)); + Console.WriteLine("ECMBind Version: {0}", versionString.ToString()); + Assert.Pass(); + } + } +} \ No newline at end of file diff --git a/ln.ethercat.tests/ln.ethercat.tests.csproj b/ln.ethercat.tests/ln.ethercat.tests.csproj new file mode 100644 index 0000000..f687b1f --- /dev/null +++ b/ln.ethercat.tests/ln.ethercat.tests.csproj @@ -0,0 +1,16 @@ + + + + netcoreapp3.1 + + false + + + + + + + + + + diff --git a/ln.ethercat/ECDataTypeConverter.cs b/ln.ethercat/ECDataTypeConverter.cs new file mode 100644 index 0000000..5e9af7f --- /dev/null +++ b/ln.ethercat/ECDataTypeConverter.cs @@ -0,0 +1,42 @@ +using System; +using System.Text; +using ln.type; + +namespace ln.ethercat +{ + public static class ECDataTypeConverter + { + public static object ConvertFromEthercat(ECDataTypes dataType, byte[] rawData) + { + if (rawData == null) + return null; + + int offset = 0; + + switch (dataType) + { + case ECDataTypes.NONE: + return null; + case ECDataTypes.REAL: + return rawData.GetSingle(ref offset, Endianess.BIG); + case ECDataTypes.LREAL: + return rawData.GetDouble(ref offset, Endianess.BIG); + case ECDataTypes.STRING: + return Encoding.ASCII.GetString(rawData); + case ECDataTypes.INT: + return rawData.GetShort(ref offset, Endianess.BIG); + case ECDataTypes.DINT: + return rawData.GetInt(ref offset, Endianess.BIG); + case ECDataTypes.UINT: + return rawData.GetUShort(ref offset, Endianess.BIG); + case ECDataTypes.UDINT: + return rawData.GetUInt(ref offset, Endianess.BIG); + case ECDataTypes.USINT: + return rawData[offset]; + default: + return rawData; + } + } + + } +} \ No newline at end of file diff --git a/ln.ethercat/ECDataTypes.cs b/ln.ethercat/ECDataTypes.cs new file mode 100644 index 0000000..5b48983 --- /dev/null +++ b/ln.ethercat/ECDataTypes.cs @@ -0,0 +1,34 @@ +namespace ln.ethercat +{ + public enum ECDataTypes : ushort + { + NONE = 0, + BOOL = 0x0001, BIT = 0x0001, + SINT = 0x0002, + INT = 0x0003, + DINT = 0x0004, + USINT = 0x0005, + UINT = 0x0006, + UDINT = 0x0007, + REAL = 0x0008, + VISIBLE_STRING = 0x0009, + STRING = 0x0009, + OCTET_STRING = 0x000A, + ARRAY_OF_BYTE = 0x000A, + UNICODE_STRING = 0x000B, + TIME_OF_DAY = 0x000C, + TIME_DIFFERENCE = 0x000D, + DOMAIN = 0x000F, + INTEGER24 = 0x0010, + UNSIGNED40 = 0x0018, + UNSIGNED48 = 0x0019, + UNSIGNED56 = 0x001A, + UNSIGNED64 = 0x001B, + + ARRAY_OF_UINT = 0x001F, + LREAL = 0x0020, + LINT = 0x0260, + ULINT = 0x0261, + BYTE = 0x0262 + } +} diff --git a/ln.ethercat/ECMBind.cs b/ln.ethercat/ECMBind.cs new file mode 100644 index 0000000..7d717c9 --- /dev/null +++ b/ln.ethercat/ECMBind.cs @@ -0,0 +1,126 @@ +using System; +using System.Runtime.InteropServices; +using System.Text; + +namespace ln.ethercat +{ + + + [StructLayout(LayoutKind.Sequential)] + public struct PDOEntry + { + public int slave; + public short index; + public byte subindex; + public int addr_offset; + public int addr_bit; + public int bitlength; + public int type; + + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 64)] + public StringBuilder name; + + + public override string ToString() + { + return string.Format("PDOEntry(slave={0},index={1},subindex={2},addr_offset={3},addr_bit={4},bitlength={5},type={6})", slave,index,subindex,addr_offset,addr_bit,bitlength,type); + } + } + + [StructLayout(LayoutKind.Sequential)] + public struct dto_servicedescriptor + { + public int slave; + public UInt16 index; + public UInt16 datatype; + public UInt16 objectcode; + public byte maxsub; + + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 128)] + public StringBuilder name; + } + + public delegate void cb_enum_indeces(int slave, int index); + public delegate void cb_enum_sdo_descriptors(int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name); + public delegate void cb_enum_pdo(UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength); + + + public static class ECMBind + { + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_version(StringBuilder versionString); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_initialize(String ifname); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_config_init(); + + [DllImport("lib/libecmbind.so")] + public static extern IntPtr ecmbind_get_iomap(); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_get_expected_wkc_size(); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_config_map(); + + [DllImport("lib/libecmbind.so")] + public static extern ECSlaveState ecmbind_read_state(); + + [DllImport("lib/libecmbind.so")] + public static extern ECSlaveState ecmbind_get_slave_state(int slave); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_write_slave_state(int slave, ECSlaveState state); + + [DllImport("lib/libecmbind.so")] + public static extern ECSlaveState ecmbind_request_state(int slave, ECSlaveState reqState, int timeout); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_processdata(); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_recover(); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecd_read_pdo_map(Int32 slave); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_get_pdo_entries_length(); + + [DllImport("lib/libecmbind.so")] + public static extern Int32 ecmbind_get_pdo_entries(ref PDOEntry[] table, int length); + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_pdo_read(int slave, int index, int subindex, byte[] buffer, int size); + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_pdo_write(int slave, int index, int subindex, byte[] buffer, int size); + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_sdo_read(int slave, int index, int subindex, byte[] buffer, int size); + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_sdo_write(int slave, int index, int subindex, byte[] buffer, int size); + + + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_enumerate_servicedescriptors(int Slave, cb_enum_indeces cb); + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_read_objectdescription(int slave, int index, cb_enum_sdo_descriptors cb); + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_read_objectdescription_entry(UInt16 slave, UInt16 index, UInt16 sub, cb_enum_sdo_descriptors cb); + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_pdo_enumerate(cb_enum_pdo cb); + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_iomap_get(int offset, byte[] buffer, int length); + + [DllImport("lib/libecmbind.so")] + public static extern int ecmbind_iomap_set(int offset, byte[] buffer, int length); + + } +} diff --git a/ln.ethercat/ECMaster.cs b/ln.ethercat/ECMaster.cs new file mode 100644 index 0000000..4cdccbf --- /dev/null +++ b/ln.ethercat/ECMaster.cs @@ -0,0 +1,379 @@ + + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Reflection.Metadata; +using System.Security.Cryptography.X509Certificates; +using System.Threading; +using ln.logging; +using ln.type; + +namespace ln.ethercat +{ + + public enum ECMasterState { + INITIALIZED, + STARTING, + STOPPING, + RUNNING, + } + + public delegate void ECStateChange(ECMaster sender,ECSlaveState newState); + + public class ECMaster + { + public int TIMEOUT_PREOP = 3000; + public int TIMEOUT_SAFEOP = 10000; + public int TIMEOUT_BACKTO_SAFEOP = 200; + public int INTERVALL_PROCESSDATA = 20; + + + public event ECStateChange OnStateChange; + + public string InterfaceName { get; } + + public int ExpectedWorkCounter { get; private set; } + + public int CountSlaves { get; private set; } + + public IntPtr IOMapPtr { get; private set; } + public int IOMapSize { get; private set; } + + public ECMasterState MasterState { get; private set; } = ECMasterState.INITIALIZED; + + ECSlaveState ethercatState = ECSlaveState.NONE; + public ECSlaveState EthercatState { + get => UpdateEthercatState(); + set { + if (value != ethercatState) + { + OnStateChange?.Invoke(this, value); + ethercatState = value; + Logging.Log(LogLevel.DEBUG, "ECMaster: EthercatState is now {0}", ethercatState); + } + } + + } + + + Dictionary pdoMap = new Dictionary(); + SDOCache sdoCache; + + public ECMaster(string interfaceName) + { + InterfaceName = interfaceName; + + int result = ECMBind.ecmbind_initialize(interfaceName); + Logging.Log(LogLevel.INFO, "ecmbind_initialize({0}) = {1}", interfaceName, result); + if (result<=0) + throw new Exception("ecmbind_initialize failed"); + + sdoCache = new SDOCache(this); + } + + bool stopProcessing; + public bool StopProccessing { + get => stopProcessing; + set => stopProcessing = value; + } + + Thread threadProcessData; + + public bool Start() + { + if (threadProcessData?.IsAlive ?? false) + throw new Exception("already started"); + + EthercatState = ECSlaveState.BOOT; + ExpectedWorkCounter = 0; + + EthercatState = ECSlaveState.INIT; + lock (this) + { + CountSlaves = ECMBind.ecmbind_config_init(); + if (CountSlaves <= 0) + { + Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected"); + return false; + } + + if (!CheckState(ECSlaveState.PRE_OP, TIMEOUT_PREOP)) + return false; + + IOMapSize = ECMBind.ecmbind_config_map(); + Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize); + + UpdatePDOMap(); + + threadProcessData = new Thread(MasterThread); + threadProcessData.Start(); + + if (!RequestState(ECSlaveState.SAFE_OP, out ECSlaveState slaveState, TIMEOUT_SAFEOP)) + { + Stop(); + return false; + } + + if (!ReadSDOIndeces()) + { + Logging.Log(LogLevel.WARNING, "ECMaster: could not read SDO indeces"); + } + +/* + if (!RequestState(ECSlaveState.OPERATIONAL, out slaveState, TIMEOUT_SAFEOP)) + { + if (slaveState < ECSlaveState.SAFE_OP) + Stop(); + return false; + } +*/ + } + return true; + } + + public void Stop() + { + if (threadProcessData?.IsAlive ?? false) + { + stopProcessing = true; + ECMBind.ecmbind_request_state(0, ECSlaveState.SAFE_OP, TIMEOUT_BACKTO_SAFEOP); + + threadProcessData.Join(); + threadProcessData = null; + } + } + + + object lockIOMap = new object(); + + void MasterThread() + { + ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size(); + + while (!stopProcessing) + { + int wkc; + + lock (lockIOMap) + { + wkc = ECMBind.ecmbind_processdata(); + } + Thread.Sleep(INTERVALL_PROCESSDATA); + +/* + if (wkc != ExpectedWorkCounter) + { + ExpectedWorkCounter = ECMBind.ecmbind_recover(); + if (ExpectedWorkCounter < 0) + break; + } +*/ + + } + } + + public bool ReadSDO(SDOAddr addr,out byte[] data) + { + byte[] buffer = new byte[128]; + int size; + lock (lockIOMap) + size = ECMBind.ecmbind_sdo_read(addr.Slave,addr.Index,addr.SubIndex, buffer, buffer.Length); + + if (size < 0) + { + data = new byte[0]; + return false; + } + data = buffer.Slice(0, size); + return true; + } + public bool WriteSDO(SDOAddr addr,byte[] data) + { + throw new NotImplementedException(); + } + + public ECSlaveState ReadSlaveState(int slave) => ECMBind.ecmbind_get_slave_state(slave); + + private ECSlaveState UpdateEthercatState() + { + EthercatState = ECMBind.ecmbind_read_state(); + return ethercatState; + } + + public bool CheckState(ECSlaveState minimumState, int timeout) + { + for (; timeout > 0; timeout -= 10) + { + ECSlaveState state = ECMBind.ecmbind_read_state(); + if (state >= minimumState) + { + EthercatState = state; + return true; + } + Thread.Sleep(10); + } + return false; + } + public bool RequestState(ECSlaveState requestedState, out ECSlaveState reachedState, int timeout) + { + reachedState = ECMBind.ecmbind_request_state(0, requestedState, timeout); + Logging.Log(LogLevel.DEBUG, "ECMaster.RequestState({1}): lowest slave state: {0}", reachedState, requestedState); + EthercatState = reachedState; + return (reachedState >= requestedState); + } + + public int RequestSlaveState(int slave, ECSlaveState slaveState) => ECMBind.ecmbind_write_slave_state(slave, slaveState); + + public bool GetSDO(SDOAddr address,out SDO sdo) + { + sdo = sdoCache.GetOrCreateDescriptor(address); + return true; + } + + public IEnumerable GetSDOs(int slave) => sdoCache.GetSlaveCache(slave).Values; + + +/* public bool ReadSDOSubDescriptors(SDODescriptor descriptor) + { + bool success = true; + + for (int sub = 1; sub < descriptor.SubDescriptors.Length; sub++) + { + if (IOLocked(()=>ECMBind.ecmbind_read_objectdescription_entry((ushort)descriptor.Slave, (ushort)descriptor.Index, (ushort)sub, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int current_sub, String name) => { + //Logging.Log(LogLevel.DEBUG,"SDO Entry --> {0} {1} {2} {3} {4} {5}",slave,index, dataType, objectCode, current_sub, name); + + descriptor.SubDescriptors[current_sub].DataType = dataType; + descriptor.SubDescriptors[current_sub].Name = name; + })) <= 0) + { + //Logging.Log(LogLevel.WARNING,"ECMaster: ReadSDODescriptors({0}: failed to read descriptor for 0x{1:x8}.{2}", descriptor.Slave, descriptor.Index, sub); + success = false; + } + } + return success; + } +*/ + public bool ReadSDOIndeces() + { + for (int slave=1; slave <= CountSlaves; slave++) + { + if (!ReadSDOIndex(slave)) + return false; + } + return true; + } + public bool ReadSDOIndex(int slave) + { + IOLocked(()=>ECMBind.ecmbind_enumerate_servicedescriptors(slave, (int slave, int index) => { + sdoCache.GetOrCreateDescriptor(new SDOAddr(slave, index)); + })); + Logging.Log(LogLevel.DEBUG, "Indexed SDOs of slave {0}", slave); + return true; + } + +/* + public bool ReadSDODescriptor(SDODescriptor descriptor) + { + IOLocked(()=>ECMBind.ecmbind_enumerate_servicedescriptors(slave, (int slave, int index) => { + descriptors.Add(new SDODescriptor(){ + Slave = slave, + Index = index + }); + })); + + } + + public SDODescriptor[] ReadSDODescriptors(int slave) + { + List descriptors = new List(); + + IOLocked(()=>ECMBind.ecmbind_enumerate_servicedescriptors(slave, (int slave, int index) => { + descriptors.Add(new SDODescriptor(){ + Slave = slave, + Index = index + }); + })); + + foreach (SDODescriptor descriptor in descriptors) + { + if (IOLocked(()=>ECMBind.ecmbind_read_objectdescription(descriptor.Slave, descriptor.Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name) => { + descriptor.MaxSubindex = maxsub; + descriptor.ObjectCode = objectCode; + descriptor.CreateAndInitializeSubDescriptors(maxsub+1); + descriptor.SubDescriptors[0].DataType = dataType; + descriptor.SubDescriptors[0].Name = name ?? ""; + + //Logging.Log(LogLevel.DEBUG, "SDODescriptor: {0}", descriptor); + //Logging.Log(LogLevel.DEBUG,"SDO --> {0} {1} {2} {3} {4} {5}",slave,index, dataType,objectCode, maxsub, name); + + switch (descriptor.ObjectCode) + { + case ECObjectCodes.VAR: + break; + default: + ReadSDOSubDescriptors(descriptor); + break; + } + })) <= 0) + { + Logging.Log(LogLevel.WARNING,"ECMaster: ReadSDODescriptors({0}: failed to read descriptor for 0x{1:x8}", slave, descriptor.Index); + } + + } + + return descriptors.ToArray(); + } +*/ + + public void UpdatePDOMap() + { + List pdoList = new List(); + ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{ + if (addr_bit != 0) + Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported"); + else + pdoList.Add(new PDO(this,slave, index, subindex){ + AddressOffset = addr_offset, + AddressBit = addr_bit, + BitLength = bitlength + }); + }); + + lock (this) + { + IOMapPtr = ECMBind.ecmbind_get_iomap(); + pdoMap.Clear(); + + foreach (PDO pdo in pdoList) + pdoMap.Add(pdo.Address, pdo); + } + + } + + public PDO[] GetPDOMap() => pdoMap.Values.ToArray(); + public bool TryGetPDO(SDOAddr address, out PDO pdo) => pdoMap.TryGetValue(address, out pdo); + + +/* + public unsafe bool IOMapExtract(int offset,int size, byte[] buffer) + { + if (buffer.Length < size) + throw new ArgumentOutOfRangeException(nameof(buffer)); + + byte* iomap = (byte*)(IOMapPtr.ToPointer()) + offset; + fixed (byte* b = buffer) + { + Buffer.MemoryCopy(iomap, b, size, size); + } + + return true; + } +*/ + + T IOLocked(Func f) { + lock (lockIOMap) + return f(); + } + } +} \ No newline at end of file diff --git a/ln.ethercat/ECObjectCodes.cs b/ln.ethercat/ECObjectCodes.cs new file mode 100644 index 0000000..4549413 --- /dev/null +++ b/ln.ethercat/ECObjectCodes.cs @@ -0,0 +1,18 @@ + + +using System; + +namespace ln.ethercat +{ + public enum ECObjectCodes : UInt16 + { + NULL = 0, + DOMAIN = 2, + DEFTYPE = 5, + DEFSTRUCT = 6, + VAR = 7, + ARRAY = 8, + RECORD = 9, + ENUMDefinition = 40 + } +} \ No newline at end of file diff --git a/ln.ethercat/ECSlaveState.cs b/ln.ethercat/ECSlaveState.cs new file mode 100644 index 0000000..9b3060f --- /dev/null +++ b/ln.ethercat/ECSlaveState.cs @@ -0,0 +1,18 @@ + +using System; + +namespace ln.ethercat +{ + [Flags] + public enum ECSlaveState : UInt16 + { + NONE = 0, + INIT = 1, + PRE_OP = 2, + BOOT = 3, + SAFE_OP = 4, + OPERATIONAL = 8, + ERROR = 16, + ACK = 16 + } +} \ No newline at end of file diff --git a/ln.ethercat/PDO.cs b/ln.ethercat/PDO.cs new file mode 100644 index 0000000..b90935c --- /dev/null +++ b/ln.ethercat/PDO.cs @@ -0,0 +1,32 @@ + +using System; +using System.ComponentModel.DataAnnotations; + +namespace ln.ethercat +{ + public class PDO + { + readonly ECMaster ECMaster; + + public SDO SDO { get; } + public SDOAddr Address => SDO.Address; + + public int AddressOffset { get; set; } + public int AddressBit { get; set; } + public int BitLength { get; set; } + public int ByteLength { + get => (BitLength + 7) / 8; + set => BitLength = value * 8; + } + + public PDO(ECMaster ecMaster, UInt16 slave, UInt16 index, byte subIndex) + { + ECMaster = ecMaster; + ECMaster.GetSDO(new SDOAddr(slave, index, subIndex), out SDO sdo); + SDO = sdo; + } + + + + } +} \ No newline at end of file diff --git a/ln.ethercat/SDO.cs b/ln.ethercat/SDO.cs new file mode 100644 index 0000000..fb8f78c --- /dev/null +++ b/ln.ethercat/SDO.cs @@ -0,0 +1,92 @@ + + +using System; +using ln.json.mapping; +using ln.logging; +using ln.type; + +namespace ln.ethercat +{ + public class SDO + { + + public static SDO Create(ECMaster ecMaster, SDOAddr address) + { + SDO sdo = new SDO(ecMaster, address); + if (address.SubIndex == 0) + { + if (ECMBind.ecmbind_read_objectdescription(address.Slave, address.Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name)=>{ + sdo.DataType = dataType; + sdo.ObjectCode = objectCode; + sdo.MaxSubIndex = maxsub; + sdo.Name = name; + }) <= 0) + { + Logging.Log(LogLevel.WARNING, "cannot create SDO instance for {0}. not found.", address); + return null; + } + } else { + if (ECMBind.ecmbind_read_objectdescription_entry((ushort)address.Slave, (ushort)address.Index, (ushort)address.SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name)=>{ + sdo.DataType = dataType; + sdo.MaxSubIndex = -1; + sdo.Name = name; + }) <= 0) + { + Logging.Log(LogLevel.WARNING, "cannot create SDO instance for {0}. not found.", address); + return null; + } + } + return sdo; + } + + public SDOAddr Address { get; } + public string Name { get; private set; } + + public ECDataTypes DataType { get; set; } + public ECObjectCodes ObjectCode { get; set; } + public int MaxSubIndex { get; set; } + + public bool IsPartOfPDO => ECMaster.TryGetPDO(Address, out PDO pdo); + + byte[] rawData; + public byte[] RawData { + get { + if (ECMaster.TryGetPDO(Address, out PDO pdo)) + { + if ((rawData == null) || (rawData.Length != pdo.ByteLength)) + rawData = new byte[pdo.ByteLength]; + + ECMBind.ecmbind_iomap_get(pdo.AddressOffset, rawData, pdo.ByteLength); + } else { + ECMaster.ReadSDO(Address, out rawData); + } + return rawData; + } + set => rawData = value; + } + public object Value { + get => ECDataTypeConverter.ConvertFromEthercat(DataType ,RawData); + set => throw new NotImplementedException(); + } + + ECMaster ECMaster { get; } + + SDO(ECMaster ecMaster, SDOAddr address) + { + ECMaster = ecMaster; + Address = address; + } + + public override string ToString() + { + return string.Format("[SDO Slave={0} Index={1:X4}.{7} MaxSubindex={2} ObjectCode={5} DataType={3} Name={4} RawData={6}]", Address.Slave, Address.Index, MaxSubIndex, DataType, Name, ObjectCode, RawData?.ToHexString(), Address.SubIndex); + } + + public override bool Equals(object obj) => (obj is SDO other) && Address.Equals(other.Address); + public override int GetHashCode() => Address.GetHashCode(); + + public T GetValue() => Cast.To(Value); + + } + +} \ No newline at end of file diff --git a/ln.ethercat/SDOAddr.cs b/ln.ethercat/SDOAddr.cs new file mode 100644 index 0000000..9b79b06 --- /dev/null +++ b/ln.ethercat/SDOAddr.cs @@ -0,0 +1,35 @@ + + +namespace ln.ethercat +{ + public class SDOAddr + { + public int Slave { get; private set; } + public int Index { get; private set; } + public int SubIndex { get; private set; } + + public long Linear { get; private set; } + public int Compact { get; private set; } + + public SDOAddr(int slave, int index) : this(slave, index, 0) { } + public SDOAddr(int slave, int index, int subindex) + { + Slave = slave; + Index = index; + SubIndex = subindex; + + Linear = ((long)(ulong)Slave << 32) | ((long)Index << 16) | (long)SubIndex; + Compact = ((Slave << 24) | (Index << 8) | SubIndex) ^ ((Slave & 0xFF00) << 24); + } + + public override bool Equals(object obj) => (obj is SDOAddr other) && (Linear == other.Linear); + public override int GetHashCode() => Compact; + + public override string ToString() + { + return string.Format("[SDOAddr Slave={0} Index={1:X4}.{2} ]", Slave, Index, SubIndex); + } + + + } +} \ No newline at end of file diff --git a/ln.ethercat/SDOCache.cs b/ln.ethercat/SDOCache.cs new file mode 100644 index 0000000..d491877 --- /dev/null +++ b/ln.ethercat/SDOCache.cs @@ -0,0 +1,48 @@ + +using System; +using System.Collections.Generic; +using System.Reflection.Metadata; +using ln.collections; + +namespace ln.ethercat +{ + public class SDOCache + { + ECMaster ECMaster { get; } + + Dictionary> slaveCaches = new Dictionary>(); + + public SDOCache(ECMaster ecMaster) + { + ECMaster = ecMaster; + } + + public BTree GetSlaveCache(int slave) + { + if (!slaveCaches.TryGetValue(slave, out BTree slaveCache)) + { + slaveCache = new BTree(); + slaveCaches.Add(slave, slaveCache); + } + return slaveCache; + } + + + public SDO GetOrCreateDescriptor(SDOAddr address) + { + BTree slaveCache = GetSlaveCache(address.Slave); + + if (!slaveCache.TryGet(address.Linear, out SDO sdo)) + { + sdo = SDO.Create(ECMaster, address); + slaveCache.Add(sdo.Address.Linear, sdo); + } + return sdo; + } + + public void Clear() => slaveCaches.Clear(); + public bool Contains(SDOAddr addr) => GetSlaveCache(addr.Slave).ContainsKey(addr); + public SDO this[SDOAddr address] => GetSlaveCache(address.Slave)[address.Linear]; + + } +} \ No newline at end of file diff --git a/ln.ethercat/controller/ControlLoop.cs b/ln.ethercat/controller/ControlLoop.cs new file mode 100644 index 0000000..a703972 --- /dev/null +++ b/ln.ethercat/controller/ControlLoop.cs @@ -0,0 +1,92 @@ + + +using System; + +namespace ln.ethercat.controller +{ + public delegate double LoopGetProcessValueDelegate(); + + public abstract class ControlLoop + { + public Controller Controller { get; } + + public LoopGetProcessValueDelegate GetProcessValueDelegate { get; set; } + + public double SetPoint { get; set; } + public double ErrorValue { get; private set; } + public double Output { get; private set; } + + public ControlLoop(Controller controller, LoopGetProcessValueDelegate getProcessValueDelegate) + { + Controller = controller; + GetProcessValueDelegate = getProcessValueDelegate; + } + + public virtual void Clear() + { + ErrorValue = 0; + Output = 0; + } + public virtual void Loop() => Loop(GetProcessValueDelegate()); + public virtual void Loop(double processValue) + { + ErrorValue = SetPoint - processValue; + Output = LoopImplementation(); + } + + protected abstract double LoopImplementation(); + protected abstract void ClearImplementation(); + } + + public class PIDControlLoop : ControlLoop + { + public double Kp { get; set; } = 1.0; + public double Ki { get; set; } = 1.0; + public double Tn { + get => 1.0 / Ki; + set => Ki = (value == 0) ? 0.0 : (1.0 / value); + } + + public double Kd { get; set; } = 1.0; + + + public PIDControlLoop(Controller controller, LoopGetProcessValueDelegate getProcessValueDelegate) + :base(controller, getProcessValueDelegate) + {} + + public double Integral { get; set; } + public double MinIntregal { get; set; } = double.MinValue; + public double MaxIntegral { get; set; } = double.MaxValue; + public double LastError { get; private set; } + + protected override void ClearImplementation() + { + Integral = Math.Clamp(0, MinIntregal, MaxIntegral); + LastError = 0; + } + + protected override double LoopImplementation() + { + double output = (Kp * ErrorValue); + + if (Ki > 0.0) + { + Integral += (ErrorValue * Controller.ControllerLoopInterval * Ki) * Kp; + Integral = Math.Clamp(Integral, MinIntregal, MaxIntegral); + output += Integral; + } + if (Kd != 0) + { + output += (Kd * (ErrorValue - LastError) * Controller.ControllerLoopInterval) * Kp; + LastError = ErrorValue; + } + + return output; + } + + + + } + + +} \ No newline at end of file diff --git a/ln.ethercat/controller/Controller.cs b/ln.ethercat/controller/Controller.cs new file mode 100644 index 0000000..f17e8d3 --- /dev/null +++ b/ln.ethercat/controller/Controller.cs @@ -0,0 +1,101 @@ + +using System.Collections.Generic; +using System.Threading; +using ln.ethercat.controller.drives; +using ln.logging; + +namespace ln.ethercat.controller +{ + + public delegate void ControllerLogicDelegate(Controller controller); + + public class Controller + { + public event ControllerLogicDelegate ControllerLogic; + + + List driveControllers = new List(); + public DriveController[] DriveControllers => driveControllers.ToArray(); + + List controlLoops = new List(); + public ControlLoop[] ControlLoops => controlLoops.ToArray(); + + public bool IsRunning => threadController?.IsAlive ?? false; + + public double ControllerLoopInterval { get; set; } = 0.1; + public double ControllerLoopFrequency { + get => 1.0 / ControllerLoopInterval; + set => ControllerLoopInterval = 1.0 / value; + } + + bool stopRequested; + Thread threadController; + + public Controller() + { + } + + public void Start() + { + if (threadController?.IsAlive ?? false) + { + Logging.Log(LogLevel.WARNING, "Controller; Start(): already started"); + } else { + stopRequested = false; + threadController = new Thread(ControllerThread); + threadController.Start(); + } + } + + public void Stop() + { + if (threadController?.IsAlive ?? false) + { + stopRequested = true; + threadController.Join(); + threadController = null; + } + } + + public void Add(DriveController driveController) => driveControllers.Add(driveController); + public void Remove(DriveController driveController) => driveControllers.Remove(driveController); + + public void Add(ControlLoop controlLoop) => controlLoops.Add(controlLoop); + public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop); + + public DriveStates DrivesState { + get { + DriveStates lowestState = DriveStates.OPERATIONAL; + foreach (DriveController driveController in driveControllers) + { + DriveStates driveState = driveController.DriveState; + if (driveState < lowestState) + lowestState = driveState; + } + return lowestState; + } + } + + void ControllerThread() + { + while (!stopRequested) + { + foreach (DriveController driveController in driveControllers) + driveController.UpdateStates(); + + foreach (ControlLoop controlLoop in controlLoops) + controlLoop.Loop(); + + ControllerLogic?.Invoke(this); + + foreach (DriveController driveController in driveControllers) + driveController.UpdateDrive(); + + Thread.Sleep((int)(1000.0 * ControllerLoopInterval)); + } + } + + + + } +} \ No newline at end of file diff --git a/ln.ethercat/controller/drives/CIA402Controller.cs b/ln.ethercat/controller/drives/CIA402Controller.cs new file mode 100644 index 0000000..988f588 --- /dev/null +++ b/ln.ethercat/controller/drives/CIA402Controller.cs @@ -0,0 +1,242 @@ + + +using System; +using ln.logging; + +namespace ln.ethercat.controller.drives +{ + public enum CIA402States { + UNDEFINED, + NOT_READY_TO_SWITCH_ON, + SWITCH_ON_DISABLED, + READY_TO_SWITCH_ON, + SWITCHED_ON, + OPERATION_ENABLED, + QUICK_STOP_ACTIVE, + FAULT_REACTION_ACTIVE, + FAULT + } + + public enum CIA402ModesOfOperation : byte + { + NO_MODE_CHANGE = 0, + VL_VELOCITY_MODE = 2, + HOMING_MODE = 6, + INTERPOLATED_POSITION_MODE = 7, + CYCLIC_SYNC_POSITION = 8, + CYCLIC_SYNC_VELOCITY = 9, + CYCLIC_SYNC_TORQUE = 10 + } + + public class CIA402Controller : DriveController + { + public readonly SDOAddr saControlWord; + public readonly SDOAddr saStatusWord; + public readonly SDOAddr saErrorCode; + public readonly SDOAddr saTargetPosition; + public readonly SDOAddr saTargetSpeed; + public readonly SDOAddr saTargetTorque; + public readonly SDOAddr saActualPosition; + public readonly SDOAddr saActualSpeed; + public readonly SDOAddr saActualTorque; + public readonly SDOAddr saModesOfOperation; + public readonly SDOAddr saModesOfOperationDisplay; + + public CIA402Controller(ECMaster ecMaster,int slave) + :base(ecMaster, slave) + { + saErrorCode = new SDOAddr(slave, 0x603f); + saControlWord = new SDOAddr(slave, 0x6040); + saStatusWord = new SDOAddr(slave, 0x6041); + + saTargetPosition = new SDOAddr(slave, 0x607A); + saTargetSpeed = new SDOAddr(slave, 0x60FF); + saTargetTorque = new SDOAddr(slave, 0x6071); + + saActualPosition = new SDOAddr(slave, 0x6064); + saActualSpeed = new SDOAddr(slave, 0x606C); + saActualTorque = new SDOAddr(slave, 0x6077); + + saModesOfOperation = new SDOAddr(slave, 0x6060); + saModesOfOperationDisplay = new SDOAddr(slave, 0x6061); + } + + public override void UpdateStates() { } + public override void UpdateDrive() { } + + public override DriveStates DriveState + { + get { + switch (GetCIA402State()) + { + case CIA402States.NOT_READY_TO_SWITCH_ON: + case CIA402States.SWITCH_ON_DISABLED: + case CIA402States.READY_TO_SWITCH_ON: + return DriveStates.INIT; + case CIA402States.SWITCHED_ON: + case CIA402States.QUICK_STOP_ACTIVE: + return DriveStates.POWERED; + case CIA402States.OPERATION_ENABLED: + return DriveStates.OPERATIONAL; + case CIA402States.FAULT_REACTION_ACTIVE: + case CIA402States.FAULT: + return DriveStates.ERROR; + default: + return DriveStates.UNDEFINED; + } + } + } + + public override string OEMDriveState { + get => GetCIA402State().ToString(); + } + + public override DriveMode DriveMode { + get { + switch (ModeOfOperation) + { + case CIA402ModesOfOperation.CYCLIC_SYNC_POSITION: + return DriveMode.POSITION; + case CIA402ModesOfOperation.CYCLIC_SYNC_VELOCITY: + return DriveMode.SPEED; + case CIA402ModesOfOperation.CYCLIC_SYNC_TORQUE: + return DriveMode.TORQUE; + default: + return DriveMode.UNDEFINED; + } + } + set { + switch (value) + { + case DriveMode.POSITION: + ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION; + break; + case DriveMode.SPEED: + ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION; + break; + case DriveMode.TORQUE: + ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION; + break; + default: + Logging.Log(LogLevel.WARNING, "CIA402Controller: DriveMode {0} not supported", value); + break; + } + } + } + + public CIA402ModesOfOperation ModeOfOperation { + get => (CIA402ModesOfOperation)GetSDOValue(saModesOfOperationDisplay); + set => SetSDOValue(saModesOfOperation, (byte)value); + } + + + public override decimal ActualPosition => GetSDOValue(saActualPosition); + public override decimal ActualSpeed => GetSDOValue(saActualSpeed); + public override decimal ActualTorque => GetSDOValue(saActualTorque); + + public override decimal TargetPosition { + get => GetSDOValue(saTargetPosition); + set => SetSDOValue(saTargetPosition, value); + } + + public override decimal TargetSpeed { + get => GetSDOValue(saTargetSpeed); + set => SetSDOValue(saTargetSpeed, value); + } + public override decimal TargetTorque { + get => GetSDOValue(saTargetTorque); + set => SetSDOValue(saTargetTorque, value); + } + + public override int ErrorCode => GetSDOValue(saErrorCode); + + public override string ErrorText => ErrorCode.ToString(); + + public override void EnableDrive(bool enable) + { + if (enable) + { + switch (CIA402State) + { + case CIA402States.SWITCHED_ON: + SetSDOValue(saControlWord, (UInt16)0x000F); + break; + default: + Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't enable drive", CIA402State.ToString()); + break; + } + } else { + switch (CIA402State) + { + case CIA402States.OPERATION_ENABLED: + SetSDOValue(saControlWord, (UInt16)0x0007); + break; + default: + Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't disable drive", CIA402State.ToString()); + break; + } + } + } + + public override void Power(bool poweron) + { + if (poweron) + { + switch (CIA402State) + { + case CIA402States.FAULT: + case CIA402States.FAULT_REACTION_ACTIVE: + Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive in fault state, not ready to switch power on"); + break; + case CIA402States.NOT_READY_TO_SWITCH_ON: + Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive not ready to switch power on"); + break; + case CIA402States.SWITCH_ON_DISABLED: + case CIA402States.READY_TO_SWITCH_ON: + SetSDOValue(saControlWord, (UInt16)0x0007); // Switch ON + break; + } + } else { + SetSDOValue(saControlWord, (UInt16)0x0006); + } + } + + public override void ClearFault() + { + switch (CIA402State) + { + case CIA402States.FAULT: + SetSDOValue(saControlWord, (UInt16)0x0086); + break; + } + } + + CIA402States CIA402State => GetCIA402State(); + public CIA402States GetCIA402State() + { + if (ECMaster.GetSDO(saStatusWord, out SDO sdoStatusWord)) + { + UInt16 statusword = sdoStatusWord.GetValue(); + + if ((statusword & 0x004F)==0) + return CIA402States.NOT_READY_TO_SWITCH_ON; + if ((statusword & 0x004F)==0x0040) + return CIA402States.SWITCH_ON_DISABLED; + if ((statusword & 0x006F)==0x0021) + return CIA402States.READY_TO_SWITCH_ON; + if ((statusword & 0x006F)==0x0023) + return CIA402States.SWITCHED_ON; + if ((statusword & 0x006F)==0x0027) + return CIA402States.OPERATION_ENABLED; + if ((statusword & 0x006F)==0x0007) + return CIA402States.QUICK_STOP_ACTIVE; + if ((statusword & 0x004F)==0x000F) + return CIA402States.FAULT_REACTION_ACTIVE; + if ((statusword & 0x004F)==0x0008) + return CIA402States.FAULT; + } + return CIA402States.UNDEFINED; + } + + } +} \ No newline at end of file diff --git a/ln.ethercat/controller/drives/DriveController.cs b/ln.ethercat/controller/drives/DriveController.cs new file mode 100644 index 0000000..164d01f --- /dev/null +++ b/ln.ethercat/controller/drives/DriveController.cs @@ -0,0 +1,88 @@ + + +using System; +using System.Runtime; + +namespace ln.ethercat.controller.drives +{ + public enum DriveStates { + UNDEFINED, + BOOT, + INIT, + ERROR, + POWERED, + OPERATIONAL, + } + + public enum DriveMode { + UNDEFINED, + TORQUE, + SPEED, + POSITION + } + + public abstract class DriveController + { + protected ECMaster ECMaster { get; } + public int Slave { get; } + + public DriveController(ECMaster ecMaster, int slave) + { + ECMaster = ecMaster; + Slave = slave; + } + + /* called by controller before control loops and logic */ + public abstract void UpdateStates(); + /* called by controller after user logic */ + public abstract void UpdateDrive(); + + public abstract DriveStates DriveState { get; } + public abstract string OEMDriveState { get; } + public abstract Int32 ErrorCode { get; } + public abstract string ErrorText { get; } + + public abstract void ClearFault(); + + public abstract DriveMode DriveMode { get; set; } + + public void PowerOn() => Power(true); + public void PowerOff() => Power(false); + public abstract void Power(bool poweron); + + public void EnableDrive() => EnableDrive(true); + public void DisableDrive() => EnableDrive(false); + public abstract void EnableDrive(bool enabled); + + public abstract decimal ActualPosition { get; } + public abstract decimal ActualSpeed { get; } + public abstract decimal ActualTorque { get; } + public abstract decimal TargetPosition { get; set; } + public abstract decimal TargetSpeed { get; set; } + public abstract decimal TargetTorque { get; set; } + + + public T GetSDOValue(UInt16 index,byte subIndex) + { + if (ECMaster.GetSDO(new SDOAddr(Slave, index, subIndex), out SDO sdo)) + return sdo.GetValue(); + throw new Exception("DriveController: failed to get sdo value"); + } + public T GetSDOValue(SDOAddr sa) + { + if (ECMaster.GetSDO(sa, out SDO sdo)) + return sdo.GetValue(); + throw new Exception("DriveController: failed to get sdo value"); + } + + public void SetSDOValue(UInt16 slave, UInt16 index, byte subIndex, T value) => SetSDOValue(new SDOAddr(slave,index,subIndex), value); + public void SetSDOValue(SDOAddr sa, T value) + { + if (ECMaster.GetSDO(sa, out SDO sdo)) + sdo.Value = value; + } + + + } + +} \ No newline at end of file diff --git a/ln.ethercat/lib/libecmbind.so b/ln.ethercat/lib/libecmbind.so new file mode 100755 index 0000000..d49743a Binary files /dev/null and b/ln.ethercat/lib/libecmbind.so differ diff --git a/ln.ethercat/ln.ethercat.csproj b/ln.ethercat/ln.ethercat.csproj new file mode 100644 index 0000000..205ce7d --- /dev/null +++ b/ln.ethercat/ln.ethercat.csproj @@ -0,0 +1,25 @@ + + + + netcoreapp3.1 + true + 0.1.0-ci + Harald Wolff-Thobaben + l--n.de + A simple ethercat master based on SOEM (https://openethercatsociety.github.io/) + (c) 2020 Harald Wolff-Thobaben + ethercat master + + + + + + + + + + + + + +