131 lines
3.6 KiB
C#
131 lines
3.6 KiB
C#
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}]";
|
|
}
|
|
}
|