Initial commit

2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/Communication/Olei.LidarSensor/LidarDataPacket.cs
+++ b/srcs/RobotNet10/RobotApp/Communication/Olei.LidarSensor/LidarDataPacket.cs
@@ -0,0 +1,130 @@
+namespace Olei.LidarSensor;
+
+/// <summary>
+/// Complete LiDAR data packet (1240 bytes total)
+/// Contains header (40 bytes) + 150 data blocks (1200 bytes)
+/// </summary>
+public sealed class LidarDataPacket
+{
+    /// <summary>
+    /// Total packet size in bytes
+    /// </summary>
+    public const int PACKET_SIZE = 1240;
+
+    /// <summary>
+    /// Header size in bytes
+    /// </summary>
+    public const int HEADER_SIZE = 40;
+
+    /// <summary>
+    /// Number of data blocks per packet
+    /// </summary>
+    public const int DATA_BLOCK_COUNT = 150;
+
+    /// <summary>
+    /// Size of each data block in bytes
+    /// </summary>
+    public const int DATA_BLOCK_SIZE = 8;
+
+    /// <summary>
+    /// Total size of all data blocks
+    /// </summary>
+    public const int DATA_BLOCKS_TOTAL_SIZE = DATA_BLOCK_COUNT * DATA_BLOCK_SIZE;
+
+    /// <summary>
+    /// Packet header information
+    /// </summary>
+    public LidarHeader Header { get; set; }
+
+    /// <summary>
+    /// Array of data blocks (150 blocks)
+    /// Using array instead of list for better performance
+    /// </summary>
+    public LidarDataBlock[] DataBlocks { get; }
+
+    /// <summary>
+    /// Timestamp when this packet was received
+    /// </summary>
+    public DateTime ReceivedTime { get; set; }
+
+    public LidarDataPacket()
+    {
+        DataBlocks = new LidarDataBlock[DATA_BLOCK_COUNT];
+        ReceivedTime = DateTime.UtcNow;
+    }
+
+    /// <summary>
+    /// Get all valid data points (where angle is valid)
+    /// </summary>
+    public IEnumerable<LidarDataBlock> GetValidDataBlocks()
+    {
+        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
+        {
+            if (DataBlocks[i].IsValid)
+            {
+                yield return DataBlocks[i];
+            }
+        }
+    }
+
+    /// <summary>
+    /// Get count of valid data blocks
+    /// </summary>
+    public int GetValidDataBlockCount()
+    {
+        int count = 0;
+        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
+        {
+            if (DataBlocks[i].IsValid)
+            {
+                count++;
+            }
+        }
+        return count;
+    }
+
+    /// <summary>
+    /// Get all valid points as (angle, distance) tuples
+    /// </summary>
+    public IEnumerable<(double angle, double distance)> GetValidPoints()
+    {
+        byte distanceScale = Header.DistanceScale;
+        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
+        {
+            if (DataBlocks[i].IsValid)
+            {
+                yield return (
+                    DataBlocks[i].GetAngleDegrees(),
+                    DataBlocks[i].GetDistance(distanceScale)
+                );
+            }
+        }
+    }
+
+    /// <summary>
+    /// Get all valid points in Cartesian coordinates (X, Y)
+    /// </summary>
+    public IEnumerable<(double x, double y)> GetValidCartesianPoints()
+    {
+        byte distanceScale = Header.DistanceScale;
+        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
+        {
+            if (DataBlocks[i].IsValid)
+            {
+                yield return (
+                    DataBlocks[i].GetX(distanceScale),
+                    DataBlocks[i].GetY(distanceScale)
+                );
+            }
+        }
+    }
+
+    /// <summary>
+    /// Get summary information about this packet
+    /// </summary>
+    public override string ToString()
+    {
+        return $"LidarPacket [Valid: {Header.IsValidFrame}, ValidBlocks: {GetValidDataBlockCount()}/{DATA_BLOCK_COUNT}, " +
+               $"RotationRate: {Header.RotationRate}, HasError: {Header.HasError}, Time: {ReceivedTime:HH:mm:ss.fff}]";
+    }
+}