Initial commit

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

@@ -0,0 +1,93 @@
+using System.Runtime.InteropServices;
+
+namespace Olei.LidarSensor;
+
+/// <summary>
+/// Data block structure for each measurement point (8 bytes)
+/// All data is in little-endian format
+/// </summary>
+[StructLayout(LayoutKind.Sequential, Pack = 1)]
+public struct LidarDataBlock
+{
+    /// <summary>
+    /// Invalid angle marker
+    /// </summary>
+    public const ushort INVALID_ANGLE = 0xFF00;
+
+    /// <summary>
+    /// Raw angle value (0~35999)
+    /// Unit: 0.01°/LSB, range 0° ~ 359.99°
+    /// Data block is invalid if this value >= 0xFF00
+    /// </summary>
+    public ushort AngleRaw;
+
+    /// <summary>
+    /// Distance readout data (unsigned integer)
+    /// Actual distance = readout data × distance scale
+    /// </summary>
+    public ushort DistanceRaw;
+
+    /// <summary>
+    /// Signal strength (0~65535)
+    /// Indicates the strength of the received signal
+    /// </summary>
+    public ushort SignalStrength;
+
+    /// <summary>
+    /// Reserved for future use
+    /// </summary>
+    public ushort Reserved;
+
+    /// <summary>
+    /// Check if this data block is valid
+    /// </summary>
+    public readonly bool IsValid => AngleRaw < INVALID_ANGLE;
+
+    /// <summary>
+    /// Get angle in degrees (0.00° ~ 359.99°)
+    /// </summary>
+    public readonly double GetAngleDegrees()
+    {
+        return AngleRaw * 0.01;
+    }
+
+    /// <summary>
+    /// Get angle in radians
+    /// </summary>
+    public readonly double GetAngleRadians()
+    {
+        return AngleRaw * 0.01 * Math.PI / 180.0;
+    }
+
+    /// <summary>
+    /// Get actual distance based on distance scale
+    /// </summary>
+    /// <param name="distanceScale">Distance scale from header</param>
+    /// <returns>Actual distance in the same unit as distance scale</returns>
+    public readonly double GetDistance(byte distanceScale)
+    {
+        return DistanceRaw * distanceScale;
+    }
+
+    /// <summary>
+    /// Get X coordinate (distance * cos(angle))
+    /// </summary>
+    /// <param name="distanceScale">Distance scale from header</param>
+    public readonly double GetX(byte distanceScale)
+    {
+        double distance = GetDistance(distanceScale);
+        double angleRad = GetAngleRadians();
+        return distance * Math.Cos(angleRad);
+    }
+
+    /// <summary>
+    /// Get Y coordinate (distance * sin(angle))
+    /// </summary>
+    /// <param name="distanceScale">Distance scale from header</param>
+    public readonly double GetY(byte distanceScale)
+    {
+        double distance = GetDistance(distanceScale);
+        double angleRad = GetAngleRadians();
+        return distance * Math.Sin(angleRad);
+    }
+}