144 lines
4.5 KiB
C#
144 lines
4.5 KiB
C#
using System.Runtime.InteropServices;
|
||
|
||
namespace Olei.LidarSensor;
|
||
|
||
/// <summary>
|
||
/// High-performance parser for LiDAR data packets
|
||
/// Uses Span<byte> and MemoryMarshal for zero-allocation parsing
|
||
/// </summary>
|
||
public static class LidarPacketParser
|
||
{
|
||
/// <summary>
|
||
/// Parse raw UDP packet data into LidarDataPacket
|
||
/// </summary>
|
||
/// <param name="buffer">Raw byte buffer from UDP socket</param>
|
||
/// <param name="length">Actual length of received data</param>
|
||
/// <param name="packet">Output packet to populate (reusable)</param>
|
||
/// <returns>True if parsing successful, false if invalid data</returns>
|
||
public static bool TryParse(byte[] buffer, int length, LidarDataPacket packet)
|
||
{
|
||
if (length < LidarDataPacket.PACKET_SIZE)
|
||
return false;
|
||
|
||
return TryParse(buffer.AsSpan(0, length), packet);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Parse raw UDP packet data into LidarDataPacket using Span
|
||
/// High performance, zero-allocation parsing
|
||
/// </summary>
|
||
/// <param name="data">Raw byte span</param>
|
||
/// <param name="packet">Output packet to populate (reusable)</param>
|
||
/// <returns>True if parsing successful, false if invalid data</returns>
|
||
public static bool TryParse(ReadOnlySpan<byte> data, LidarDataPacket packet)
|
||
{
|
||
if (data.Length < LidarDataPacket.PACKET_SIZE)
|
||
return false;
|
||
|
||
// Parse header (40 bytes)
|
||
if (!TryParseHeader(data.Slice(0, LidarDataPacket.HEADER_SIZE), out var header))
|
||
return false;
|
||
|
||
packet.Header = header;
|
||
packet.ReceivedTime = DateTime.UtcNow;
|
||
|
||
// Parse data blocks (150 blocks × 8 bytes)
|
||
ReadOnlySpan<byte> dataBlocksSpan = data.Slice(
|
||
LidarDataPacket.HEADER_SIZE,
|
||
LidarDataPacket.DATA_BLOCKS_TOTAL_SIZE
|
||
);
|
||
|
||
for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
|
||
{
|
||
int offset = i * LidarDataPacket.DATA_BLOCK_SIZE;
|
||
ReadOnlySpan<byte> blockSpan = dataBlocksSpan.Slice(offset, LidarDataPacket.DATA_BLOCK_SIZE);
|
||
|
||
packet.DataBlocks[i] = ParseDataBlock(blockSpan);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Parse header from byte span
|
||
/// </summary>
|
||
private static bool TryParseHeader(ReadOnlySpan<byte> data, out LidarHeader header)
|
||
{
|
||
if (data.Length < LidarDataPacket.HEADER_SIZE)
|
||
{
|
||
header = default;
|
||
return false;
|
||
}
|
||
|
||
// Use MemoryMarshal for fast struct parsing
|
||
header = MemoryMarshal.Read<LidarHeader>(data);
|
||
|
||
// Validate frame ID
|
||
return header.IsValidFrame;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Parse single data block from byte span
|
||
/// </summary>
|
||
private static LidarDataBlock ParseDataBlock(ReadOnlySpan<byte> data)
|
||
{
|
||
// Use MemoryMarshal for fast struct parsing
|
||
return MemoryMarshal.Read<LidarDataBlock>(data);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Validate packet without full parsing
|
||
/// Useful for quick validation before processing
|
||
/// </summary>
|
||
public static bool IsValidPacket(ReadOnlySpan<byte> data)
|
||
{
|
||
if (data.Length < LidarDataPacket.PACKET_SIZE)
|
||
return false;
|
||
|
||
// Check frame ID (first 4 bytes)
|
||
uint frameId = MemoryMarshal.Read<uint>(data);
|
||
return frameId == LidarHeader.FRAME_ID;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Extract distance scale from packet without full parsing
|
||
/// </summary>
|
||
public static byte GetDistanceScale(ReadOnlySpan<byte> data)
|
||
{
|
||
if (data.Length < 7)
|
||
return 0;
|
||
|
||
return data[6]; // Distance scale is at offset 6
|
||
}
|
||
|
||
/// <summary>
|
||
/// Extract timestamp from packet without full parsing
|
||
/// </summary>
|
||
public static uint GetTimestamp(ReadOnlySpan<byte> data)
|
||
{
|
||
if (data.Length < 32)
|
||
return 0;
|
||
|
||
return MemoryMarshal.Read<uint>(data.Slice(28, 4));
|
||
}
|
||
|
||
/// <summary>
|
||
/// Extract error status from packet without full parsing
|
||
/// </summary>
|
||
public static byte GetErrorStatus(ReadOnlySpan<byte> data)
|
||
{
|
||
if (data.Length < 36)
|
||
return 0;
|
||
|
||
return data[35]; // Error status is at offset 35
|
||
}
|
||
|
||
/// <summary>
|
||
/// Quick check if packet has any errors
|
||
/// </summary>
|
||
public static bool HasErrors(ReadOnlySpan<byte> data)
|
||
{
|
||
return GetErrorStatus(data) != 0;
|
||
}
|
||
}
|