413 lines
32 KiB
Markdown
413 lines
32 KiB
Markdown
# 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
|