Initial commit
This commit is contained in:
@@ -0,0 +1,412 @@
|
||||
# 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
|
||||
Reference in New Issue
Block a user