32 KiB
32 KiB
Olei LiDAR Sensor - Architecture
System Architecture
┌─────────────────────────────────────────────────────────────┐
│ Olei LiDAR Sensor │
│ (LR-1F / LR-1BS Hardware) │
└───────────────────────┬─────────────────────────────────────┘
│ UDP/IP (Port 2368)
│ 1240 bytes packets
│ Little-endian format
▼
┌─────────────────────────────────────────────────────────────┐
│ OleiLidarServer (Main Component) │
├─────────────────────────────────────────────────────────────┤
│ │
│ ┌────────────────────────────────────────────────┐ │
│ │ Dedicated Receive Thread │ │
│ │ (ThreadPriority.AboveNormal) │ │
│ │ │ │
│ │ ┌──────────────────────────────────┐ │ │
│ │ │ UdpClient.Receive() │ │ │
│ │ │ • Non-blocking receive │ │ │
│ │ │ • 1MB receive buffer │ │ │
│ │ └──────────────┬───────────────────┘ │ │
│ │ │ │ │
│ │ ▼ │ │
│ │ ┌──────────────────────────────────┐ │ │
│ │ │ Quick Validation │ │ │
│ │ │ • Check Frame ID │ │ │
│ │ │ • Verify packet size │ │ │
│ │ └──────────────┬───────────────────┘ │ │
│ │ │ │ │
│ │ ▼ │ │
│ │ ┌──────────────────────────────────┐ │ │
│ │ │ Get Packet from Pool │ │ │
│ │ │ • ConcurrentBag<Packet> │ │ │
│ │ │ • Reuse allocated objects │ │ │
│ │ └──────────────┬───────────────────┘ │ │
│ │ │ │ │
│ │ ▼ │ │
│ │ ┌──────────────────────────────────┐ │ │
│ │ │ LidarPacketParser │ │ │
│ │ │ • Zero-allocation parsing │ │ │
│ │ │ • Span<byte> based │ │ │
│ │ │ • MemoryMarshal.Read<T> │ │ │
│ │ └──────────────┬───────────────────┘ │ │
│ │ │ │ │
│ │ ▼ │ │
│ │ ┌──────────────────────────────────┐ │ │
│ │ │ Raise DataReceived Event │ │ │
│ │ │ • Thread-safe event invocation │ │ │
│ │ │ • Pass LidarDataPacket │ │ │
│ │ └──────────────────────────────────┘ │ │
│ │ │ │
│ └────────────────────────────────────────────────┘ │
│ │
│ ┌────────────────────────────────────────────────┐ │
│ │ Resource Management │ │
│ │ • ArrayPool<byte> for buffers │ │
│ │ • ConcurrentBag for packet pooling │ │
│ │ • Interlocked counters for statistics │ │
│ └────────────────────────────────────────────────┘ │
│ │
└────────────────────┬────────────────────────────────────────┘
│ Events
│ • DataReceived
│ • ErrorOccurred
▼
┌─────────────────────────────────────────────────────────────┐
│ Your Application │
│ │
│ server.DataReceived += (sender, e) => │
│ { │
│ var packet = e.Packet; │
│ // Process LiDAR data │
│ e.Server.ReturnPacketToPool(packet); // IMPORTANT! │
│ }; │
│ │
└─────────────────────────────────────────────────────────────┘
Data Flow
Packet Structure (1240 bytes)
┌────────────────────────────────────────────────────────────────┐
│ PACKET (1240 bytes) │
├────────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────────────────────────────────────────────────┐ │
│ │ HEADER (40 bytes) │ │
│ ├──────────────────────────────────────────────────────────┤ │
│ │ Offset │ Size │ Field │ │
│ ├────────┼──────┼──────────────────────────────────────────┤ │
│ │ 0 │ 4 │ Frame ID (0xFEF0010F) │ │
│ │ 4 │ 2 │ Protocol Version (0x0200) │ │
│ │ 6 │ 1 │ Distance Scale │ │
│ │ 7 │ 3 │ Brand Name │ │
│ │ 10 │ 12 │ Commercial Type │ │
│ │ 22 │ 2 │ Internal Type Code │ │
│ │ 24 │ 2 │ Hardware Version │ │
│ │ 26 │ 2 │ Software Version │ │
│ │ 28 │ 4 │ Time Stamp │ │
│ │ 32 │ 2 │ Rotation Rate & Direction │ │
│ │ 34 │ 1 │ Safe Zone Status │ │
│ │ 35 │ 1 │ Error Status │ │
│ │ 36 │ 4 │ NTP Timestamp (integer part) │ │
│ └──────────────────────────────────────────────────────────┘ │
│ │
│ ┌──────────────────────────────────────────────────────────┐ │
│ │ DATA BLOCKS (150 × 8 = 1200 bytes) │ │
│ ├──────────────────────────────────────────────────────────┤ │
│ │ │ │
│ │ Block 0: [Angle][Distance][Strength][Reserved] │ │
│ │ └─2B──┘└───2B───┘└───2B───┘└───2B───┘ │ │
│ │ │ │
│ │ Block 1: [Angle][Distance][Strength][Reserved] │ │
│ │ │ │
│ │ Block 2: [Angle][Distance][Strength][Reserved] │ │
│ │ │ │
│ │ ... │ │
│ │ │ │
│ │ Block 149: [Angle][Distance][Strength][Reserved] │ │
│ │ │ │
│ │ Each block represents one measurement point: │ │
│ │ • Angle: 0-35999 (0.01° resolution) │ │
│ │ • Distance: Raw value × Distance Scale │ │
│ │ • Strength: Signal strength (0-65535) │ │
│ │ • Invalid if Angle >= 0xFF00 │ │
│ │ │ │
│ └──────────────────────────────────────────────────────────┘ │
│ │
└────────────────────────────────────────────────────────────────┘
Class Diagram
┌─────────────────────────────────────────────────────────────┐
│ LidarHeader │
│ (struct, 40 bytes) │
├─────────────────────────────────────────────────────────────┤
│ + Id: uint │
│ + ProtocolVersion: ushort │
│ + DistanceScale: byte │
│ + TimeStamp: uint │
│ + RotationRateAndDirection: ushort │
│ + ErrorStatus: byte │
│ + ... │
├─────────────────────────────────────────────────────────────┤
│ + IsValidFrame: bool │
│ + RotationRate: ushort │
│ + IsCounterClockwise: bool │
│ + HasMotorFault: bool │
│ + HasAbnormalVoltage: bool │
│ + HasTemperatureFault: bool │
└─────────────────────────────────────────────────────────────┘
│
│ used by
▼
┌─────────────────────────────────────────────────────────────┐
│ LidarDataPacket │
│ (class, 1240 bytes total) │
├─────────────────────────────────────────────────────────────┤
│ + Header: LidarHeader │
│ + DataBlocks: LidarDataBlock[150] │
│ + ReceivedTime: DateTime │
├─────────────────────────────────────────────────────────────┤
│ + GetValidDataBlocks(): IEnumerable<LidarDataBlock> │
│ + GetValidDataBlockCount(): int │
│ + GetValidPoints(): IEnumerable<(angle, distance)> │
│ + GetValidCartesianPoints(): IEnumerable<(x, y)> │
└─────────────────────────────────────────────────────────────┘
│
│ contains
▼
┌─────────────────────────────────────────────────────────────┐
│ LidarDataBlock │
│ (struct, 8 bytes) │
├─────────────────────────────────────────────────────────────┤
│ + AngleRaw: ushort │
│ + DistanceRaw: ushort │
│ + SignalStrength: ushort │
│ + Reserved: ushort │
├─────────────────────────────────────────────────────────────┤
│ + IsValid: bool │
│ + GetAngleDegrees(): double │
│ + GetAngleRadians(): double │
│ + GetDistance(scale): double │
│ + GetX(scale): double │
│ + GetY(scale): double │
└─────────────────────────────────────────────────────────────┘
┌─────────────────────────────────────────────────────────────┐
│ LidarPacketParser (static) │
├─────────────────────────────────────────────────────────────┤
│ + TryParse(byte[], length, packet): bool │
│ + TryParse(ReadOnlySpan<byte>, packet): bool │
│ + IsValidPacket(ReadOnlySpan<byte>): bool │
│ + GetDistanceScale(ReadOnlySpan<byte>): byte │
│ + GetTimestamp(ReadOnlySpan<byte>): uint │
│ + GetErrorStatus(ReadOnlySpan<byte>): byte │
│ + HasErrors(ReadOnlySpan<byte>): bool │
└─────────────────────────────────────────────────────────────┘
│
│ used by
▼
┌─────────────────────────────────────────────────────────────┐
│ OleiLidarServer │
│ : IDisposable │
├─────────────────────────────────────────────────────────────┤
│ - _udpClient: UdpClient │
│ - _receiveThread: Thread │
│ - _bufferPool: ArrayPool<byte> │
│ - _packetPool: ConcurrentBag<LidarDataPacket> │
│ - _isRunning: bool │
├─────────────────────────────────────────────────────────────┤
│ + DataReceived: event │
│ + ErrorOccurred: event │
├─────────────────────────────────────────────────────────────┤
│ + Start(): void │
│ + Stop(): void │
│ + ReturnPacketToPool(packet): void │
│ + GetStatistics(): string │
│ + ResetStatistics(): void │
│ + Dispose(): void │
├─────────────────────────────────────────────────────────────┤
│ - ReceiveLoop(): void │
│ - ProcessPacket(data): void │
└─────────────────────────────────────────────────────────────┘
Threading Model
┌─────────────────────────────────────────────────────────────┐
│ Main Thread │
│ • Start() / Stop() server │
│ • Subscribe to events │
│ • Application logic │
└───────────────────────┬─────────────────────────────────────┘
│
│ Starts/Stops
▼
┌─────────────────────────────────────────────────────────────┐
│ Dedicated Receive Thread │
│ (ThreadPriority.AboveNormal) │
│ │
│ while (_isRunning) │
│ { │
│ // Non-blocking check for available data │
│ if (Available > 0) │
│ { │
│ byte[] data = Receive(); │
│ ProcessPacket(data); // Parse & raise event │
│ } │
│ else │
│ { │
│ Thread.Sleep(1); // Prevent busy-wait │
│ } │
│ } │
│ │
└───────────────────────┬─────────────────────────────────────┘
│
│ Raises Event
▼
┌─────────────────────────────────────────────────────────────┐
│ Event Handler Thread │
│ (Same as Receive Thread Context) │
│ │
│ server.DataReceived += (sender, e) => │
│ { │
│ // WARNING: This runs on receive thread! │
│ // Keep processing quick or dispatch to another thread│
│ ProcessData(e.Packet); │
│ e.Server.ReturnPacketToPool(e.Packet); │
│ }; │
│ │
└─────────────────────────────────────────────────────────────┘
Memory Management Strategy
┌─────────────────────────────────────────────────────────────┐
│ Memory Pools │
├─────────────────────────────────────────────────────────────┤
│ │
│ ┌──────────────────────────────────────┐ │
│ │ ArrayPool<byte>.Shared │ │
│ │ ┌──────────────────────────┐ │ │
│ │ │ Buffer 1 (2480 bytes) │ │ │
│ │ ├──────────────────────────┤ │ │
│ │ │ Buffer 2 (2480 bytes) │ │ │
│ │ ├──────────────────────────┤ │ │
│ │ │ Buffer 3 (2480 bytes) │ │ │
│ │ └──────────────────────────┘ │ │
│ │ │ │
│ │ • Rented in ReceiveLoop() │ │
│ │ • Returned after thread exits │ │
│ │ • Shared across application │ │
│ └──────────────────────────────────────┘ │
│ │
│ ┌──────────────────────────────────────┐ │
│ │ ConcurrentBag<LidarDataPacket> │ │
│ │ ┌──────────────────────────┐ │ │
│ │ │ Packet 1 (reusable) │ │ │
│ │ ├──────────────────────────┤ │ │
│ │ │ Packet 2 (reusable) │ │ │
│ │ ├──────────────────────────┤ │ │
│ │ │ Packet 3 (reusable) │ │ │
│ │ ├──────────────────────────┤ │ │
│ │ │ ... │ │ │
│ │ ├──────────────────────────┤ │ │
│ │ │ Packet N (max 50) │ │ │
│ │ └──────────────────────────┘ │ │
│ │ │ │
│ │ • Pre-allocated (10 initially) │ │
│ │ • Taken for each received packet │ │
│ │ • Must be returned by user! │ │
│ │ • Limited to 50 to prevent bloat │ │
│ └──────────────────────────────────────┘ │
│ │
└─────────────────────────────────────────────────────────────┘
Lifecycle:
1. Receive data → Get buffer from ArrayPool
2. Parse → Get packet from ConcurrentBag (or new)
3. Raise event → User processes packet
4. User returns → Packet back to ConcurrentBag
5. Thread exits → Buffer back to ArrayPool
Benefits:
✓ Minimal GC pressure after warm-up
✓ Constant memory usage
✓ Fast allocation/deallocation
✓ Thread-safe pools
Performance Considerations
Hot Path Optimization
Critical Path (minimize latency):
1. UdpClient.Receive() ← I/O bound
2. Quick validation ← Few CPU cycles
3. Get packet from pool ← Lock-free
4. MemoryMarshal.Read<T>() ← Zero-copy
5. Raise event ← Delegate invoke
Total latency target: < 1ms
Memory Allocation Profile
Cold Start (first few packets):
• Buffer pool allocation
• Packet pool pre-allocation
• Event delegate allocation
Warm Running (steady state):
• Near-zero allocations
• No GC Gen0 collections (ideal)
• Flat memory profile
Error Recovery
┌─────────────────────────────────────────────────────────────┐
│ Error Scenarios │
├─────────────────────────────────────────────────────────────┤
│ │
│ 1. Invalid Packet │
│ → Quick validation fails │
│ → Increment error counter │
│ → Continue receiving │
│ │
│ 2. Socket Exception │
│ → Catch in ReceiveLoop │
│ → Raise ErrorOccurred event │
│ → Continue or exit based on error type │
│ │
│ 3. Parse Exception │
│ → Catch in ProcessPacket │
│ → Increment error counter │
│ → Return packet to pool │
│ → Continue receiving │
│ │
│ 4. Hardware Error (from LiDAR) │
│ → Parsed from Header.ErrorStatus │
│ → Accessible via HasMotorFault, etc. │
│ → User handles in DataReceived event │
│ │
└─────────────────────────────────────────────────────────────┘
Summary
This architecture provides:
- High throughput: Dedicated thread, minimal latency
- Low memory: Pooling strategy, zero-allocation parsing
- Thread safety: Lock-free where possible, Interlocked for counters
- Robustness: Error handling at every level
- Simplicity: Event-based API, easy to use
- Performance: < 1ms latency, > 1000 packets/sec