361 lines
12 KiB
C#
361 lines
12 KiB
C#
|
|
|
|
using System;
|
|
using System.Buffers;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
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 = 1;
|
|
public int INTERVALL_WATCHDOG = 250;
|
|
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
List<PDO> pdoMap = new List<PDO>();
|
|
public PDO[] GetPDOMap() => pdoMap.ToArray();
|
|
|
|
Dictionary<UInt16, ECSlave> slaves = new Dictionary<ushort, ECSlave>();
|
|
|
|
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");
|
|
}
|
|
|
|
bool stopProcessing;
|
|
public bool StopProccessing {
|
|
get => stopProcessing;
|
|
set => stopProcessing = value;
|
|
}
|
|
|
|
Thread threadProcessData;
|
|
Thread threadWatchdog;
|
|
|
|
public bool Start()
|
|
{
|
|
if ((threadProcessData?.IsAlive ?? false) || (threadWatchdog?.IsAlive ?? false))
|
|
throw new Exception("already started");
|
|
|
|
stopProcessing = false;
|
|
|
|
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();
|
|
|
|
Thread.Sleep(20);
|
|
|
|
if (!RequestState(ECSlaveState.SAFE_OP, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
|
|
{
|
|
Stop();
|
|
return false;
|
|
}
|
|
|
|
threadWatchdog = new Thread(Watchdog);
|
|
threadWatchdog.Start();
|
|
|
|
}
|
|
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;
|
|
|
|
if (!Thread.CurrentThread.Equals(threadWatchdog))
|
|
{
|
|
threadWatchdog?.Join();
|
|
threadWatchdog = null;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
object lockIOMap = new object();
|
|
byte[] iomap = new byte[8192];
|
|
|
|
void Watchdog()
|
|
{
|
|
// if (!ReadSDOIndeces())
|
|
// {
|
|
// Logging.Log(LogLevel.WARNING, "ECMaster: could not read SDO indeces");
|
|
// }
|
|
|
|
if (!RequestState(ECSlaveState.OPERATIONAL, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
|
|
{
|
|
if (slaveState < ECSlaveState.SAFE_OP)
|
|
Stop();
|
|
}
|
|
|
|
while (!stopProcessing)
|
|
{
|
|
UpdateEthercatState();
|
|
Thread.Sleep(250);
|
|
}
|
|
}
|
|
|
|
void MasterThread()
|
|
{
|
|
ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
|
|
|
|
while (!stopProcessing)
|
|
{
|
|
int wkc;
|
|
|
|
lock (lockIOMap)
|
|
{
|
|
wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
|
|
if (wkc != ExpectedWorkCounter)
|
|
{
|
|
int success = ECMBind.ecmbind_recover();
|
|
if (success > 0)
|
|
ExpectedWorkCounter = success;
|
|
else
|
|
{
|
|
RequestSlaveState(0, ECSlaveState.SAFE_OP);
|
|
}
|
|
}
|
|
}
|
|
Thread.Sleep(INTERVALL_PROCESSDATA);
|
|
|
|
/*
|
|
if (wkc != ExpectedWorkCounter)
|
|
{
|
|
ExpectedWorkCounter = ECMBind.ecmbind_recover();
|
|
if (ExpectedWorkCounter < 0)
|
|
break;
|
|
}
|
|
*/
|
|
|
|
}
|
|
}
|
|
|
|
public bool ReadSDO(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
|
|
{
|
|
rawValue =
|
|
new byte[128];
|
|
int dataSize;
|
|
|
|
lock (lockIOMap)
|
|
{
|
|
dataSize =
|
|
ECMBind.ecmbind_sdo_read(slave, index, subIndex, rawValue, rawValue.Length );
|
|
if (dataSize <= 0)
|
|
{
|
|
Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
|
|
throw new IOException();
|
|
}
|
|
}
|
|
rawValue = rawValue.Slice(0, dataSize);
|
|
return true;
|
|
}
|
|
|
|
public bool WriteSDO(UInt16 slave,UInt16 index, byte subIndex, byte[] rawValue)
|
|
{
|
|
int result;
|
|
lock (lockIOMap)
|
|
result = ECMBind.ecmbind_sdo_write(slave, index, subIndex, rawValue, rawValue.Length);
|
|
Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
|
|
return result > 0;
|
|
}
|
|
|
|
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 GetSlave(UInt16 slave_id, out ECSlave slave)
|
|
{
|
|
if (!slaves.TryGetValue(slave_id, out slave))
|
|
{
|
|
if (slave_id > CountSlaves)
|
|
return false;
|
|
|
|
slave = new ECSlave(this, slave_id);
|
|
slaves.Add(slave_id, slave);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
public bool GetSDOValue(SDOAddr address, out SDOValue sdoValue) => GetSDOValue(address.Slave, address.Index, address.SubIndex, out sdoValue);
|
|
public bool GetSDOValue(UInt16 slave_id, UInt16 index,byte subIndex, out SDOValue sdoValue)
|
|
{
|
|
sdoValue = null;
|
|
return GetSlave(slave_id, out ECSlave slave) && slave.GetDescriptor(index, out SDODescriptor descriptor) && descriptor.GetValue(subIndex, out sdoValue);
|
|
}
|
|
|
|
public bool GetIOmapData(int offset,int length,out byte[] rawData)
|
|
{
|
|
lock (lockIOMap)
|
|
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
|
|
{
|
|
rawData = iomap.Slice(offset, length);
|
|
return true;
|
|
}
|
|
Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1}) failed", offset, length);
|
|
rawData = null;
|
|
return false;
|
|
}
|
|
public bool SetIOmapData(int offset,int length,byte[] rawData)
|
|
{
|
|
lock (lockIOMap)
|
|
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
|
|
{
|
|
Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
|
|
Buffer.BlockCopy(rawData, 0, iomap, offset, length);
|
|
return true;
|
|
}
|
|
Logging.Log(LogLevel.ERROR, "SetIOmapData({0},{1}) failed", offset, length);
|
|
rawData = null;
|
|
return false;
|
|
}
|
|
|
|
public bool GetPDOItem(SDOValue sdoValue, out PDO pdo)
|
|
{
|
|
foreach (PDO _pdo in pdoMap)
|
|
{
|
|
if (_pdo.SDOValue.Equals(sdoValue))
|
|
{
|
|
pdo = _pdo;
|
|
return true;
|
|
}
|
|
}
|
|
pdo = null;
|
|
return false;
|
|
}
|
|
|
|
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 = pdoList;
|
|
}
|
|
}
|
|
|
|
|
|
T IOLocked<T>(Func<T> f) {
|
|
lock (lockIOMap)
|
|
return f();
|
|
}
|
|
}
|
|
} |