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/ParseDerivedValues.cs
+++ b/srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataProcessing/ParseDerivedValues.cs
@@ -0,0 +1,114 @@
+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);
+    }
+}
+