248 lines
10 KiB
C#
248 lines
10 KiB
C#
using Sick.SafetyScanners.DataStructures;
|
|
using Sick.SafetyScanners.Helpers;
|
|
|
|
namespace Sick.SafetyScanners.DataProcessing;
|
|
|
|
/// <summary>
|
|
/// Parser for application data block from UDP packets
|
|
/// Contains application inputs and outputs (local inputs/outputs, velocities, monitoring cases, etc.)
|
|
/// Thread-safe implementation
|
|
/// </summary>
|
|
public sealed class ParseApplicationData
|
|
{
|
|
/// <summary>
|
|
/// Parses the application data block from a UDP sequence
|
|
/// </summary>
|
|
/// <param name="buffer">Packet buffer containing the data</param>
|
|
/// <param name="header">Parsed data header (must contain valid block offset/size)</param>
|
|
public ApplicationData ParseUdpSequence(PacketBuffer buffer, DataHeader header)
|
|
{
|
|
if (buffer == null)
|
|
throw new ArgumentNullException(nameof(buffer));
|
|
|
|
if (header == null)
|
|
throw new ArgumentNullException(nameof(header));
|
|
|
|
// Check if application data block is enabled
|
|
if (!CheckIfApplicationDataIsPublished(header))
|
|
{
|
|
return new ApplicationData
|
|
{
|
|
IsEmpty = true
|
|
};
|
|
}
|
|
|
|
// Check if header is valid
|
|
if (header.IsEmpty)
|
|
{
|
|
return new ApplicationData
|
|
{
|
|
IsEmpty = true
|
|
};
|
|
}
|
|
|
|
var bufferData = buffer.GetBuffer();
|
|
var offset = header.ApplicationDataBlockOffset;
|
|
|
|
// Validate buffer size (at least 260 bytes for full application data block)
|
|
if (offset + 260 > bufferData.Length)
|
|
{
|
|
throw new ArgumentException(
|
|
$"Buffer too small to contain application data block (offset: {offset}, buffer size: {bufferData.Length})",
|
|
nameof(buffer));
|
|
}
|
|
|
|
var span = bufferData.Span.Slice(offset);
|
|
|
|
// Parse application inputs (offsets 0-74)
|
|
var inputs = ParseApplicationInputs(span);
|
|
|
|
// Parse application outputs (offsets 140-259)
|
|
var outputs = ParseApplicationOutputs(span);
|
|
|
|
return new ApplicationData
|
|
{
|
|
Inputs = inputs,
|
|
Outputs = outputs,
|
|
IsEmpty = false
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parses application inputs from the data block
|
|
/// Inputs span from offset 0 to approximately offset 74
|
|
/// </summary>
|
|
private static ApplicationInputs ParseApplicationInputs(ReadOnlySpan<byte> span)
|
|
{
|
|
// Parse unsafe inputs (offsets 0-7)
|
|
var unsafeInputsSources = ParseBitVector32(span, 0);
|
|
var unsafeInputsFlags = ParseBitVector32(span, 4);
|
|
|
|
// Parse monitoring case inputs (offsets 12-51)
|
|
var monitoringCases = new List<ushort>(20);
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
monitoringCases.Add(ReadWriteHelper.ReadUint16LittleEndian(span, 12 + i * 2));
|
|
}
|
|
|
|
// Parse monitoring case flags (offset 52)
|
|
var monitoringCaseFlags = ParseBitVector20(span, 52);
|
|
|
|
// Parse linear velocity inputs (offsets 56-60)
|
|
var velocity0 = (short)ReadWriteHelper.ReadUint16LittleEndian(span, 56);
|
|
var velocity1 = (short)ReadWriteHelper.ReadUint16LittleEndian(span, 58);
|
|
|
|
// Parse linear velocity flags (offset 60)
|
|
var velocityFlags = ReadWriteHelper.ReadUint8LittleEndian(span, 60);
|
|
var isVelocity0Valid = (velocityFlags & 0x01) != 0;
|
|
var isVelocity1Valid = (velocityFlags & 0x02) != 0;
|
|
// Bits 2,3 reserved
|
|
var isVelocity0TransmittedSafely = (velocityFlags & 0x10) != 0;
|
|
var isVelocity1TransmittedSafely = (velocityFlags & 0x20) != 0;
|
|
|
|
// Parse sleep mode input (offset 74)
|
|
var sleepModeInput = (sbyte)ReadWriteHelper.ReadUint8LittleEndian(span, 74);
|
|
|
|
return new ApplicationInputs
|
|
{
|
|
UnsafeInputsInputSources = unsafeInputsSources,
|
|
UnsafeInputsFlags = unsafeInputsFlags,
|
|
MonitoringCases = monitoringCases,
|
|
MonitoringCaseFlags = monitoringCaseFlags,
|
|
Velocity0 = velocity0,
|
|
Velocity1 = velocity1,
|
|
IsVelocity0Valid = isVelocity0Valid,
|
|
IsVelocity1Valid = isVelocity1Valid,
|
|
IsVelocity0TransmittedSafely = isVelocity0TransmittedSafely,
|
|
IsVelocity1TransmittedSafely = isVelocity1TransmittedSafely,
|
|
SleepModeInput = sleepModeInput
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parses application outputs from the data block
|
|
/// Outputs span from offset 140 to approximately offset 259
|
|
/// </summary>
|
|
private static ApplicationOutputs ParseApplicationOutputs(ReadOnlySpan<byte> span)
|
|
{
|
|
// Parse evaluation paths outputs (offsets 140-151)
|
|
var evalOut = ParseBitVector20(span, 140);
|
|
var evalOutIsSafe = ParseBitVector20(span, 144);
|
|
var evalOutIsValid = ParseBitVector20(span, 148);
|
|
|
|
// Parse monitoring case outputs (offsets 152-195)
|
|
var outputMonitoringCases = new List<ushort>(20);
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
outputMonitoringCases.Add(ReadWriteHelper.ReadUint16LittleEndian(span, 152 + i * 2));
|
|
}
|
|
|
|
var outputMonitoringCaseFlags = ParseBitVector20(span, 192);
|
|
|
|
// Parse sleep mode output (offset 193)
|
|
var sleepModeOutput = (sbyte)ReadWriteHelper.ReadUint8LittleEndian(span, 193);
|
|
|
|
// Parse error flags (offset 194)
|
|
var errorFlags = ReadWriteHelper.ReadUint8LittleEndian(span, 194);
|
|
var hostErrorFlagContaminationWarning = (errorFlags & 0x01) != 0;
|
|
var hostErrorFlagContaminationError = (errorFlags & 0x02) != 0;
|
|
var hostErrorFlagManipulationError = (errorFlags & 0x04) != 0;
|
|
var hostErrorFlagGlare = (errorFlags & 0x08) != 0;
|
|
var hostErrorFlagReferenceContourIntruded = (errorFlags & 0x10) != 0;
|
|
var hostErrorFlagCriticalError = (errorFlags & 0x20) != 0;
|
|
|
|
// Parse linear velocity outputs (offsets 200-204)
|
|
var outputVelocity0 = (short)ReadWriteHelper.ReadUint16LittleEndian(span, 200);
|
|
var outputVelocity1 = (short)ReadWriteHelper.ReadUint16LittleEndian(span, 202);
|
|
|
|
var outputVelocityFlags = ReadWriteHelper.ReadUint8LittleEndian(span, 204);
|
|
var isOutputVelocity0Valid = (outputVelocityFlags & 0x01) != 0;
|
|
var isOutputVelocity1Valid = (outputVelocityFlags & 0x02) != 0;
|
|
// Bits 2,3 reserved
|
|
var isOutputVelocity0TransmittedSafely = (outputVelocityFlags & 0x10) != 0;
|
|
var isOutputVelocity1TransmittedSafely = (outputVelocityFlags & 0x20) != 0;
|
|
// Bits 6,7 reserved
|
|
|
|
// Parse resulting velocities (offsets 208-247)
|
|
var resultingVelocities = new List<short>(20);
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
resultingVelocities.Add(ReadWriteHelper.ReadInt16LittleEndian(span, 208 + i * 2));
|
|
}
|
|
|
|
var resultingVelocityFlags = ParseBitVector20(span, 248);
|
|
|
|
// Parse output flags (offset 259)
|
|
var outputFlags = ReadWriteHelper.ReadUint8LittleEndian(span, 259);
|
|
var flagsSleepModeOutputIsValid = (outputFlags & 0x01) != 0;
|
|
var flagsHostErrorFlagsAreValid = (outputFlags & 0x02) != 0;
|
|
|
|
return new ApplicationOutputs
|
|
{
|
|
EvalOut = evalOut,
|
|
EvalOutIsSafe = evalOutIsSafe,
|
|
EvalOutIsValid = evalOutIsValid,
|
|
MonitoringCases = outputMonitoringCases,
|
|
MonitoringCaseFlags = outputMonitoringCaseFlags,
|
|
SleepModeOutput = sleepModeOutput,
|
|
HostErrorFlagContaminationWarning = hostErrorFlagContaminationWarning,
|
|
HostErrorFlagContaminationError = hostErrorFlagContaminationError,
|
|
HostErrorFlagManipulationError = hostErrorFlagManipulationError,
|
|
HostErrorFlagGlare = hostErrorFlagGlare,
|
|
HostErrorFlagReferenceContourIntruded = hostErrorFlagReferenceContourIntruded,
|
|
HostErrorFlagCriticalError = hostErrorFlagCriticalError,
|
|
Velocity0 = outputVelocity0,
|
|
Velocity1 = outputVelocity1,
|
|
IsVelocity0Valid = isOutputVelocity0Valid,
|
|
IsVelocity1Valid = isOutputVelocity1Valid,
|
|
IsVelocity0TransmittedSafely = isOutputVelocity0TransmittedSafely,
|
|
IsVelocity1TransmittedSafely = isOutputVelocity1TransmittedSafely,
|
|
ResultingVelocity = resultingVelocities,
|
|
ResultingVelocityIsValid = resultingVelocityFlags,
|
|
FlagsSleepModeOutputIsValid = flagsSleepModeOutputIsValid,
|
|
FlagsHostErrorFlagsAreValid = flagsHostErrorFlagsAreValid
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parses a 32-bit bit vector (32 boolean flags) from a uint32 value
|
|
/// </summary>
|
|
private static IReadOnlyList<bool> ParseBitVector32(ReadOnlySpan<byte> span, int offset)
|
|
{
|
|
var value = ReadWriteHelper.ReadUint32LittleEndian(span, offset);
|
|
var flags = new List<bool>(32);
|
|
|
|
for (int i = 0; i < 32; i++)
|
|
{
|
|
flags.Add((value & (0x01U << i)) != 0);
|
|
}
|
|
|
|
return flags;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parses a 20-bit bit vector (20 boolean flags) from a uint32 value
|
|
/// </summary>
|
|
private static IReadOnlyList<bool> ParseBitVector20(ReadOnlySpan<byte> span, int offset)
|
|
{
|
|
var value = ReadWriteHelper.ReadUint32LittleEndian(span, offset);
|
|
var flags = new List<bool>(20);
|
|
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
flags.Add((value & (0x01U << i)) != 0);
|
|
}
|
|
|
|
return flags;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Checks if application data block is published (enabled)
|
|
/// </summary>
|
|
private static bool CheckIfApplicationDataIsPublished(DataHeader header)
|
|
{
|
|
return !(header.ApplicationDataBlockOffset == 0 && header.ApplicationDataBlockSize == 0);
|
|
}
|
|
}
|
|
|