572 lines
20 KiB
C#
572 lines
20 KiB
C#
|
|
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using System.Threading;
|
|
using ln.ethercat.controller;
|
|
using ln.ethercat.controller.drives;
|
|
using ln.logging;
|
|
using ln.type;
|
|
|
|
namespace ln.ethercat
|
|
{
|
|
|
|
public enum ECMasterState {
|
|
INITIALIZED,
|
|
STARTING,
|
|
STOPPING,
|
|
RUNNING,
|
|
}
|
|
|
|
public struct PDOMappingRequest
|
|
{
|
|
public ushort Slave;
|
|
public ushort Index;
|
|
public byte SubIndex;
|
|
public bool RxPDO;
|
|
|
|
public PDOMappingRequest(ushort slave,ushort index,byte subIndex,bool rxpdo)
|
|
{
|
|
Slave = slave;
|
|
Index = index;
|
|
SubIndex = subIndex;
|
|
RxPDO = rxpdo;
|
|
}
|
|
|
|
}
|
|
|
|
public delegate void ECStateChange(ECMaster sender,ECSlaveState newState);
|
|
public delegate void ProcessDataExchanged(ECMaster ecMaster);
|
|
|
|
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 event ProcessDataExchanged OnProcessDataExchanged;
|
|
|
|
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 =>ethercatState;
|
|
private 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");
|
|
|
|
Controller = new Controller(this);
|
|
}
|
|
|
|
bool stopWatchdog;
|
|
bool stopProcessData;
|
|
|
|
Thread threadProcessData;
|
|
Thread threadWatchdog;
|
|
|
|
public Controller Controller { get; }
|
|
List<DriveController> driveControllers = new List<DriveController>();
|
|
public DriveController[] DriveControllers => driveControllers.ToArray();
|
|
|
|
|
|
public void Start()
|
|
{
|
|
lock (this)
|
|
{
|
|
if (threadWatchdog?.IsAlive ?? false)
|
|
throw new Exception("already started");
|
|
|
|
ethercatState = ECSlaveState.NONE;
|
|
ExpectedWorkCounter = 0;
|
|
|
|
stopWatchdog = false;
|
|
|
|
threadWatchdog = new Thread(Watchdog);
|
|
threadWatchdog.Start();
|
|
}
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
lock (this)
|
|
{
|
|
if (threadWatchdog?.IsAlive ?? false)
|
|
{
|
|
stopWatchdog = true;
|
|
|
|
lock (this)
|
|
Monitor.PulseAll(this);
|
|
|
|
threadWatchdog.Join();
|
|
}
|
|
}
|
|
}
|
|
|
|
void Watchdog()
|
|
{
|
|
while (!stopWatchdog)
|
|
{
|
|
IOMapSize = 0;
|
|
if (threadProcessData?.IsAlive ?? false)
|
|
{
|
|
stopProcessData = true;
|
|
while (threadProcessData.IsAlive)
|
|
threadProcessData.Join(250);
|
|
}
|
|
stopProcessData = false;
|
|
|
|
EthercatState = ECSlaveState.INIT;
|
|
|
|
CountSlaves = ECMBind.ecmbind_config_init();
|
|
if (CountSlaves <= 0)
|
|
{
|
|
Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected, scheduling restart...");
|
|
Thread.Sleep(2500);
|
|
continue;
|
|
}
|
|
|
|
|
|
if (!WaitForState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
|
|
{
|
|
Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach PRE_OP. restart...");
|
|
Thread.Sleep(2500);
|
|
continue;
|
|
}
|
|
|
|
|
|
/* Bus State PRE_OP */
|
|
ScanControllers();
|
|
|
|
EthercatState = ECSlaveState.PRE_OP;
|
|
|
|
ConfigureProcessDataMappings();
|
|
SetupPDOMapping();
|
|
UpdatePDOMap();
|
|
|
|
threadProcessData = new Thread(ProcessData);
|
|
threadProcessData.Start();
|
|
|
|
Thread.Sleep(20);
|
|
RequestSlaveState(0, ECSlaveState.SAFE_OP);
|
|
|
|
if (!WaitForState(ECSlaveState.SAFE_OP, TIMEOUT_SAFEOP))
|
|
{
|
|
Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach SAFE_OP. restart...");
|
|
Thread.Sleep(2500);
|
|
continue;
|
|
}
|
|
|
|
EthercatState = ECSlaveState.SAFE_OP;
|
|
|
|
Thread.Sleep(20);
|
|
RequestSlaveState(0, ECSlaveState.OPERATIONAL);
|
|
|
|
Thread.Sleep(250);
|
|
if (!WaitForState(ECSlaveState.OPERATIONAL, TIMEOUT_SAFEOP))
|
|
{
|
|
Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach OPERATIONAL. restart...");
|
|
Thread.Sleep(2500);
|
|
continue;
|
|
}
|
|
|
|
EthercatState = ECSlaveState.OPERATIONAL;
|
|
|
|
/***** we handle the controller cycle from here on *****/
|
|
|
|
Controller.Initialize();
|
|
|
|
DateTime nextControllerRun = DateTime.Now;
|
|
while (!stopWatchdog)
|
|
{
|
|
EthercatState = IOLocked(()=>ECMBind.ecmbind_read_state());
|
|
if (EthercatState != ECSlaveState.OPERATIONAL)
|
|
break;
|
|
|
|
DateTime currentTime = DateTime.Now;
|
|
nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
|
|
if (currentTime > nextControllerRun)
|
|
{
|
|
Logging.Log(LogLevel.WARNING, "missed controller cycle");
|
|
while (currentTime > nextControllerRun)
|
|
nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
|
|
}
|
|
TimeSpan timeToNextCycle = nextControllerRun - currentTime;
|
|
try
|
|
{
|
|
Thread.Sleep(timeToNextCycle);
|
|
Controller.Cycle();
|
|
} catch (Exception e)
|
|
{
|
|
Logging.Log(e);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void ProcessData()
|
|
{
|
|
int wkc;
|
|
|
|
lock (lockIOMap)
|
|
ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
|
|
|
|
Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): start");
|
|
|
|
while (!stopProcessData)
|
|
{
|
|
lock (lockIOMap)
|
|
{
|
|
wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
|
|
if (wkc != ExpectedWorkCounter)
|
|
{
|
|
int success = ECMBind.ecmbind_recover();
|
|
if (success > 0)
|
|
{
|
|
Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery successfull...");
|
|
ExpectedWorkCounter = success;
|
|
//SetupPDOMapping();
|
|
} else
|
|
{
|
|
RequestSlaveState(0, ECSlaveState.INIT);
|
|
Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery failed, scheduling restart...");
|
|
ScheduleRestart();
|
|
stopProcessData = true;
|
|
}
|
|
} else {
|
|
OnProcessDataExchanged?.Invoke(this);
|
|
}
|
|
}
|
|
Thread.Sleep(INTERVALL_PROCESSDATA);
|
|
}
|
|
|
|
Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): finished");
|
|
}
|
|
|
|
public void ScanControllers()
|
|
{
|
|
driveControllers.Clear();
|
|
|
|
for (UInt16 slave_id = 1; slave_id <= CountSlaves; slave_id++)
|
|
{
|
|
if (ReadSDO(slave_id, 0x1000, 0, out byte[] bDeviceType))
|
|
{
|
|
ushort profilecode = BitConverter.ToUInt16(bDeviceType);
|
|
Logging.Log(LogLevel.INFO, "ECMaster: ScanControllers: Slave {0} has DeviceType {1} [{2}]", slave_id, profilecode, bDeviceType.ToHexString());
|
|
|
|
switch (profilecode)
|
|
{
|
|
case 402:
|
|
driveControllers.Add(new CIA402Controller(this, slave_id));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
bool WaitForState(ECSlaveState expectedState, int timeout)
|
|
{
|
|
int waitIncrement = 100;
|
|
|
|
ECSlaveState currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
|
|
while ((timeout > 0) && (currentState != expectedState))
|
|
{
|
|
Thread.Sleep(waitIncrement);
|
|
timeout -= waitIncrement;
|
|
currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
|
|
}
|
|
return currentState == expectedState;
|
|
}
|
|
|
|
|
|
|
|
object lockIOMap = new object();
|
|
byte[] iomap = new byte[8192];
|
|
|
|
void SetupPDOMapping()
|
|
{
|
|
IOMapSize = ECMBind.ecmbind_config_map();
|
|
Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
|
|
//UpdatePDOMap();
|
|
}
|
|
|
|
HashSet<PDOMappingRequest> mappingRequests = new HashSet<PDOMappingRequest>();
|
|
|
|
public void ConfigureProcessDataMappings()
|
|
{
|
|
Logging.Log(LogLevel.INFO, "configure PDO mappings");
|
|
|
|
for (UInt16 slave = 1; slave <= CountSlaves; slave++)
|
|
{
|
|
List<int> rxPDOs = new List<int>();
|
|
List<int> txPDOs = new List<int>();
|
|
|
|
foreach (PDOMappingRequest mappingRequest in mappingRequests)
|
|
{
|
|
if (mappingRequest.Slave == slave)
|
|
{
|
|
if (GetSDOValue(mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex, out SDOValue sdoValue))
|
|
{
|
|
int mappingEntry = (mappingRequest.Index << 16) | (mappingRequest.SubIndex << 8) | sdoValue.BitLength;
|
|
Logging.Log(LogLevel.DEBUG, "PDO mapping: {0:X8}", mappingEntry);
|
|
|
|
if (mappingRequest.RxPDO)
|
|
rxPDOs.Add(mappingEntry);
|
|
else
|
|
txPDOs.Add(mappingEntry);
|
|
|
|
} else {
|
|
Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not retrieve SDOValue {0}:{1:X4}.{2}", mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex);
|
|
}
|
|
}
|
|
}
|
|
|
|
MemoryStream rxStream = new MemoryStream();
|
|
rxStream.WriteByte((byte)rxPDOs.Count);
|
|
rxStream.WriteByte(0);
|
|
foreach (int pdoMapping in rxPDOs)
|
|
rxStream.WriteBytes(pdoMapping.GetBytes());
|
|
|
|
Logging.Log(LogLevel.DEBUG, "RxPDO mappings for slave {0} => {1}", slave, rxPDOs.Count);
|
|
|
|
if (!WriteSDOCA(slave, 0x1600, 0, rxStream.ToArray()))
|
|
{
|
|
Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write RxPDO mappings for slave {0}", slave);
|
|
}
|
|
|
|
MemoryStream txStream = new MemoryStream();
|
|
txStream.WriteByte((byte)txPDOs.Count);
|
|
txStream.WriteByte(0);
|
|
foreach (int pdoMapping in txPDOs)
|
|
txStream.WriteBytes(pdoMapping.GetBytes());
|
|
|
|
Logging.Log(LogLevel.DEBUG, "TxPDO mappings for slave {0} => {1}", slave, txPDOs.Count);
|
|
|
|
if (!WriteSDOCA(slave, 0x1A00, 0, txStream.ToArray()))
|
|
{
|
|
Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write TxPDO mappings for slave {0}", slave);
|
|
}
|
|
|
|
if (!WriteSDOCA(slave, 0x1c12, 0, new byte[]{ 0x01, 0x00, 0x00, 0x16 }))
|
|
{
|
|
Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 2 PDO assignment for slave {0}", slave);
|
|
}
|
|
if (!WriteSDOCA(slave, 0x1c13, 0, new byte[]{ 0x01, 0x00, 0x00, 0x1A }))
|
|
{
|
|
Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 3 PDO assignment for slave {0}", slave);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void RequestPDOMapping(UInt16 slave,UInt16 index,byte subIndex, bool RxPDO) => RequestPDOMapping(new PDOMappingRequest(slave,index,subIndex,RxPDO));
|
|
public void RequestPDOMapping(IEnumerable<PDOMappingRequest> mappingRequests) => RequestPDOMapping(mappingRequests.ToArray());
|
|
public void RequestPDOMapping(params PDOMappingRequest[] mappingRequests)
|
|
{
|
|
foreach (PDOMappingRequest mappingRequest in mappingRequests)
|
|
this.mappingRequests.Add(mappingRequest);
|
|
}
|
|
|
|
|
|
void ScheduleRestart()
|
|
{
|
|
ThreadPool.QueueUserWorkItem((o)=>{
|
|
Stop();
|
|
Thread.Sleep(500);
|
|
Start();
|
|
});
|
|
}
|
|
|
|
|
|
|
|
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}) [{5}] == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString(), dataSize);
|
|
throw new IOException();
|
|
}
|
|
}
|
|
rawValue = rawValue.Slice(0, dataSize);
|
|
return true;
|
|
}
|
|
|
|
public bool ReadSDOCA(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
|
|
{
|
|
rawValue =
|
|
new byte[128];
|
|
int dataSize;
|
|
|
|
lock (lockIOMap)
|
|
{
|
|
dataSize =
|
|
ECMBind.ecmbind_sdo_read_ca(slave, index, subIndex, rawValue, rawValue.Length );
|
|
if (dataSize <= 0)
|
|
{
|
|
Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read_ca({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 bool WriteSDOCA(UInt16 slave,UInt16 index, byte subIndex, byte[] rawValue)
|
|
{
|
|
int result;
|
|
lock (lockIOMap)
|
|
result = ECMBind.ecmbind_sdo_write_ca(slave, index, subIndex, rawValue, rawValue.Length);
|
|
//Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDOCA({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
|
|
return result > 0;
|
|
}
|
|
|
|
public ECSlaveState ReadSlaveState(int slave) => IOLocked(()=>ECMBind.ecmbind_get_slave_state(slave));
|
|
int RequestSlaveState(int slave, ECSlaveState slaveState) => IOLocked(()=>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);
|
|
//Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1})", 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>();
|
|
lock (lockIOMap)
|
|
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 (lockIOMap)
|
|
{
|
|
IOMapPtr = ECMBind.ecmbind_get_iomap();
|
|
pdoMap = pdoList;
|
|
}
|
|
}
|
|
|
|
|
|
T IOLocked<T>(Func<T> f) {
|
|
lock (lockIOMap)
|
|
return f();
|
|
}
|
|
}
|
|
} |