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