417 lines
15 KiB
C#
417 lines
15 KiB
C#
using System.Text;
|
|
|
|
namespace Sick.ColaB.Parsers;
|
|
|
|
/// <summary>
|
|
/// Parser for SICK scanner LMDscandata telegram
|
|
/// Supports both binary (ColaB) and ASCII (ColaA) formats
|
|
/// </summary>
|
|
public static class LmdScandataParser
|
|
{
|
|
// Constants for validation
|
|
private const int MinReasonableDistanceCount = 50;
|
|
private const int MaxReasonableDistanceCount = 5000;
|
|
|
|
/// <summary>
|
|
/// Parse LMDscandata from binary ColaB format
|
|
/// This is the primary parser for binary protocol data
|
|
/// </summary>
|
|
public static ScanDataResult? ParseBinary(ReadOnlySpan<byte> commandData)
|
|
{
|
|
try
|
|
{
|
|
// Binary LMDscandata format:
|
|
// Starts with ASCII header: "sRA LMDscandata " or "sSN LMDscandata "
|
|
// Then binary data follows:
|
|
// - Various fields (version, device number, etc.)
|
|
// - "DIST1" (5 bytes ASCII) followed by:
|
|
// - Scale factor (4 bytes double)
|
|
// - Scale offset (4 bytes double)
|
|
// - Starting angle (4 bytes signed int, units: 1/10000 degree)
|
|
// - Angular step (2 bytes unsigned short, units: 1/10000 degree)
|
|
// - Number of items (2 bytes, big endian uint16)
|
|
// - Distance values (each 2 bytes, big endian uint16, units: mm)
|
|
// - "RSSI1" (5 bytes ASCII) followed by:
|
|
// - Number of items (2 bytes, big endian uint16)
|
|
// - RSSI values (each 1 or 2 bytes depending on resolution)
|
|
|
|
if (commandData.Length < 50)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Check if it contains "LMDscandata"
|
|
var commandStart = Encoding.ASCII.GetString(commandData[..Math.Min(50, commandData.Length)]);
|
|
if (!commandStart.Contains("LMDscandata", StringComparison.Ordinal))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
double startAngleDeg = 0.0;
|
|
double angularStepDeg = 0.0;
|
|
double scaleFactor = 1.0;
|
|
double scaleOffset = 0.0;
|
|
|
|
// Find "DIST1" in the data
|
|
var dist1Pattern = Encoding.ASCII.GetBytes("DIST1");
|
|
var dist1Index = FindPattern(commandData, dist1Pattern);
|
|
|
|
if (dist1Index == -1)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Parse header fields at fixed offsets after DIST1
|
|
int headerStart = dist1Index + 5; // Right after "DIST1"
|
|
|
|
// Scale factor (4 bytes double, big endian) at offset headerStart
|
|
if (headerStart + 4 <= commandData.Length)
|
|
{
|
|
uint scaleFactorInt = ReadUInt32BigEndian(commandData, headerStart);
|
|
scaleFactor = BitConverter.ToSingle(BitConverter.GetBytes(scaleFactorInt), 0);
|
|
}
|
|
|
|
// Scale offset (4 bytes double, big endian) at offset headerStart + 4
|
|
if (headerStart + 8 <= commandData.Length)
|
|
{
|
|
uint scaleOffsetInt = ReadUInt32BigEndian(commandData, headerStart + 4);
|
|
scaleOffset = BitConverter.ToSingle(BitConverter.GetBytes(scaleOffsetInt), 0);
|
|
}
|
|
|
|
// Starting angle (4 bytes signed int, big endian) at offset headerStart + 8
|
|
if (headerStart + 12 <= commandData.Length)
|
|
{
|
|
int startAngleInt = (int)ReadUInt32BigEndian(commandData, headerStart + 8);
|
|
startAngleDeg = startAngleInt / 10000.0;
|
|
}
|
|
|
|
// Angular step width (2 bytes unsigned short, big endian) at offset headerStart + 12
|
|
if (headerStart + 14 <= commandData.Length)
|
|
{
|
|
ushort angularStepInt = ReadUInt16BigEndian(commandData, headerStart + 12);
|
|
angularStepDeg = angularStepInt / 10000.0;
|
|
}
|
|
|
|
// Find distance count - try multiple offsets
|
|
if (!TryFindDistanceCount(commandData, headerStart, out ushort distCount, out int distCountOffset))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Read distance values (each 2 bytes, big endian, units: mm)
|
|
var distValuesOffset = distCountOffset + 2;
|
|
if (distValuesOffset + (distCount * 2) > commandData.Length)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var ranges = new List<double>();
|
|
|
|
for (int i = 0; i < distCount; i++)
|
|
{
|
|
var offset = distValuesOffset + (i * 2);
|
|
if (offset + 2 > commandData.Length)
|
|
{
|
|
break;
|
|
}
|
|
|
|
// Read uint16 big endian
|
|
ushort distValue = ReadUInt16BigEndian(commandData, offset);
|
|
// Apply scale factor and convert from mm to meters
|
|
double distanceM = (distValue * scaleFactor + scaleOffset) / 1000.0;
|
|
ranges.Add(distanceM);
|
|
}
|
|
|
|
// Find "RSSI1" for intensities (optional)
|
|
var rssi1Pattern = Encoding.ASCII.GetBytes("RSSI1");
|
|
var rssi1Index = FindPattern(commandData, rssi1Pattern, rssiSearchStart: distValuesOffset + (distCount * 2));
|
|
var intensities = new List<double>();
|
|
|
|
if (rssi1Index != -1)
|
|
{
|
|
// Read RSSI count
|
|
var rssiCountOffset = rssi1Index + 5;
|
|
if (rssiCountOffset + 2 <= commandData.Length)
|
|
{
|
|
ushort rssiCount = ReadUInt16BigEndian(commandData, rssiCountOffset);
|
|
|
|
// Read RSSI values (usually 1 byte each, but can be 2 bytes for 16-bit resolution)
|
|
var rssiValuesOffset = rssiCountOffset + 2;
|
|
|
|
// Try to determine if RSSI is 8-bit or 16-bit
|
|
bool use16BitRssi = (rssiValuesOffset + (rssiCount * 2) <= commandData.Length) &&
|
|
(rssiCount == ranges.Count);
|
|
|
|
if (use16BitRssi)
|
|
{
|
|
// 16-bit RSSI values (big endian)
|
|
for (int i = 0; i < rssiCount && i < ranges.Count; i++)
|
|
{
|
|
ushort rssiValue = ReadUInt16BigEndian(commandData, rssiValuesOffset + (i * 2));
|
|
// Normalize to 0-1 range (assuming max value is 65535)
|
|
intensities.Add(rssiValue);
|
|
}
|
|
}
|
|
else if (rssiValuesOffset + rssiCount <= commandData.Length)
|
|
{
|
|
// 8-bit RSSI values
|
|
for (int i = 0; i < rssiCount && i < ranges.Count; i++)
|
|
{
|
|
// Normalize to 0-1 range (assuming max value is 255)
|
|
intensities.Add(commandData[rssiValuesOffset + i]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (int i = 0; i < ranges.Count; i++)
|
|
{
|
|
intensities.Add(1.0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (ranges.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
return new ScanDataResult
|
|
{
|
|
Ranges = [.. ranges],
|
|
Intensities = intensities.Count > 0 ? [.. intensities] : null,
|
|
StartAngleDeg = startAngleDeg,
|
|
AngularStepDeg = angularStepDeg,
|
|
ScaleFactor = scaleFactor,
|
|
ScaleOffset = scaleOffset,
|
|
Timestamp = DateTime.UtcNow
|
|
};
|
|
}
|
|
catch
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parse LMDscandata from ASCII format (via ColaB decoding)
|
|
/// This is a fallback parser when binary parsing fails
|
|
/// </summary>
|
|
public static ScanDataResult? ParseAscii(ReadOnlySpan<byte> commandData)
|
|
{
|
|
try
|
|
{
|
|
// Decode command string to check if it's scan data
|
|
var commandString = ColaBHelper.DecodeCommand(commandData);
|
|
|
|
// Check if this is scan data telegram
|
|
if (!commandString.Contains("LMDscandata", StringComparison.Ordinal))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
return ParseAsciiString(commandString);
|
|
}
|
|
catch
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parse LMDscandata from ASCII string format
|
|
/// Format: "sSN LMDscandata <version> <device> ... DIST1 <count> <distances> ... RSSI1 <count> <intensities> ..."
|
|
/// </summary>
|
|
public static ScanDataResult? ParseAsciiString(string telegram)
|
|
{
|
|
try
|
|
{
|
|
// Split telegram into fields (space-separated)
|
|
var fields = telegram.Split([' '], StringSplitOptions.RemoveEmptyEntries);
|
|
|
|
if (fields.Length < 20)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var ranges = new List<double>();
|
|
var intensities = new List<double>();
|
|
|
|
int distIndex = -1;
|
|
int rssiIndex = -1;
|
|
|
|
// Find DIST1 field
|
|
for (int i = 0; i < fields.Length; i++)
|
|
{
|
|
if (fields[i].StartsWith("DIST", StringComparison.Ordinal))
|
|
{
|
|
distIndex = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Find RSSI1 field
|
|
for (int i = 0; i < fields.Length; i++)
|
|
{
|
|
if (fields[i].StartsWith("RSSI", StringComparison.Ordinal))
|
|
{
|
|
rssiIndex = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Parse distances
|
|
if (distIndex >= 0 && distIndex + 1 < fields.Length)
|
|
{
|
|
// Next field after DIST1 is the count (in hex)
|
|
if (int.TryParse(fields[distIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int distCount))
|
|
{
|
|
// Parse distance values (in hex, units are mm, convert to meters)
|
|
for (int i = 0; i < distCount && distIndex + 2 + i < fields.Length; i++)
|
|
{
|
|
if (int.TryParse(fields[distIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int distValue))
|
|
{
|
|
// Convert from mm to meters
|
|
ranges.Add(distValue / 1000.0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Parse intensities (RSSI)
|
|
if (rssiIndex >= 0 && rssiIndex + 1 < fields.Length)
|
|
{
|
|
// Next field after RSSI1 is the count (in hex)
|
|
if (int.TryParse(fields[rssiIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int rssiCount))
|
|
{
|
|
// Parse RSSI values (in hex)
|
|
for (int i = 0; i < rssiCount && rssiIndex + 2 + i < fields.Length; i++)
|
|
{
|
|
if (int.TryParse(fields[rssiIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int rssiValue))
|
|
{
|
|
intensities.Add(rssiValue);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (ranges.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
return new ScanDataResult
|
|
{
|
|
Ranges = [.. ranges],
|
|
Intensities = intensities.Count > 0 ? [.. intensities] : null,
|
|
StartAngleDeg = 0.0, // ASCII format doesn't include angle info
|
|
AngularStepDeg = 0.0,
|
|
ScaleFactor = 1.0,
|
|
ScaleOffset = 0.0,
|
|
Timestamp = DateTime.UtcNow
|
|
};
|
|
}
|
|
catch
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
#region Helper Methods
|
|
|
|
private static int FindPattern(ReadOnlySpan<byte> data, byte[] pattern, int rssiSearchStart = 0)
|
|
{
|
|
int searchStart = rssiSearchStart;
|
|
for (int i = searchStart; i <= data.Length - pattern.Length; i++)
|
|
{
|
|
bool found = true;
|
|
for (int j = 0; j < pattern.Length; j++)
|
|
{
|
|
if (data[i + j] != pattern[j])
|
|
{
|
|
found = false;
|
|
break;
|
|
}
|
|
}
|
|
if (found)
|
|
{
|
|
return i;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
private static uint ReadUInt32BigEndian(ReadOnlySpan<byte> data, int offset)
|
|
{
|
|
if (offset + 4 > data.Length)
|
|
return 0;
|
|
|
|
return (uint)((data[offset] << 24) | (data[offset + 1] << 16) |
|
|
(data[offset + 2] << 8) | data[offset + 3]);
|
|
}
|
|
|
|
private static ushort ReadUInt16BigEndian(ReadOnlySpan<byte> data, int offset)
|
|
{
|
|
if (offset + 2 > data.Length)
|
|
return 0;
|
|
|
|
return (ushort)((data[offset] << 8) | data[offset + 1]);
|
|
}
|
|
|
|
private static bool TryFindDistanceCount(ReadOnlySpan<byte> commandData, int headerStart,
|
|
out ushort distCount, out int distCountOffset)
|
|
{
|
|
distCount = 0;
|
|
distCountOffset = -1;
|
|
|
|
// Try offset +14 first (matches actual data)
|
|
int tryOffset1 = headerStart + 14;
|
|
if (TryReadDistanceCount(commandData, tryOffset1, out distCount, out distCountOffset))
|
|
return true;
|
|
|
|
// Try offset +19 (per C++ reference code)
|
|
int tryOffset2 = headerStart + 19;
|
|
if (TryReadDistanceCount(commandData, tryOffset2, out distCount, out distCountOffset))
|
|
return true;
|
|
|
|
// Search for valid count
|
|
int searchStart = headerStart;
|
|
int searchEnd = Math.Min(headerStart + 30, commandData.Length - 2);
|
|
|
|
for (int offset = searchStart; offset <= searchEnd; offset++)
|
|
{
|
|
if (TryReadDistanceCount(commandData, offset, out distCount, out distCountOffset))
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
private static bool TryReadDistanceCount(ReadOnlySpan<byte> commandData, int offset,
|
|
out ushort count, out int countOffset)
|
|
{
|
|
count = 0;
|
|
countOffset = -1;
|
|
|
|
if (offset + 2 > commandData.Length)
|
|
return false;
|
|
|
|
ushort testCount = ReadUInt16BigEndian(commandData, offset);
|
|
|
|
if (testCount >= MinReasonableDistanceCount && testCount <= MaxReasonableDistanceCount)
|
|
{
|
|
int requiredBytes = offset + 2 + (testCount * 2);
|
|
if (requiredBytes <= commandData.Length)
|
|
{
|
|
count = testCount;
|
|
countOffset = offset;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
#endregion
|
|
}
|