private void ConfigureSlaves(IList <SlaveInfo> slaves)
{
var callbacks = new List <EcHL.PO2SOCallback>();
foreach (var slave in slaves)
{
// SDO / PDO config / PDO assign
var currentSlaveIndex = (ushort)(Convert.ToUInt16(slaves.ToList().IndexOf(slave)) + 1);
var extensions = slave.Extensions;
var sdoWriteRequests = slave.GetConfiguration(extensions).ToList();
EcHL.PO2SOCallback callback = slaveIndex =>
{
sdoWriteRequests.ToList().ForEach(sdoWriteRequest =>
{
EcUtilities.CheckErrorCode(this.Context, EcUtilities.SdoWrite(this.Context, slaveIndex, sdoWriteRequest.Index, sdoWriteRequest.SubIndex, sdoWriteRequest.Dataset), nameof(EcHL.SdoWrite));
});
return(0);
};
EcHL.RegisterCallback(this.Context, currentSlaveIndex, callback);
callbacks.Add(callback);
}
callbacks.ForEach(callback =>
{
GC.KeepAlive(callback);
});
}
public static SlaveInfo ReloadHardware(string esiDirectoryPath, IExtensionFactory extensionFactory, string interfaceName, SlaveInfo referenceRootSlaveInfo)
{
IntPtr context;
SlaveInfo newRootSlaveInfo;
SlaveInfo referenceSlaveInfo;
IEnumerable <SlaveInfo> referenceSlaveInfoSet;
referenceSlaveInfo = null;
referenceSlaveInfoSet = null;
if (NetworkInterface.GetAllNetworkInterfaces().Where(x => x.GetPhysicalAddress().ToString() == interfaceName).FirstOrDefault()?.OperationalStatus != OperationalStatus.Up)
{
throw new Exception($"The network interface '{interfaceName}' is not linked. Aborting action.");
}
context = EcHL.CreateContext();
newRootSlaveInfo = EcUtilities.ScanDevices(context, interfaceName, referenceRootSlaveInfo);
EcHL.FreeContext(context);
if (referenceRootSlaveInfo != null)
{
referenceSlaveInfoSet = referenceRootSlaveInfo.Descendants().ToList();
}
newRootSlaveInfo.Descendants().ToList().ForEach(slaveInfo =>
{
referenceSlaveInfo = slaveInfo.Csa == slaveInfo.OldCsa ? referenceSlaveInfoSet?.FirstOrDefault(x => x.Csa == slaveInfo.Csa) : null;
ExtensibilityHelper.GetDynamicSlaveInfoData(esiDirectoryPath, extensionFactory, slaveInfo);
ExtensibilityHelper.UpdateSlaveExtensions(extensionFactory, slaveInfo, referenceSlaveInfo);
});
return(newRootSlaveInfo);
}
public static SlavePdo[] UploadPdoConfig(IntPtr context, UInt16 slave, UInt16 smIndex)
{
int pdoCount;
IntPtr ecPdoInfoPtrSet;
SlavePdo[] slavePdoSet;
SyncManagerType syncManagerType;
DataDirection dataDirection;
//
EcHL.GetSyncManagerType(context, slave, smIndex, out syncManagerType);
switch (syncManagerType)
{
case SyncManagerType.Inputs:
dataDirection = DataDirection.Input;
break;
case SyncManagerType.Outputs:
dataDirection = DataDirection.Output;
break;
default:
throw new ArgumentException();
}
EcHL.UploadPdoConfig(context, slave, smIndex, out ecPdoInfoPtrSet, out pdoCount);
slavePdoSet = Enumerable.Range(0, pdoCount).Select(index =>
{
ec_pdo_info_t ecPdoInfo;
SlavePdo slavePdo;
IntPtr ecPdoInfoPtr;
ecPdoInfoPtr = IntPtr.Add(ecPdoInfoPtrSet, index * Marshal.SizeOf(typeof(ec_pdo_info_t)));
ecPdoInfo = Marshal.PtrToStructure <ec_pdo_info_t>(ecPdoInfoPtr);
slavePdo = new SlavePdo(null, ecPdoInfo.name, ecPdoInfo.index, 0, true, true, smIndex - 0x1C10);
slavePdo.SetVariableSet(Enumerable.Range(0, ecPdoInfo.variableCount).Select(index2 =>
{
ec_variable_info_t ecVariableInfo;
SlaveVariable slaveVariable;
IntPtr ecVariableInfoPtr;
ecVariableInfoPtr = IntPtr.Add(ecPdoInfo.variableInfoSet, index2 * Marshal.SizeOf(typeof(ec_variable_info_t)));
ecVariableInfo = Marshal.PtrToStructure <ec_variable_info_t>(ecVariableInfoPtr);
slaveVariable = new SlaveVariable(slavePdo, ecVariableInfo.name, ecVariableInfo.index, ecVariableInfo.subIndex, dataDirection, EcUtilities.GetOneDasDataTypeFromEthercatDataType(ecVariableInfo.dataType));
return(slaveVariable);
}).ToList());
EcHL.Free(ecPdoInfo.variableInfoSet);
return(slavePdo);
}).ToArray();
EcHL.Free(ecPdoInfoPtrSet);
return(slavePdoSet);
}
protected virtual void Dispose(bool disposing)
{
if (!disposedValue)
{
if (_ioMapPtr != IntPtr.Zero)
{
Marshal.FreeHGlobal(_ioMapPtr);
}
_cts?.Cancel();
try
{
_watchdogTask?.Wait();
}
catch (Exception ex) when(ex.InnerException.GetType() == typeof(TaskCanceledException))
{
//
}
if (this.Context != IntPtr.Zero)
{
EcHL.FreeContext(this.Context);
this.Context = IntPtr.Zero;
}
disposedValue = true;
}
}
public EcMaster(EcSettings settings, ILogger logger)
{
_settings = settings.ShallowCopy();
_logger = logger;
this.Context = EcHL.CreateContext();
// DC
_dcRingBuffer = new long[_dcRingBufferSize];
_dcEpoch = new DateTime(2000, 1, 1);
_dcDriftCompensationRate = Convert.ToInt32(_settings.DriftCompensationRate / _settings.CycleFrequency); // 850 ppm max clock drift
// data
_ioMapPtr = Marshal.AllocHGlobal(_settings.IoMapLength);
unsafe
{
new Span <byte>(_ioMapPtr.ToPointer(), _settings.IoMapLength).Clear();
}
// diagnostics
_isReconfiguring = false;
_lock = new object();
_cts = new CancellationTokenSource();
}
private void ConfigureSync01(IList <SlaveInfo> slaveInfoSet)
{
// SYNC0 / SYNC1
foreach (SlaveInfo slaveInfo in slaveInfoSet)
{
ushort slaveIndex;
DistributedClocksSettings distributedClocksSettings;
slaveIndex = (ushort)(Convert.ToUInt16(slaveInfoSet.ToList().IndexOf(slaveInfo)) + 1);
distributedClocksSettings = slaveInfo.SlaveExtensionSet.OfType <DistributedClocksSettings>().ToList().FirstOrDefault();
if (distributedClocksSettings != null)
{
DistributedClocksParameters parameters;
byte[] assignActivate;
if (!slaveInfo.SlaveEsi.Dc.TimeLoopControlOnly)
{
assignActivate = null;
parameters = distributedClocksSettings.CalculateDcParameters(ref assignActivate, _settings.CycleFrequency);
EcUtilities.CheckErrorCode(this.Context, EcHL.ConfigureSync01(this.Context, slaveIndex, ref assignActivate, assignActivate.Count(), parameters.CycleTime0, parameters.CycleTime1, parameters.ShiftTime0));
}
}
}
}
/// <summary>
/// Initializes EtherCAT and returns found slaves.
/// </summary>
/// <param name="interfaceName">The name of the network adapter.</param>
/// <returns>Returns found slave.</returns>
public static SlaveInfo ScanDevices(IntPtr context, string interfaceName, SlaveInfo referenceSlave = null)
{
ec_slave_info_t[] refSlaveIdentifications = null;
if (referenceSlave != null)
{
refSlaveIdentifications = EcUtilities.ToSlaveIdentifications(referenceSlave);
}
else
{
refSlaveIdentifications = new ec_slave_info_t[] { }
};
// scan devices
var networkInterface = NetworkInterface
.GetAllNetworkInterfaces()
.Where(nic => nic.Name == interfaceName)
.FirstOrDefault();
if (networkInterface == null)
{
throw new Exception($"{ ErrorMessage.SoemWrapper_NetworkInterfaceNotFound } Interface name: '{ interfaceName }'.");
}
if (RuntimeInformation.IsOSPlatform(OSPlatform.Windows))
{
interfaceName = $@"rpcap://\Device\NPF_{networkInterface.Id}";
}
else if (RuntimeInformation.IsOSPlatform(OSPlatform.Linux) ||
RuntimeInformation.IsOSPlatform(OSPlatform.OSX))
{
interfaceName = $"{interfaceName}";
}
else
{
throw new PlatformNotSupportedException();
}
EcUtilities.CheckErrorCode(context, EcHL.ScanDevices(context, interfaceName, out var slaveIdentifications, out var slaveCount));
// create slaveInfo from received data
var offset = 0;
var newSlaveIdentifications = new ec_slave_info_t[slaveCount + 1]; // correct because EC master = slaveIdentifications[0]
for (int i = 0; i <= newSlaveIdentifications.Count() - 1; i++)
{
newSlaveIdentifications[i] = Marshal.PtrToStructure <ec_slave_info_t>(IntPtr.Add(slaveIdentifications, offset));
offset += Marshal.SizeOf(typeof(ec_slave_info_t));
}
// validate CSA
while (EcUtilities.EnsureValidCsa(context, newSlaveIdentifications, refSlaveIdentifications))
{
//
}
return(EcUtilities.ToSlaveInfo(newSlaveIdentifications));
}
/// <summary>
/// Create virtual network device.
/// </summary>
/// <param name="interfaceName">Virtual device interface name.</param>
/// <returns>Device Id of virtual network device.</returns>
public int CreateVirtualNetworkDevice(string interfaceName, out string interfaceNameSet)
{
int deviceId = 0;
IntPtr ptrInterfaceNameSet = EcHL.CreateVirtualNetworkDevice(interfaceName, out deviceId);
interfaceNameSet = Marshal.PtrToStringAnsi(ptrInterfaceNameSet);
return(deviceId);
}
private void ConfigureDc()
{
uint systemTimeDifference;
EcUtilities.CheckErrorCode(this.Context, EcHL.ConfigureDc(this.Context, _settings.FrameCount, _settings.TargetTimeDifference, out systemTimeDifference), nameof(EcHL.ConfigureDc));
_logger.LogInformation($"DC system time diff. is <= { systemTimeDifference & 0x7FFF } ns");
}
/// <summary>
/// Create virtual serial port.
/// </summary>
/// <param name="slaveTerminalName">Name of virtual serial port.</param>
/// <returns>Device Id of virtual serial port.</returns>
public int CreateVirtualSerialPort(out string slaveTerminalName)
{
int deviceId = 0;
IntPtr ptrSlaveTerminalName = EcHL.CreateVirtualSerialPort(out deviceId);
slaveTerminalName = Marshal.PtrToStringAnsi(ptrSlaveTerminalName);
return(deviceId);
}
public void Configure(SlaveInfo rootSlaveInfo = null)
{
SlaveInfo actualSlaveInfo;
IList <SlaveInfo> slaveInfoSet;
IList <SlaveInfo> actualSlaveInfoSet;
NetworkInterface networkInterface;
networkInterface = NetworkInterface.GetAllNetworkInterfaces().Where(nic => nic.Name == _settings.InterfaceName).FirstOrDefault();
if (networkInterface?.OperationalStatus != OperationalStatus.Up)
{
throw new Exception($"Network interface '{_settings.InterfaceName}' is not linked. Aborting action.");
}
#region "PreOp"
actualSlaveInfo = EcUtilities.ScanDevices(this.Context, _settings.InterfaceName, null);
if (rootSlaveInfo == null)
{
rootSlaveInfo = actualSlaveInfo;
rootSlaveInfo.Descendants().ToList().ForEach(current =>
{
ExtensibilityHelper.CreateDynamicData(_settings.EsiDirectoryPath, _extensionFactory, current);
});
}
slaveInfoSet = rootSlaveInfo.Descendants().ToList();
actualSlaveInfoSet = actualSlaveInfo.Descendants().ToList();
this.ValidateSlaves(slaveInfoSet, actualSlaveInfoSet);
this.ConfigureSlaves(slaveInfoSet);
this.ConfigureIoMap(slaveInfoSet);
this.ConfigureDc();
this.ConfigureSync01(slaveInfoSet);
#endregion
#region "SafeOp"
EcUtilities.CheckErrorCode(this.Context, EcHL.CheckSafeOpState(this.Context), nameof(EcHL.CheckSafeOpState));
#endregion
#region "Op"
EcUtilities.CheckErrorCode(this.Context, EcHL.RequestOpState(this.Context), nameof(EcHL.RequestOpState));
#endregion
if (_watchdogTask == null)
{
_watchdogTask = Task.Run(() => this.WatchdogRoutine(), _cts.Token);
}
}
public static int SdoRead(IntPtr context, UInt16 slaveIndex, UInt16 sdoIndex, byte sdoSubIndex, ref byte[] dataset)
{
return(EcHL.NoCaSdoRead(
context,
slaveIndex,
sdoIndex,
sdoSubIndex,
dataset
));
}
public void Configure(SlaveInfo rootSlave = null)
{
var networkInterface = NetworkInterface
.GetAllNetworkInterfaces()
.Where(nic => nic.Name == _settings.InterfaceName)
.FirstOrDefault();
if (networkInterface?.OperationalStatus != OperationalStatus.Up)
{
throw new Exception($"Network interface '{_settings.InterfaceName}' is not linked. Aborting action.");
}
#region "PreOp"
var actualSlave = EcUtilities.ScanDevices(this.Context, _settings.InterfaceName, null);
if (rootSlave == null)
{
rootSlave = actualSlave;
rootSlave.Descendants().ToList().ForEach(current =>
{
EcUtilities.CreateDynamicData(_settings.EsiDirectoryPath, current);
});
}
var slaves = rootSlave.Descendants().ToList();
var actualSlaves = actualSlave.Descendants().ToList();
this.ValidateSlaves(slaves, actualSlaves);
this.ConfigureSlaves(slaves);
this.ConfigureIoMap(slaves);
this.ConfigureDc();
this.ConfigureSync01(slaves);
#endregion
#region "SafeOp"
EcUtilities.CheckErrorCode(this.Context, EcHL.CheckSafeOpState(this.Context), nameof(EcHL.CheckSafeOpState));
#endregion
#region "Op"
EcUtilities.CheckErrorCode(this.Context, EcHL.RequestCommonState(this.Context, (UInt16)SlaveState.Operational), nameof(EcHL.RequestCommonState));
#endregion
if (_watchdogTask == null)
{
_watchdogTask = Task.Run(() => this.WatchdogRoutine(), _cts.Token);
}
}
private void ConfigureIoMap(IList <SlaveInfo> slaves)
{
var ioMapByteOffset = 0;
var ioMapBitOffset = 0;
var slavePdoOffsets = default(int[]);
var slaveRxPdoOffsets = new int[slaves.Count() + 1];
var slaveTxPdoOffsets = new int[slaves.Count() + 1];
_actualIoMapSize = EcHL.ConfigureIoMap(this.Context, _ioMapPtr, slaveRxPdoOffsets, slaveTxPdoOffsets, out _expectedWorkingCounter);
foreach (DataDirection dataDirection in Enum.GetValues(typeof(DataDirection)))
{
switch (dataDirection)
{
case DataDirection.Output:
slavePdoOffsets = slaveRxPdoOffsets;
break;
case DataDirection.Input:
slavePdoOffsets = slaveTxPdoOffsets;
break;
default:
throw new NotImplementedException();
}
foreach (var slave in slaves)
{
ioMapByteOffset = slavePdoOffsets[slaves.ToList().IndexOf(slave) + 1];
foreach (var variable in slave.DynamicData.Pdos.Where(x => x.SyncManager >= 0).ToList().SelectMany(x => x.Variables).ToList().Where(x => x.DataDirection == dataDirection))
{
variable.DataPtr = IntPtr.Add(_ioMapPtr, ioMapByteOffset);
variable.BitOffset = ioMapBitOffset;
if (variable.DataType == EthercatDataType.Boolean)
{
variable.BitOffset = ioMapBitOffset; // bool is treated as bit-oriented
}
Debug.WriteLine($"{variable.Name} {variable.DataPtr.ToInt64() - _ioMapPtr.ToInt64()}/{variable.BitOffset}");
ioMapBitOffset += variable.BitLength;
if (ioMapBitOffset > 7)
{
ioMapBitOffset = ioMapBitOffset % 8;
ioMapByteOffset += (variable.BitLength + 7) / 8;
}
}
}
}
_logger.LogInformation($"IO map configured ({slaves.Count()} {(slaves.Count() > 1 ? "slaves" : "slave")}, {_actualIoMapSize} bytes)");
}
public static SlaveInfo ScanDevices(string interfaceName, SlaveInfo referenceSlaveInfo = null)
{
IntPtr context;
SlaveInfo slaveInfo;
context = EcHL.CreateContext();
slaveInfo = EcUtilities.ScanDevices(context, interfaceName, referenceSlaveInfo);
EcHL.FreeContext(context);
return(slaveInfo);
}
public static int SdoWrite(IntPtr context, UInt16 slaveIndex, UInt16 sdoIndex, byte sdoSubIndex, IEnumerable <object> dataset)
{
return(EcHL.SdoWrite(
context,
slaveIndex,
sdoIndex,
sdoSubIndex,
dataset.SelectMany(value => value.ToByteArray()).ToArray(),
Convert.ToUInt32(dataset.Count()),
dataset.Select(data => Marshal.SizeOf(data)).ToArray()
));
}
private void WatchdogRoutine()
{
while (!_cts.IsCancellationRequested)
{
if (_watchDogActive)
{
var state = EcHL.ReadState(this.Context);
if (state < 8)
{
_statusCheckFailedCounter++;
if (_statusCheckFailedCounter >= _settings.MaxRetries)
{
try
{
lock (_lock)
{
_isReconfiguring = true;
_logger.LogInformation("reconfiguration started");
}
this.Configure();
_logger.LogInformation("reconfiguration successful");
_isReconfiguring = false;
}
catch (Exception)
{
_logger.LogWarning("reconfiguration failed");
}
finally
{
_statusCheckFailedCounter = 0;
}
}
}
else
{
if (_isReconfiguring)
{
_logger.LogInformation("communication restored");
_isReconfiguring = false;
}
_statusCheckFailedCounter = 0;
}
}
_cts.Token.WaitHandle.WaitOne(TimeSpan.FromSeconds(_settings.WatchdogSleepTime));
}
}
public static SlaveInfo ScanDevices(string interfaceName, SlaveInfo referenceRootSlave = null)
{
if (NetworkInterface.GetAllNetworkInterfaces().Where(x => x.Name == interfaceName).FirstOrDefault()?.OperationalStatus != OperationalStatus.Up)
{
throw new Exception($"The network interface '{interfaceName}' is not linked. Aborting action.");
}
var context = EcHL.CreateContext();
var rootSlave = EcUtilities.ScanDevices(context, interfaceName, referenceRootSlave);
EcHL.FreeContext(context);
return(rootSlave);
}
public static SlavePdo[] UploadPdoConfig(IntPtr context, UInt16 slave, UInt16 smIndex)
{
EcHL.GetSyncManagerType(context, slave, smIndex, out var syncManagerType);
DataDirection dataDirection;
switch (syncManagerType)
{
case SyncManagerType.Inputs:
dataDirection = DataDirection.Input;
break;
case SyncManagerType.Outputs:
dataDirection = DataDirection.Output;
break;
default:
throw new ArgumentException();
}
EcHL.UploadPdoConfig(context, slave, smIndex, out var ecPdoInfoPtrs, out var pdoCount);
var slavePdos = Enumerable.Range(0, pdoCount).Select(index =>
{
var ecPdoInfoPtr = IntPtr.Add(ecPdoInfoPtrs, index * Marshal.SizeOf(typeof(ec_pdo_info_t)));
var ecPdoInfo = Marshal.PtrToStructure <ec_pdo_info_t>(ecPdoInfoPtr);
var slavePdo = new SlavePdo(null, ecPdoInfo.name, ecPdoInfo.index, 0, true, true, smIndex - 0x1C10);
slavePdo.SetVariables(Enumerable.Range(0, ecPdoInfo.variableCount).Select(index2 =>
{
var ecVariableInfoPtr = IntPtr.Add(ecPdoInfo.variableInfos, index2 * Marshal.SizeOf(typeof(ec_variable_info_t)));
var ecVariableInfo = Marshal.PtrToStructure <ec_variable_info_t>(ecVariableInfoPtr);
var slaveVariable = new SlaveVariable(slavePdo, ecVariableInfo.name, ecVariableInfo.index, ecVariableInfo.subIndex, dataDirection, ecVariableInfo.dataType);
return(slaveVariable);
}).ToList());
EcHL.Free(ecPdoInfo.variableInfos);
return(slavePdo);
}).ToArray();
EcHL.Free(ecPdoInfoPtrs);
return(slavePdos);
}
public void CanAccessNativeLib()
{
// Arrange
Directory.EnumerateFiles("./runtimes/", "*soem_wrapper.*", SearchOption.AllDirectories).ToList().ForEach(filePath =>
{
if (filePath.Contains(RuntimeEnvironment.RuntimeArchitecture))
{
File.Copy(filePath, Path.GetFileName(filePath), true);
}
});
// Act
var context = EcHL.CreateContext();
// Assert
Assert.True(context != IntPtr.Zero);
EcHL.FreeContext(context);
}
protected virtual void Dispose(bool disposing)
{
if (!disposedValue)
{
if (_ioMapPtr != IntPtr.Zero)
{
Marshal.FreeHGlobal(_ioMapPtr);
}
_cts?.Cancel();
_watchdogTask?.Wait();
if (this.Context != IntPtr.Zero)
{
EcHL.FreeContext(this.Context);
this.Context = IntPtr.Zero;
}
disposedValue = true;
}
}
/// <summary>
/// Checks if unmanaged status code represents an error. If true this error will be thrown.
/// </summary>
/// <param name="errorCode">The error code.</param>
/// <param name="callerMemberName">The name of the calling function.</param>
public static void CheckErrorCode(IntPtr context, int errorCode, [CallerMemberName()] string callerMemberName = "")
{
if (errorCode <= 0)
{
string message_server;
string message_SOEM = string.Empty;
string message_combined = string.Empty;
errorCode = -errorCode;
// message_server
message_server = ErrorMessage.ResourceManager.GetString($"Native_0x{ errorCode.ToString("X4") }");
if (string.IsNullOrWhiteSpace(message_server))
{
message_server = ErrorMessage.Native_0xFFFF;
}
// message_SOEM
while (EcHL.HasEcError(context))
{
if (!string.IsNullOrWhiteSpace(message_SOEM))
{
message_SOEM += "\n";
}
message_SOEM += Marshal.PtrToStringAnsi(EcHL.GetNextError(context));
}
// message_combined
message_combined = $"{ callerMemberName } failed (0x{ errorCode.ToString("X4") }): { message_server }";
if (!string.IsNullOrWhiteSpace(message_SOEM))
{
message_combined += $"\n\nEtherCAT message:\n\n{ message_SOEM }";
}
throw new Exception(message_combined);
}
}
public static string GetSlaveStateDescription(IntPtr context, IEnumerable <SlaveInfo> slaves)
{
var slaveStateDescription = new StringBuilder();
foreach (var slave in slaves)
{
var slaveIndex = Convert.ToUInt16(slaves.ToList().IndexOf(slave) + 1);
// Since the registers 0x092c and 0x0932 cannot be read always, the value will be reset before
var systemTimeDifference = int.MaxValue;
var speedCounterDifference = ushort.MaxValue;
ushort requestedState = 0;
ushort actualState = 0;
ushort alStatusCode = 0;
ushort outputPdoCount = 0;
ushort inputPdoCount = 0;
var returnValue = EcHL.ReadSlaveState(context, slaveIndex, ref requestedState, ref actualState, ref alStatusCode, ref systemTimeDifference, ref speedCounterDifference, ref outputPdoCount, ref inputPdoCount);
// ActualState <> RequestedState OrElse AlStatusCode > 0x0 Then
if (true)
{
if (returnValue < 0)
{
slaveStateDescription.AppendLine($"-- Error reading data ({slave.DynamicData.Name}) --");
}
else
{
string hasCompleteAccess = slave.Esi.Mailbox?.CoE?.CompleteAccess == true
? " True"
: "False";
slaveStateDescription.AppendLine($"Slave {slaveIndex,3} | CA: {hasCompleteAccess} | Req-State: 0x{requestedState:X4} | Act-State: 0x{actualState:X4} | AL-Status: 0x{alStatusCode:X4} | Sys-Time Diff: 0x{systemTimeDifference:X8} | Speed Counter Diff: 0x{speedCounterDifference:X4} | #Pdo out: {outputPdoCount} | #Pdo in: {inputPdoCount} | ({slave.DynamicData.Name})");
}
}
}
return(slaveStateDescription.ToString());
}
private void ConfigureSlaves(IList <SlaveInfo> slaveInfoSet)
{
List <EcHL.PO2SOCallback> callbackSet;
callbackSet = new List <EcHL.PO2SOCallback>();
foreach (SlaveInfo slaveInfo in slaveInfoSet)
{
ushort currentSlaveIndex;
IEnumerable <SlaveExtensionLogic> extensionSet;
IEnumerable <SdoWriteRequest> sdoWriteRequestSet;
EcHL.PO2SOCallback callback;
// SDO / PDO config / PDO assign
currentSlaveIndex = (ushort)(Convert.ToUInt16(slaveInfoSet.ToList().IndexOf(slaveInfo)) + 1);
extensionSet = slaveInfo.SlaveExtensionSet.Select(slaveExtension => _extensionFactory.BuildLogic <SlaveExtensionLogic>(slaveExtension)).ToList();
sdoWriteRequestSet = slaveInfo.GetConfiguration(extensionSet).ToList();
callback = slaveIndex =>
{
sdoWriteRequestSet.ToList().ForEach(sdoWriteRequest =>
{
EcUtilities.CheckErrorCode(this.Context, EcUtilities.SdoWrite(this.Context, slaveIndex, sdoWriteRequest.Index, sdoWriteRequest.SubIndex, sdoWriteRequest.Dataset), nameof(EcHL.SdoWrite));
});
return(0);
};
EcHL.RegisterCallback(this.Context, currentSlaveIndex, callback);
callbackSet.Add(callback);
}
callbackSet.ForEach(callback =>
{
GC.KeepAlive(callback);
});
}
private void ConfigureSync01(IList <SlaveInfo> slaves)
{
// SYNC0 / SYNC1
foreach (var slave in slaves)
{
var slaveIndex = (ushort)(Convert.ToUInt16(slaves.ToList().IndexOf(slave)) + 1);
var distributedClocksSettings = slave
.Extensions
.OfType <DistributedClocksExtension>()
.ToList()
.FirstOrDefault();
if (distributedClocksSettings != null)
{
if (!slave.Esi.Dc.TimeLoopControlOnly)
{
byte[] assignActivate = null;
var parameters = distributedClocksSettings.CalculateDcParameters(ref assignActivate, _settings.CycleFrequency);
EcUtilities.CheckErrorCode(this.Context, EcHL.ConfigureSync01(this.Context, slaveIndex, ref assignActivate, assignActivate.Count(), parameters.CycleTime0, parameters.CycleTime1, parameters.ShiftTime0));
}
}
}
}
/// <summary>
/// Return current state of slave.
/// </summary>
/// <param name="slaveIndex">Slave index.</param>
/// <returns>Current slave state./returns>
public SlaveState GetState(int slaveIndex)
{
ushort slaveState = EcHL.GetState(this.Context, slaveIndex);
return((SlaveState)slaveState);
}
public void UpdateIO(DateTime referenceDateTime)
{
lock (_lock)
{
if (_isReconfiguring)
{
return;
}
_counter = (int)((_counter + 1) % _settings.CycleFrequency);
_actualWorkingCounter = EcHL.UpdateIo(this.Context, out _dcTime);
#region "Diagnostics"
// statistics
if (_counter == 0)
{
Trace.WriteLine($"lost frames: {(double)_lostFrameCounter / _settings.CycleFrequency:P2} / wkc mismatch: {(double)_wkcMismatchCounter / _settings.CycleFrequency:P2}");
if (_lostFrameCounter == _settings.CycleFrequency)
{
_logger.LogWarning($"frame loss occured ({ _settings.CycleFrequency } frames)");
_lostFrameCounter = 0;
}
if (_wkcMismatchCounter == _settings.CycleFrequency)
{
_logger.LogWarning($"working counter mismatch { _actualWorkingCounter }/{ _expectedWorkingCounter }");
//Trace.WriteLine(EcUtilities.GetSlaveStateDescription(_ecSettings.RootSlaves.SelectMany(x => x.Descendants()).ToList()));
_wkcMismatchCounter = 0;
}
_lostFrameCounter = 0;
_wkcMismatchCounter = 0;
}
// no frame
if (_actualWorkingCounter == -1)
{
_lostFrameCounter += 1;
this.UtcDateTime = DateTime.MinValue;
this.DcRingBufferAverage = 0;
return;
}
// working counter mismatch
if (_expectedWorkingCounter != _actualWorkingCounter)
{
_wkcMismatchCounter += 1;
this.UtcDateTime = DateTime.MinValue;
this.DcRingBufferAverage = 0;
return;
}
#endregion
// the UpdateIo timer tries to fire at discrete times. The timer is allowed to be early or delayed by < (CycleTime - Offset) and the resulting DC time will be floored to nearest 10 ms.
this.UtcDateTime = _dcEpoch.AddTicks(Convert.ToInt64(_dcTime / _dateTime_To_Ns)); // for time loop control
// for compensation, if DC is not initialized with real time or not initialized at all
if (_offset == TimeSpan.Zero)
{
_offset = referenceDateTime - this.UtcDateTime;
}
else
{
this.UtcDateTime += _offset;
}
// dc drift compensation
_dcRingBuffer[_dcRingBufferIndex] = Convert.ToInt64(referenceDateTime.Ticks - this.UtcDateTime.Ticks) * _dateTime_To_Ns;
this.DcRingBufferAverage = Convert.ToInt64(_dcRingBuffer.Average());
if (!_isDcCompensationRunning && Math.Abs(this.DcRingBufferAverage) > 1500000) // 1.5 ms
{
_isDcCompensationRunning = true;
_logger.LogInformation("DC drift compensation started");
}
else if (_isDcCompensationRunning && Math.Abs(this.DcRingBufferAverage) < 1000000) // 1.0 ms
{
_isDcCompensationRunning = false;
_logger.LogInformation("DC drift compensation finished");
}
if (_isDcCompensationRunning)
{
EcHL.CompensateDcDrift(this.Context, Convert.ToInt32(Math.Min(Math.Max(this.DcRingBufferAverage, -_dcDriftCompensationRate), _dcDriftCompensationRate)));
}
_dcRingBufferIndex = (_dcRingBufferIndex + 1) % _dcRingBufferSize;
}
}
/// <summary>
/// Update serial handshake processing for slave device.
/// </summary>
/// <param name="slaveIndex">The index of the corresponding slave.</param>
public void UpdateSerialIo(int slaveIndex)
{
EcHL.UpdateSerialIo(this.Context, slaveIndex);
}
/// <summary>
/// Request slave state transition.
/// </summary>
/// <param name="slaveIndex">Slave index.</param>
/// <param name="slaveState">Slave target state.</param>
/// <returns>True if transition was successful, false otherwise./returns>
public bool RequestState(int slaveIndex, SlaveState slaveState)
{
UInt16 stateSet = EcHL.RequestState(this.Context, slaveIndex, (UInt16)slaveState);
return((SlaveState)stateSet == slaveState);
}
/// <summary>
/// Download firmware file to slave.
/// 1. All detected slaves are set to PREOP state.
/// 2. Target slave is set to INIT state.
/// 3. Target slave is set to BOOT state.
/// 4. Firmware file is downloaded to target slave.
/// 5. Target slave is set to INIT state regardless of whether
/// the file download was successful or not.
/// </summary>
/// <param name="slaveIndex">Slave index.</param>
/// <param name="fileName">Absolute path to firmware file.</param>
/// <returns>True if operation was successful, false otherwise./returns>
public bool DownloadFirmware(int slaveIndex, string fileName)
{
FileInfo fileInfo = new FileInfo(fileName);
if (!fileInfo.Exists)
{
return(false);
}
bool success = false;
if (EcHL.RequestCommonState(this.Context, (UInt16)SlaveState.PreOp) == 1)
{
UInt16 currentState = EcHL.RequestState(this.Context, slaveIndex, (UInt16)SlaveState.Init);
if (currentState == (UInt16)SlaveState.Init)
{
currentState = EcHL.RequestState(this.Context, slaveIndex, (UInt16)SlaveState.Boot);
if (currentState == (UInt16)SlaveState.Boot)
{
int currentPackageNumber = -1;
int totalPackages = 0;
int remainingSize = -1;
EcHL.FOECallback callback = (slaveIndex, packageNumber, datasize) =>
{
if (packageNumber == 0)
{
_logger.LogInformation($"FoE: Write {datasize} bytes to {slaveIndex}. slave");
}
else
{
_logger.LogInformation($"FoE: {packageNumber}. package with {remainingSize - datasize} bytes written to {slaveIndex}. slave. Remaining data: {datasize} bytes");
}
if (currentPackageNumber != packageNumber)
{
currentPackageNumber = packageNumber;
if (packageNumber != 0)
{
totalPackages++;
}
}
remainingSize = datasize;
return(0);
};
EcHL.RegisterFOECallback(this.Context, callback);
GC.KeepAlive(callback);
int wk = EcHL.DownloadFirmware(this.Context, slaveIndex, fileName, (int)fileInfo.Length);
_logger.LogInformation($"FoE: {totalPackages} packages written");
success = (remainingSize == 0) && (wk > 0);
EcHL.RequestState(this.Context, slaveIndex, (UInt16)SlaveState.Init);
}
}
}
return(success);
}
