102 lines
3.0 KiB
C#
102 lines
3.0 KiB
C#
using System.Runtime.InteropServices;
|
|
using System.Text;
|
|
|
|
namespace Olei.LidarSensor;
|
|
|
|
/// <summary>
|
|
/// Frame header structure for Olei LiDAR sensor (40 bytes)
|
|
/// All data is in little-endian format
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Sequential, Pack = 1)]
|
|
public struct LidarHeader
|
|
{
|
|
/// <summary>
|
|
/// Frame ID, always 0xFEF0010F
|
|
/// </summary>
|
|
public const uint FRAME_ID = 0xFEF0010F;
|
|
|
|
/// <summary>
|
|
/// Protocol version, current is 0x0200
|
|
/// </summary>
|
|
public const ushort PROTOCOL_VERSION = 0x0200;
|
|
|
|
public uint Id;
|
|
public ushort ProtocolVersion;
|
|
public byte DistanceScale;
|
|
|
|
// Brand name code (3 bytes)
|
|
public byte BrandName1;
|
|
public byte BrandName2;
|
|
public byte BrandName3;
|
|
|
|
// Commercial type code (12 bytes)
|
|
public ulong CommercialType1;
|
|
public uint CommercialType2;
|
|
|
|
public ushort InternalTypeCode;
|
|
public ushort HardwareVersion;
|
|
public ushort SoftwareVersion;
|
|
public uint TimeStamp;
|
|
public ushort RotationRateAndDirection;
|
|
public byte SafeZoneStatus;
|
|
public byte ErrorStatus;
|
|
public uint NtpTimestampInteger;
|
|
|
|
/// <summary>
|
|
/// Check if the frame ID is valid
|
|
/// </summary>
|
|
public readonly bool IsValidFrame => Id == FRAME_ID;
|
|
|
|
/// <summary>
|
|
/// Get rotation rate (Bit[14:0])
|
|
/// </summary>
|
|
public readonly ushort RotationRate => (ushort)(RotationRateAndDirection & 0x7FFF);
|
|
|
|
/// <summary>
|
|
/// Get rotation direction (Bit[15]): false = clockwise, true = counter clockwise
|
|
/// </summary>
|
|
public readonly bool IsCounterClockwise => (RotationRateAndDirection & 0x8000) != 0;
|
|
|
|
/// <summary>
|
|
/// Check if motor fault occurred (BIT0 of ErrorStatus)
|
|
/// </summary>
|
|
public readonly bool HasMotorFault => (ErrorStatus & 0x01) != 0;
|
|
|
|
/// <summary>
|
|
/// Check if abnormal voltage occurred (BIT1 of ErrorStatus)
|
|
/// </summary>
|
|
public readonly bool HasAbnormalVoltage => (ErrorStatus & 0x02) != 0;
|
|
|
|
/// <summary>
|
|
/// Check if temperature fault occurred (BIT2 of ErrorStatus)
|
|
/// </summary>
|
|
public readonly bool HasTemperatureFault => (ErrorStatus & 0x04) != 0;
|
|
|
|
/// <summary>
|
|
/// Check if any error occurred
|
|
/// </summary>
|
|
public readonly bool HasError => ErrorStatus != 0;
|
|
|
|
/// <summary>
|
|
/// Get brand name as string
|
|
/// </summary>
|
|
public readonly string GetBrandName()
|
|
{
|
|
Span<byte> bytes = stackalloc byte[3];
|
|
bytes[0] = BrandName1;
|
|
bytes[1] = BrandName2;
|
|
bytes[2] = BrandName3;
|
|
return Encoding.ASCII.GetString(bytes).TrimEnd('\0');
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get OUTPUT bits (BIT[3:0] of SafeZoneStatus)
|
|
/// </summary>
|
|
public readonly byte GetOutputBits => (byte)(SafeZoneStatus & 0x0F);
|
|
|
|
/// <summary>
|
|
/// Get INPUT bits (BIT[7:4] of SafeZoneStatus)
|
|
/// </summary>
|
|
public readonly byte GetInputBits => (byte)((SafeZoneStatus >> 4) & 0x0F);
|
|
}
|