94 lines
2.6 KiB
C#
94 lines
2.6 KiB
C#
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);
|
||
}
|
||
}
|