379 lines
12 KiB
C#
379 lines
12 KiB
C#
|
|
|
|
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();
|
|
}
|
|
}
|
|
} |