115 lines
5.3 KiB
C#
115 lines
5.3 KiB
C#
using Sick.SafetyScanners.DataStructures;
|
|
using Sick.SafetyScanners.Helpers;
|
|
|
|
namespace Sick.SafetyScanners.DataProcessing;
|
|
|
|
/// <summary>
|
|
/// Parser for derived values block from UDP packets
|
|
/// Contains configuration of data output: multiplication factor, number of beams, scan time, angles, resolution
|
|
/// Thread-safe implementation
|
|
/// </summary>
|
|
public sealed class ParseDerivedValues
|
|
{
|
|
/// <summary>
|
|
/// Parses the derived values 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 DerivedValues ParseUdpSequence(PacketBuffer buffer, DataHeader header)
|
|
{
|
|
if (buffer == null)
|
|
throw new ArgumentNullException(nameof(buffer));
|
|
|
|
if (header == null)
|
|
throw new ArgumentNullException(nameof(header));
|
|
|
|
// Check if derived values block is enabled
|
|
if (!CheckIfDerivedValuesIsPublished(header))
|
|
{
|
|
return new DerivedValues
|
|
{
|
|
IsEmpty = true
|
|
};
|
|
}
|
|
|
|
// Check if header is valid
|
|
if (header.IsEmpty)
|
|
{
|
|
return new DerivedValues
|
|
{
|
|
IsEmpty = true
|
|
};
|
|
}
|
|
|
|
var bufferData = buffer.GetBuffer();
|
|
var offset = header.DerivedValuesBlockOffset;
|
|
|
|
// Validate buffer size
|
|
if (offset + 20 > bufferData.Length) // Derived values block is at least 20 bytes
|
|
{
|
|
throw new ArgumentException(
|
|
$"Buffer too small to contain derived values block (offset: {offset}, buffer size: {bufferData.Length})",
|
|
nameof(buffer));
|
|
}
|
|
|
|
var span = bufferData.Span.Slice(offset);
|
|
|
|
// Parse all fields from the derived values block
|
|
// Format (all little endian):
|
|
// Offset 0: Multiplication factor (uint16)
|
|
// Offset 2: Number of beams (uint16)
|
|
// Offset 4: Scan time (uint16, milliseconds)
|
|
// Offset 6: Reserved (2 bytes)
|
|
// Offset 8: Start angle (int32, sensor units)
|
|
// Offset 12: Angular beam resolution (int32, sensor units)
|
|
// Offset 16: Interbeam period (uint32, microseconds)
|
|
|
|
var multiplicationFactor = ReadWriteHelper.ReadUint16LittleEndian(span, 0);
|
|
var numberOfBeams = ReadWriteHelper.ReadUint16LittleEndian(span, 2);
|
|
var scanTime = ReadWriteHelper.ReadUint16LittleEndian(span, 4);
|
|
var startAngleSensorUnits = ReadWriteHelper.ReadInt32LittleEndian(span, 8);
|
|
var angularBeamResolutionSensorUnits = ReadWriteHelper.ReadInt32LittleEndian(span, 12);
|
|
var interbeamPeriod = ReadWriteHelper.ReadUint32LittleEndian(span, 16);
|
|
|
|
// Convert angles from sensor units to radians
|
|
// According to COLA2 documentation: "This value, divided by 4194304, equals the actual start angle"
|
|
// According to C++ reference: value / 4194304.0 gives degrees (see setDerivedAngularBeamResolutionDegrees)
|
|
// The C++ reference's setDerivedAngularBeamResolution divides by 4194304.0, and setDerivedAngularBeamResolutionDegrees
|
|
// sets degrees directly, indicating that dividing by 4194304.0 gives degrees.
|
|
// However, looking at ParseMeasurementData which uses these values to calculate scan point angles,
|
|
// and the fact that 4194304.0 units represent a full circle (360° = 2π radians),
|
|
// it appears the C++ reference stores values as "full circle units" (1.0 = 360° = 2π radians).
|
|
// So: sensorUnits / 4194304.0 = full circle units, then * 2π = radians
|
|
// OR: sensorUnits / 4194304.0 = degrees / 360.0, so * 360 = degrees, then * π/180 = radians
|
|
// Since user reports resolution shows 60° which is too large, let's check the actual conversion.
|
|
// Based on user feedback that resolution shows 60° (not typical ~0.1°), the current formula
|
|
// (multiplying by 2π) might be converting full circle units incorrectly.
|
|
// Let's try: sensorUnits / 4194304.0 already gives degrees (as per C++ setDerivedAngularBeamResolutionDegrees pattern)
|
|
var startAngleDeg = (startAngleSensorUnits / DerivedValues.AngleResolution);
|
|
var angularBeamResolutionDeg = (angularBeamResolutionSensorUnits / DerivedValues.AngleResolution);
|
|
// Convert degrees to radians
|
|
var startAngleRad = (startAngleDeg * Math.PI / 180.0);
|
|
var angularBeamResolutionRad = (angularBeamResolutionDeg * Math.PI / 180.0);
|
|
|
|
return new DerivedValues
|
|
{
|
|
MultiplicationFactor = multiplicationFactor,
|
|
NumberOfBeams = numberOfBeams,
|
|
ScanTime = scanTime,
|
|
StartAngle = startAngleRad,
|
|
AngularBeamResolution = angularBeamResolutionRad,
|
|
InterbeamPeriod = interbeamPeriod,
|
|
IsEmpty = false
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Checks if derived values block is published (enabled)
|
|
/// </summary>
|
|
private static bool CheckIfDerivedValuesIsPublished(DataHeader header)
|
|
{
|
|
return !(header.DerivedValuesBlockOffset == 0 && header.DerivedValuesBlockSize == 0);
|
|
}
|
|
}
|
|
|