Alpha Commit
ln.build - build0.waldrennach.l--n.de build job pending Details

master
Harald Wolff 2020-12-16 09:29:40 +01:00
commit 0fb7af31c0
33 changed files with 2627 additions and 0 deletions

46
.gitignore vendored 100644
View File

@ -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

3
.gitmodules vendored 100644
View File

@ -0,0 +1,3 @@
[submodule "contrib/SOEM"]
path = contrib/SOEM
url = https://git.l--n.de/ln-dotnet/SOEM.git

24
build.ln 100644
View File

@ -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"
]
}
]
}

28
contrib.soem.make 100644
View File

@ -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)

1
contrib/SOEM 160000

@ -0,0 +1 @@
Subproject commit 342ca8632c3a495ea9700cc2ea189ca20c12c3e2

View File

@ -0,0 +1,258 @@
#include <ethercat.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <pthread.h>
#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<bytelength-1;n++)
{
buffer[n] = ((buffer[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);
}

View File

@ -0,0 +1,38 @@
#pragma once
#include <stdint.h>
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);

View File

@ -0,0 +1,220 @@
#include <ethercat.h>
#include <stdio.h>
#include <string.h>
#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_pdo_map_length;n++)
{
cb(ecd_pdo_map[n].slave, ecd_pdo_map[n].index, ecd_pdo_map[n].subindex, ecd_pdo_map[n].addr_offset, ecd_pdo_map[n].addr_bit, ecd_pdo_map[n].bitlength);
}
}
int ecmbind_iomap_get(int offset, char* buffer, int length)
{
if (offset + length > 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);
}

View File

@ -0,0 +1,295 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ethercat.h>
#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;
}

View File

@ -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);
}
}
}

View File

@ -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");
}
}
}
*/
}
}
}

View File

@ -0,0 +1,15 @@
using ln.http.api;
namespace ln.ethercat.service.api.v1
{
public class ControllerApiController : WebApiController
{
}
}

View File

@ -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));
}
}
}
}

View File

@ -0,0 +1,18 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<OutputType>Exe</OutputType>
<TargetFramework>netcoreapp3.1</TargetFramework>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="ln.type" Version="0.1.7-ci" />
<PackageReference Include="ln.logging" Version="1.0" />
<PackageReference Include="ln.json" Version="1.0.3" />
<PackageReference Include="ln.http" Version="0.3.0" />
<PackageReference Include="ln.http.api" Version="0.0.5" />
<ProjectReference Include="../ln.ethercat/ln.ethercat.csproj" />
</ItemGroup>
</Project>

62
ln.ethercat.sln 100644
View File

@ -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

View File

@ -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();
}
}
}

View File

@ -0,0 +1,16 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>netcoreapp3.1</TargetFramework>
<IsPackable>false</IsPackable>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="NUnit" Version="3.12.0" />
<PackageReference Include="NUnit3TestAdapter" Version="3.16.1" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="16.5.0"/>
<ProjectReference Include="../ln.ethercat/ln.ethercat.csproj" />
</ItemGroup>
</Project>

View File

@ -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;
}
}
}
}

View File

@ -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
}
}

View File

@ -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);
}
}

View File

@ -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<SDOAddr,PDO> pdoMap = new Dictionary<SDOAddr, PDO>();
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<SDO> 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<SDODescriptor> descriptors = new List<SDODescriptor>();
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<PDO> pdoList = new List<PDO>();
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<T>(Func<T> f) {
lock (lockIOMap)
return f();
}
}
}

View File

@ -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
}
}

View File

@ -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
}
}

32
ln.ethercat/PDO.cs 100644
View File

@ -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;
}
}
}

92
ln.ethercat/SDO.cs 100644
View File

@ -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<T>() => Cast.To<T>(Value);
}
}

View File

@ -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);
}
}
}

View File

@ -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<int, BTree<long,SDO>> slaveCaches = new Dictionary<int, BTree<long, SDO>>();
public SDOCache(ECMaster ecMaster)
{
ECMaster = ecMaster;
}
public BTree<long,SDO> GetSlaveCache(int slave)
{
if (!slaveCaches.TryGetValue(slave, out BTree<long,SDO> slaveCache))
{
slaveCache = new BTree<long, SDO>();
slaveCaches.Add(slave, slaveCache);
}
return slaveCache;
}
public SDO GetOrCreateDescriptor(SDOAddr address)
{
BTree<long,SDO> 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];
}
}

View File

@ -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;
}
}
}

View File

@ -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<DriveController> driveControllers = new List<DriveController>();
public DriveController[] DriveControllers => driveControllers.ToArray();
List<ControlLoop> controlLoops = new List<ControlLoop>();
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));
}
}
}
}

View File

@ -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<byte>(saModesOfOperationDisplay);
set => SetSDOValue<byte>(saModesOfOperation, (byte)value);
}
public override decimal ActualPosition => GetSDOValue<int>(saActualPosition);
public override decimal ActualSpeed => GetSDOValue<int>(saActualSpeed);
public override decimal ActualTorque => GetSDOValue<int>(saActualTorque);
public override decimal TargetPosition {
get => GetSDOValue<int>(saTargetPosition);
set => SetSDOValue(saTargetPosition, value);
}
public override decimal TargetSpeed {
get => GetSDOValue<int>(saTargetSpeed);
set => SetSDOValue(saTargetSpeed, value);
}
public override decimal TargetTorque {
get => GetSDOValue<int>(saTargetTorque);
set => SetSDOValue(saTargetTorque, value);
}
public override int ErrorCode => GetSDOValue<int>(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<UInt16>();
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;
}
}
}

View File

@ -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<T>(UInt16 index,byte subIndex)
{
if (ECMaster.GetSDO(new SDOAddr(Slave, index, subIndex), out SDO sdo))
return sdo.GetValue<T>();
throw new Exception("DriveController: failed to get sdo value");
}
public T GetSDOValue<T>(SDOAddr sa)
{
if (ECMaster.GetSDO(sa, out SDO sdo))
return sdo.GetValue<T>();
throw new Exception("DriveController: failed to get sdo value");
}
public void SetSDOValue<T>(UInt16 slave, UInt16 index, byte subIndex, T value) => SetSDOValue(new SDOAddr(slave,index,subIndex), value);
public void SetSDOValue<T>(SDOAddr sa, T value)
{
if (ECMaster.GetSDO(sa, out SDO sdo))
sdo.Value = value;
}
}
}

Binary file not shown.

View File

@ -0,0 +1,25 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>netcoreapp3.1</TargetFramework>
<GeneratePackageOnBuild>true</GeneratePackageOnBuild>
<Version>0.1.0-ci</Version>
<Authors>Harald Wolff-Thobaben</Authors>
<Company>l--n.de</Company>
<Description>A simple ethercat master based on SOEM (https://openethercatsociety.github.io/) </Description>
<Copyright>(c) 2020 Harald Wolff-Thobaben</Copyright>
<PackageTags>ethercat master</PackageTags>
</PropertyGroup>
<ItemGroup>
<None Update="lib/**" CopyToOutputDirectory="PreserveNewest" />
</ItemGroup>
<ItemGroup>
<PackageReference Include="ln.type" Version="0.1.7-ci" />
<PackageReference Include="ln.logging" Version="1.0" />
<PackageReference Include="ln.json" Version="1.0.3" />
<PackageReference Include="ln.collections" Version="0.1.3-ci" />
</ItemGroup>
</Project>