Alpha Commit
ln.build - build0.waldrennach.l--n.de build job pending
Details
ln.build - build0.waldrennach.l--n.de build job pending
Details
commit
0fb7af31c0
|
@ -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
|
|
@ -0,0 +1,3 @@
|
|||
[submodule "contrib/SOEM"]
|
||||
path = contrib/SOEM
|
||||
url = https://git.l--n.de/ln-dotnet/SOEM.git
|
|
@ -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"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
|
@ -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)
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 342ca8632c3a495ea9700cc2ea189ca20c12c3e2
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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);
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
|
@ -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");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*/
|
||||
}
|
||||
}
|
||||
}
|
|
@ -0,0 +1,15 @@
|
|||
|
||||
|
||||
using ln.http.api;
|
||||
|
||||
namespace ln.ethercat.service.api.v1
|
||||
{
|
||||
|
||||
public class ControllerApiController : WebApiController
|
||||
{
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
|
@ -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>
|
|
@ -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
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
|
@ -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>
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
|
@ -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
|
||||
}
|
||||
}
|
|
@ -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);
|
||||
|
||||
}
|
||||
}
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
}
|
|
@ -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
|
||||
}
|
||||
}
|
|
@ -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
|
||||
}
|
||||
}
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
|
@ -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);
|
||||
|
||||
}
|
||||
|
||||
}
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
|
@ -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];
|
||||
|
||||
}
|
||||
}
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
|
@ -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.
|
@ -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>
|
Loading…
Reference in New Issue