Initial commit

2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataProcessing/ParseTcpPacket.cs
+++ b/srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataProcessing/ParseTcpPacket.cs
@@ -0,0 +1,95 @@
+using Sick.SafetyScanners.DataStructures;
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.DataProcessing;
+
+/// <summary>
+/// Parser for incoming TCP packets in COLA2 format
+/// Thread-safe implementation
+/// </summary>
+public sealed class ParseTcpPacket
+{
+    private const int HeaderSize = 18; // COLA2 header size
+    
+    /// <summary>
+    /// Gets the expected packet length from the header
+    /// Matches: ParseTCPPacket::getExpectedPacketLength in C++
+    /// </summary>
+    public uint GetExpectedPacketLength(PacketBuffer buffer)
+    {
+        var bufferData = buffer.GetBuffer();
+        var length = ReadWriteHelper.ReadUint32BigEndian(bufferData.Span, 4);
+        return length + 8; // for STX and Length which is not included in length datafield
+    }
+    
+    /// <summary>
+    /// Gets the request ID from the packet header
+    /// Matches: ParseTCPPacket::getRequestID in C++
+    /// </summary>
+    public ushort GetRequestId(PacketBuffer buffer)
+    {
+        var bufferData = buffer.GetBuffer();
+        return ReadWriteHelper.ReadUint16BigEndian(bufferData.Span, 14);
+    }
+    
+    /// <summary>
+    /// Parses the TCP sequence to extract COLA2 header information
+    /// Matches: ParseTCPPacket::parseTCPSequence in C++
+    /// </summary>
+    public ParseResult ParseTcpSequence(PacketBuffer buffer)
+    {
+        var bufferData = buffer.GetBuffer();
+        var span = bufferData.Span;
+        
+        // Read header fields
+        var stx = ReadWriteHelper.ReadUint32BigEndian(span, 0);
+        var length = ReadWriteHelper.ReadUint32BigEndian(span, 4);
+        var hubCntr = ReadWriteHelper.ReadUint8BigEndian(span, 8);
+        var noc = ReadWriteHelper.ReadUint8BigEndian(span, 9);
+        var sessionId = ReadWriteHelper.ReadUint32BigEndian(span, 10);
+        var requestId = ReadWriteHelper.ReadUint16BigEndian(span, 14);
+        var commandType = ReadWriteHelper.ReadUint8BigEndian(span, 16);
+        var commandMode = ReadWriteHelper.ReadUint8BigEndian(span, 17);
+        
+        // Read data payload (everything after header, starting at offset 20)
+        // Matches: ParseTCPPacket::readData in C++ - returns data from offset 20
+        ReadOnlyMemory<byte> data = ReadOnlyMemory<byte>.Empty;
+        if (bufferData.Length >= 20)
+        {
+            var dataStart = 20;
+            data = bufferData[dataStart..];
+        }
+        
+        return new ParseResult
+        {
+            Stx = stx,
+            Length = length,
+            HubCntr = hubCntr,
+            NoC = noc,
+            SessionId = sessionId,
+            RequestId = requestId,
+            CommandType = commandType,
+            CommandMode = commandMode,
+            ErrorCode = null,
+            Data = data
+        };
+    }
+    
+    /// <summary>
+    /// Result of parsing a TCP packet
+    /// </summary>
+    public sealed class ParseResult
+    {
+        public uint Stx { get; init; }
+        public uint Length { get; init; }
+        public byte HubCntr { get; init; }
+        public byte NoC { get; init; }
+        public uint SessionId { get; init; }
+        public ushort RequestId { get; init; }
+        public byte CommandType { get; init; }
+        public byte CommandMode { get; init; }
+        public ushort? ErrorCode { get; init; }
+        public ReadOnlyMemory<byte> Data { get; init; }
+    }
+}
+