52 lines
1.6 KiB
C#
52 lines
1.6 KiB
C#
using Sick.SafetyScanners.DataStructures;
|
|
using Sick.SafetyScanners.Helpers;
|
|
|
|
namespace Sick.SafetyScanners.DataProcessing;
|
|
|
|
/// <summary>
|
|
/// Parser for FieldGeometryData response from COLA2 variable command
|
|
/// Thread-safe implementation
|
|
/// </summary>
|
|
public sealed class ParseFieldGeometryData
|
|
{
|
|
private const double StartAngleDegrees = -47.5; // Defined start angle in degrees in SICK coordinates
|
|
|
|
/// <summary>
|
|
/// Parses the field geometry data from a TCP sequence (COLA2 response)
|
|
/// Matches: ParseFieldGeometryData::parseTCPSequence in C++
|
|
/// </summary>
|
|
public FieldData ParseTcpSequence(PacketBuffer buffer)
|
|
{
|
|
var span = buffer.GetBufferSpan();
|
|
uint arrayLength = ReadArrayLength(span);
|
|
|
|
var geometryDistance = new List<ushort>((int)arrayLength);
|
|
for (uint i = 0; i < arrayLength; i++)
|
|
{
|
|
geometryDistance.Add(ReadArrayElement(span, i));
|
|
}
|
|
|
|
// Values are persistent for scanners
|
|
double res = (275.0 / arrayLength);
|
|
|
|
return new FieldData
|
|
{
|
|
IsValid = true,
|
|
BeamDistances = geometryDistance,
|
|
StartAngle = (StartAngleDegrees * Math.PI / 180.0),
|
|
AngularBeamResolution = (res * Math.PI / 180.0)
|
|
};
|
|
}
|
|
|
|
private uint ReadArrayLength(ReadOnlySpan<byte> span)
|
|
{
|
|
return ReadWriteHelper.ReadUint32LittleEndian(span, 4);
|
|
}
|
|
|
|
private ushort ReadArrayElement(ReadOnlySpan<byte> span, uint elemNumber)
|
|
{
|
|
return ReadWriteHelper.ReadUint16LittleEndian(span, 8 + (int)(elemNumber * 2));
|
|
}
|
|
}
|
|
|