Initial commit
This commit is contained in:
@@ -0,0 +1,247 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user