Initial commit

srcs/RobotNet10/RobotApp/Communication/Olei.LidarSensor/LidarPacketParser.cs

@@ -0,0 +1,143 @@
+using System.Runtime.InteropServices;
+
+namespace Olei.LidarSensor;
+
+/// <summary>
+/// High-performance parser for LiDAR data packets
+/// Uses Span&lt;byte&gt; 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;
+    }
+}