update imu, lidar
This commit is contained in:
@@ -0,0 +1,867 @@
|
||||
using System.Diagnostics;
|
||||
using System.Net.Sockets;
|
||||
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.Hinson;
|
||||
|
||||
/// <summary>
|
||||
/// Configuration cho Hinson FE-35 LiDAR Driver
|
||||
/// </summary>
|
||||
public class HinsonFE35LidarDriverConfig
|
||||
{
|
||||
/// <summary>
|
||||
/// Địa chỉ IP của LiDAR - mặc định: 192.168.1.88 (theo tài liệu Hinson FE)
|
||||
/// </summary>
|
||||
public string IpAddress { get; set; } = "192.168.1.88";
|
||||
|
||||
/// <summary>
|
||||
/// Port của LiDAR - mặc định: 8080
|
||||
/// </summary>
|
||||
public int Port { get; set; } = 8080;
|
||||
|
||||
/// <summary>
|
||||
/// Sử dụng UDP thay vì TCP - mặc định: false (TCP)
|
||||
/// </summary>
|
||||
public bool UseUdp { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Frame ID cho scan data (ROS-style message headers)
|
||||
/// </summary>
|
||||
public string FrameId { get; set; } = "laser";
|
||||
|
||||
/// <summary>
|
||||
/// Tầm quét tối thiểu (mét) - mặc định: 0.05 m
|
||||
/// </summary>
|
||||
public double MinRangeM { get; set; } = 0.05;
|
||||
|
||||
/// <summary>
|
||||
/// Tầm quét tối đa (mét) - mặc định: 35.0 m (FE-35FB)
|
||||
/// </summary>
|
||||
public double MaxRangeM { get; set; } = 35.0;
|
||||
|
||||
/// <summary>
|
||||
/// Góc offset (độ) cộng thêm vào dữ liệu scan.
|
||||
/// Lưu ý: góc 0° của LiDAR Hinson FE là hướng chính sau, chiều dương ngược kim đồng hồ.
|
||||
/// </summary>
|
||||
public double AngleOffsetDeg { get; set; } = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// LiDAR gắn ngược (upside down) - đảo góc quét (180° - angle) như Olei driver
|
||||
/// </summary>
|
||||
public bool Inverted { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Timeout không nhận được dữ liệu thì coi như mất kết nối (milliseconds)
|
||||
/// </summary>
|
||||
public int DataTimeoutMs { get; set; } = 2000;
|
||||
|
||||
/// <summary>
|
||||
/// Có gửi lệnh cấu hình tham số ("SCtrl") xuống LiDAR khi kết nối hay không.
|
||||
/// false: giữ nguyên cấu hình hiện tại trong LiDAR
|
||||
/// </summary>
|
||||
public bool ChangeParam { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// Tần số quay (Hz) - hợp lệ: 12 (12.5Hz), 25, 50. Chỉ dùng khi ChangeParam = true
|
||||
/// </summary>
|
||||
public int SpinFrequencyHz { get; set; } = 25;
|
||||
|
||||
/// <summary>
|
||||
/// Độ phân giải góc (độ) - hợp lệ: "0.025", "0.050", "0.100", "0.200", "0.250", "0.500".
|
||||
/// Chỉ dùng khi ChangeParam = true
|
||||
/// </summary>
|
||||
public string AngleIncrementDeg { get; set; } = "0.100";
|
||||
|
||||
/// <summary>
|
||||
/// Mức lọc nhiễu 0~3 - chỉ dùng khi ChangeParam = true
|
||||
/// </summary>
|
||||
public int NoiseFilterLevel { get; set; } = 1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Driver cho Hinson (兴颂/HINS) FE-35FB-01000 2D LiDAR
|
||||
/// Implements DeviceBase và ILidar interface
|
||||
/// Protocol: TCP/UDP - tham khảo Hinson_FE_ROS_driver_v1.2 và Hinson_FE35使用手册V1.0
|
||||
///
|
||||
/// Luồng hoạt động:
|
||||
/// 1. Kết nối TCP (hoặc UDP) tới LiDAR (mặc định 192.168.1.88:8080)
|
||||
/// 2. (Tuỳ chọn) Gửi lệnh cấu hình "SCtrl" (12 byte, CRC16-Modbus)
|
||||
/// 3. Gửi lệnh bắt đầu đo "RAuto" + 0x01 0x87 0x80 (8 byte)
|
||||
/// 4. Nhận các frame dữ liệu header "HISN":
|
||||
/// - Header 16 byte: [0..3]="HISN", các trường uint16 big-endian:
|
||||
/// start_angle, end_angle (độ), data_size, data_position, measure_size, time
|
||||
/// - Body: data_size điểm × 4 byte little-endian (distance mm, intensity)
|
||||
/// 5. Ghép các frame thành vòng quét 360° hoàn chỉnh khi
|
||||
/// end_angle == 360 và data_position == measure_size
|
||||
/// </summary>
|
||||
[Device(DeviceType.Lidar, "Hinson", "HinsonFE35LidarDriver", "1.0.0",
|
||||
Description = "Hinson FE-35FB-01000 2D LiDAR - TCP/UDP Protocol")]
|
||||
public class HinsonFE35LidarDriver : DeviceBase, ILidar
|
||||
{
|
||||
private readonly HinsonFE35LidarDriverConfig _config = new();
|
||||
|
||||
// Lệnh bắt đầu đo: "RAuto" + 0x01 + CRC (theo hins::kStartCapture)
|
||||
private static readonly byte[] StartCaptureCommand = [0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80];
|
||||
|
||||
// Frame header dữ liệu quét: "HISN"
|
||||
private static readonly byte[] RangeFrameHead = [0x48, 0x49, 0x53, 0x4E];
|
||||
|
||||
// Frame header dữ liệu vùng an toàn (area/obstacle): "WSimu" - 13 byte, bỏ qua
|
||||
private static readonly byte[] AreaFrameHead = [0x57, 0x53, 0x69, 0x6D, 0x75];
|
||||
|
||||
private const int RANGE_FRAME_HEADER_SIZE = 16;
|
||||
private const int AREA_FRAME_SIZE = 13;
|
||||
private const int BYTES_PER_POINT = 4;
|
||||
|
||||
// Giá trị distance (mm) lớn hơn ngưỡng này là không hợp lệ (theo hins::kMaxDistance)
|
||||
private const int MAX_DISTANCE_RAW_MM = 50000;
|
||||
private const double DEFAULT_ACCURACY_M = 0.03; // ±30 mm theo datasheet FE series
|
||||
|
||||
// Connection
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _tcpStream;
|
||||
private UdpClient? _udpClient;
|
||||
private CancellationTokenSource? _receiveCts;
|
||||
private Task? _receiveTask;
|
||||
|
||||
// Receive buffer (dồn dữ liệu TCP stream, tách frame)
|
||||
private readonly byte[] _rxBuffer = new byte[131072];
|
||||
private int _rxLength;
|
||||
|
||||
// Scan accumulation (một vòng 360°)
|
||||
private double[]? _scanRangesRaw; // distance mm, -1 = chưa có dữ liệu
|
||||
private double[]? _scanIntensities;
|
||||
private double _angleIncrementDeg;
|
||||
private DateTime _currentScanStartTime = DateTime.UtcNow;
|
||||
private uint _scanSequenceNumber;
|
||||
private long _lastDataReceivedTicks;
|
||||
|
||||
// Cached measurements
|
||||
private LaserScan? _currentMeasurementData;
|
||||
private DateTime? _lastScanDataTimestamp;
|
||||
private readonly Lock _scanLock = new();
|
||||
|
||||
// Statistics
|
||||
private long _framesReceived;
|
||||
private long _scansGenerated;
|
||||
|
||||
// Scan frequency calculation
|
||||
private readonly Stopwatch _scanFrequencyStopwatch = Stopwatch.StartNew();
|
||||
private long _scansInCurrentSecond;
|
||||
private readonly Stopwatch _propertyUpdateStopwatch = Stopwatch.StartNew();
|
||||
|
||||
/// <summary>
|
||||
/// Constructor with configuration
|
||||
/// </summary>
|
||||
public HinsonFE35LidarDriver(
|
||||
string deviceId,
|
||||
string deviceName, IConfigurationSection configuration)
|
||||
: base(deviceId, deviceName, DeviceType.Lidar)
|
||||
{
|
||||
configuration.Bind(_config);
|
||||
Description = "Hinson FE-35FB-01000 2D LiDAR - TCP/UDP Protocol";
|
||||
}
|
||||
|
||||
#region ILidar Implementation
|
||||
|
||||
/// <summary>
|
||||
/// Current measurement data (scan points)
|
||||
/// </summary>
|
||||
public LaserScan? CurrentMeasurementData => _currentMeasurementData;
|
||||
|
||||
/// <summary>
|
||||
/// Timestamp of the most recent scan data
|
||||
/// </summary>
|
||||
public DateTime? LastScanDataTimestamp => _lastScanDataTimestamp;
|
||||
|
||||
/// <summary>
|
||||
/// Góc quét tối thiểu (radian)
|
||||
/// </summary>
|
||||
public double MinAngleRad => 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// Góc quét tối đa (radian) - LiDAR quét đủ 360°
|
||||
/// </summary>
|
||||
public double MaxAngleRad => 2.0 * Math.PI;
|
||||
|
||||
/// <summary>
|
||||
/// Tầm quét tối thiểu (mét)
|
||||
/// </summary>
|
||||
public double MinRangeM => _config.MinRangeM;
|
||||
|
||||
/// <summary>
|
||||
/// Tầm quét tối đa (mét)
|
||||
/// </summary>
|
||||
public double MaxRangeM => _config.MaxRangeM;
|
||||
|
||||
/// <summary>
|
||||
/// Độ phân giải góc (radian) - tính từ dữ liệu thực tế
|
||||
/// </summary>
|
||||
public double? AngularResolutionRad =>
|
||||
_angleIncrementDeg > 0 ? _angleIncrementDeg * Math.PI / 180.0 : null;
|
||||
|
||||
/// <summary>
|
||||
/// Tần số quét (Hz) - đo từ tốc độ sinh LaserScan thực tế (12.5/25/50 Hz)
|
||||
/// </summary>
|
||||
public double? ScanFrequencyHz { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Field of View (radians)
|
||||
/// </summary>
|
||||
public double FieldOfViewRad => MaxAngleRad - MinAngleRad;
|
||||
|
||||
/// <summary>
|
||||
/// Hỗ trợ đo intensity
|
||||
/// </summary>
|
||||
public bool SupportsIntensity => true;
|
||||
|
||||
/// <summary>
|
||||
/// Độ chính xác đo khoảng cách (mét)
|
||||
/// </summary>
|
||||
public double? AccuracyM => DEFAULT_ACCURACY_M;
|
||||
|
||||
/// <summary>
|
||||
/// Event raised when new scan data is received
|
||||
/// </summary>
|
||||
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
|
||||
|
||||
#endregion
|
||||
|
||||
#region DeviceBase Implementation
|
||||
|
||||
protected override Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
SetProperty("IpAddress", _config.IpAddress);
|
||||
SetProperty("Port", _config.Port.ToString());
|
||||
SetProperty("Transport", _config.UseUdp ? "UDP" : "TCP");
|
||||
SetProperty("FrameId", _config.FrameId);
|
||||
SetProperty("MinRange", $"{_config.MinRangeM:F2} m");
|
||||
SetProperty("MaxRange", $"{_config.MaxRangeM:F2} m");
|
||||
SetProperty("AngularResolution", "N/A");
|
||||
SetProperty("ScanFrequency", "N/A");
|
||||
SetProperty("ConnectionStatus", "Not connected");
|
||||
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
// Dừng receive loop cũ và đóng kết nối cũ nếu có (trường hợp reconnect)
|
||||
await StopReceiveLoopAsync();
|
||||
CloseConnection();
|
||||
|
||||
if (_config.UseUdp)
|
||||
{
|
||||
_udpClient = new UdpClient();
|
||||
_udpClient.Connect(_config.IpAddress, _config.Port);
|
||||
}
|
||||
else
|
||||
{
|
||||
_tcpClient = new TcpClient
|
||||
{
|
||||
NoDelay = true,
|
||||
ReceiveTimeout = _config.DataTimeoutMs
|
||||
};
|
||||
await _tcpClient.ConnectAsync(_config.IpAddress, _config.Port, cancellationToken);
|
||||
_tcpStream = _tcpClient.GetStream();
|
||||
}
|
||||
|
||||
// Gửi lệnh cấu hình tham số nếu được yêu cầu
|
||||
if (_config.ChangeParam)
|
||||
{
|
||||
var paramCommand = BuildParamCommand(
|
||||
_config.SpinFrequencyHz, _config.AngleIncrementDeg, _config.NoiseFilterLevel);
|
||||
await SendAsync(paramCommand, cancellationToken);
|
||||
}
|
||||
|
||||
// Gửi lệnh bắt đầu đo
|
||||
await SendAsync(StartCaptureCommand, cancellationToken);
|
||||
|
||||
// Reset trạng thái nhận dữ liệu
|
||||
_rxLength = 0;
|
||||
ResetScanAccumulation();
|
||||
Interlocked.Exchange(ref _lastDataReceivedTicks, DateTime.UtcNow.Ticks);
|
||||
|
||||
// Bắt đầu vòng lặp nhận dữ liệu
|
||||
_receiveCts = new CancellationTokenSource();
|
||||
_receiveTask = Task.Run(() => ReceiveLoopAsync(_receiveCts.Token), CancellationToken.None);
|
||||
|
||||
SetProperty("ConnectionStatus", "Connected");
|
||||
}
|
||||
|
||||
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
await StopReceiveLoopAsync();
|
||||
CloseConnection();
|
||||
SetProperty("ConnectionStatus", "Disconnected");
|
||||
}
|
||||
|
||||
protected override Task OnResetAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
Interlocked.Exchange(ref _framesReceived, 0);
|
||||
Interlocked.Exchange(ref _scansGenerated, 0);
|
||||
Interlocked.Exchange(ref _scansInCurrentSecond, 0);
|
||||
_scanFrequencyStopwatch.Restart();
|
||||
ScanFrequencyHz = null;
|
||||
|
||||
SetProperty("FramesReceived", "0");
|
||||
SetProperty("ScansGenerated", "0");
|
||||
SetProperty("ScanFrequency", "N/A");
|
||||
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
await Task.Delay(500, cancellationToken);
|
||||
|
||||
if (_receiveTask == null || _receiveTask.IsCompleted)
|
||||
return false;
|
||||
|
||||
if (!_config.UseUdp && (_tcpClient == null || !_tcpClient.Connected))
|
||||
return false;
|
||||
|
||||
// Kiểm tra dữ liệu có đang về hay không
|
||||
var lastDataTicks = Interlocked.Read(ref _lastDataReceivedTicks);
|
||||
var elapsed = DateTime.UtcNow - new DateTime(lastDataTicks, DateTimeKind.Utc);
|
||||
return elapsed.TotalMilliseconds <= _config.DataTimeoutMs;
|
||||
}
|
||||
|
||||
protected override List<PropertyDescription> CreatePropertyDescriptions()
|
||||
{
|
||||
return
|
||||
[
|
||||
new PropertyDescription("IpAddress", "IP Address", "LiDAR IP address"),
|
||||
new PropertyDescription("Port", "Port", "LiDAR TCP/UDP port"),
|
||||
new PropertyDescription("Transport", "Transport", "TCP or UDP"),
|
||||
new PropertyDescription("FrameId", "Frame ID", "ROS-style frame identifier"),
|
||||
new PropertyDescription("MinRange", "Min Range", "Minimum measurement range"),
|
||||
new PropertyDescription("MaxRange", "Max Range", "Maximum measurement range"),
|
||||
new PropertyDescription("AngularResolution", "Angular Resolution", "Angle between scan points"),
|
||||
new PropertyDescription("ScanFrequency", "Scan Frequency", "Actual scan rate (Hz)"),
|
||||
new PropertyDescription("ConnectionStatus", "Connection Status", "Socket connection status"),
|
||||
new PropertyDescription("FramesReceived", "Frames Received", "Number of data frames received"),
|
||||
new PropertyDescription("ScansGenerated", "Scans Generated", "Number of full 360° scans generated"),
|
||||
new PropertyDescription("LastScanTime", "Last Scan Time", "Timestamp of last scan"),
|
||||
];
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
try
|
||||
{
|
||||
_receiveCts?.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// Already disposed, ignore
|
||||
}
|
||||
CloseConnection();
|
||||
_receiveCts?.Dispose();
|
||||
_receiveCts = null;
|
||||
}
|
||||
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Connection Helpers
|
||||
|
||||
private async Task SendAsync(byte[] data, CancellationToken cancellationToken)
|
||||
{
|
||||
if (_config.UseUdp)
|
||||
{
|
||||
if (_udpClient == null)
|
||||
throw new InvalidOperationException("UDP client not connected");
|
||||
await _udpClient.SendAsync(data, cancellationToken);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (_tcpStream == null)
|
||||
throw new InvalidOperationException("TCP stream not connected");
|
||||
await _tcpStream.WriteAsync(data, cancellationToken);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task StopReceiveLoopAsync()
|
||||
{
|
||||
try
|
||||
{
|
||||
_receiveCts?.Cancel();
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// Already disposed, ignore
|
||||
}
|
||||
|
||||
if (_receiveTask != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
await _receiveTask.WaitAsync(TimeSpan.FromSeconds(3));
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// Timeout hoặc task lỗi - bỏ qua, socket sẽ bị đóng bên dưới
|
||||
}
|
||||
_receiveTask = null;
|
||||
}
|
||||
|
||||
_receiveCts?.Dispose();
|
||||
_receiveCts = null;
|
||||
}
|
||||
|
||||
private void CloseConnection()
|
||||
{
|
||||
try
|
||||
{
|
||||
_tcpStream?.Close();
|
||||
_tcpClient?.Close();
|
||||
_udpClient?.Close();
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// Ignore errors during close
|
||||
}
|
||||
finally
|
||||
{
|
||||
_tcpStream = null;
|
||||
_tcpClient = null;
|
||||
_udpClient = null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Vòng lặp nhận dữ liệu từ LiDAR và parse frame
|
||||
/// </summary>
|
||||
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
var buffer = new byte[8192];
|
||||
|
||||
// ReceiveTimeout của TcpClient KHÔNG áp dụng cho async read: nếu rút cáp mạng
|
||||
// (không có TCP RST) thì ReadAsync treo vô hạn. Phải tự đặt timeout bằng WaitAsync
|
||||
// để phát hiện mất dữ liệu và trigger reconnect.
|
||||
var readTimeout = TimeSpan.FromMilliseconds(Math.Max(_config.DataTimeoutMs, 500));
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
int bytesRead;
|
||||
if (_config.UseUdp)
|
||||
{
|
||||
var result = await _udpClient!.ReceiveAsync(cancellationToken)
|
||||
.AsTask().WaitAsync(readTimeout, cancellationToken);
|
||||
bytesRead = result.Buffer.Length;
|
||||
AppendToRxBuffer(result.Buffer, bytesRead);
|
||||
}
|
||||
else
|
||||
{
|
||||
bytesRead = await _tcpStream!.ReadAsync(buffer, cancellationToken)
|
||||
.AsTask().WaitAsync(readTimeout, cancellationToken);
|
||||
if (bytesRead == 0)
|
||||
throw new IOException("LiDAR closed the connection");
|
||||
AppendToRxBuffer(buffer, bytesRead);
|
||||
}
|
||||
|
||||
Interlocked.Exchange(ref _lastDataReceivedTicks, DateTime.UtcNow.Ticks);
|
||||
|
||||
// Parse tất cả frame hoàn chỉnh trong buffer
|
||||
while (TryParseNextFrame()) { }
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// Normal shutdown
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
if (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
var reason = ex is TimeoutException
|
||||
? $"No data from LiDAR for > {readTimeout.TotalMilliseconds:F0} ms (cable unplugged?)"
|
||||
: ex.Message;
|
||||
OnErrorOccurred(new Exception($"Hinson LiDAR receive loop error: {reason}", ex));
|
||||
SetProperty("ConnectionStatus", "Connection lost");
|
||||
|
||||
// Báo cho DeviceBase để state machine chuyển Disconnected và
|
||||
// chạy AutoReconnectLoop (gọi lại OnConnectAsync mở lại socket)
|
||||
_ = CheckConnectionAsync();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Protocol Parsing
|
||||
|
||||
/// <summary>
|
||||
/// Dồn dữ liệu mới vào cuối rx buffer. Nếu tràn buffer (dữ liệu rác) thì reset.
|
||||
/// </summary>
|
||||
private void AppendToRxBuffer(byte[] data, int count)
|
||||
{
|
||||
if (_rxLength + count > _rxBuffer.Length)
|
||||
{
|
||||
// Buffer đầy mà không tách được frame nào - dữ liệu hỏng, bỏ hết làm lại
|
||||
_rxLength = 0;
|
||||
if (count > _rxBuffer.Length)
|
||||
return;
|
||||
}
|
||||
|
||||
Array.Copy(data, 0, _rxBuffer, _rxLength, count);
|
||||
_rxLength += count;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Xoá byteCount byte đầu của rx buffer
|
||||
/// </summary>
|
||||
private void ConsumeRxBuffer(int byteCount)
|
||||
{
|
||||
if (byteCount >= _rxLength)
|
||||
{
|
||||
_rxLength = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
Array.Copy(_rxBuffer, byteCount, _rxBuffer, 0, _rxLength - byteCount);
|
||||
_rxLength -= byteCount;
|
||||
}
|
||||
|
||||
private static bool MatchAt(byte[] buffer, int index, byte[] pattern)
|
||||
{
|
||||
for (int i = 0; i < pattern.Length; i++)
|
||||
{
|
||||
if (buffer[index + i] != pattern[i])
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Tìm và xử lý frame kế tiếp trong rx buffer.
|
||||
/// Trả về true nếu đã xử lý được một frame (cần gọi lại để xử lý tiếp).
|
||||
/// </summary>
|
||||
private bool TryParseNextFrame()
|
||||
{
|
||||
if (_rxLength < AreaFrameHead.Length)
|
||||
return false;
|
||||
|
||||
// Tìm frame header ("HISN" - dữ liệu quét, "WSimu" - dữ liệu vùng an toàn)
|
||||
int headIndex = -1;
|
||||
bool isAreaFrame = false;
|
||||
int searchEnd = _rxLength - AreaFrameHead.Length;
|
||||
for (int i = 0; i <= searchEnd; i++)
|
||||
{
|
||||
if (MatchAt(_rxBuffer, i, RangeFrameHead))
|
||||
{
|
||||
headIndex = i;
|
||||
isAreaFrame = false;
|
||||
break;
|
||||
}
|
||||
if (MatchAt(_rxBuffer, i, AreaFrameHead))
|
||||
{
|
||||
headIndex = i;
|
||||
isAreaFrame = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (headIndex < 0)
|
||||
{
|
||||
// Không có header - giữ lại vài byte cuối phòng header bị cắt giữa 2 lần nhận
|
||||
if (_rxLength > AreaFrameHead.Length)
|
||||
ConsumeRxBuffer(_rxLength - AreaFrameHead.Length);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Bỏ dữ liệu rác trước header
|
||||
if (headIndex > 0)
|
||||
ConsumeRxBuffer(headIndex);
|
||||
|
||||
if (isAreaFrame)
|
||||
{
|
||||
// Frame vùng an toàn (obstacle area) - không dùng, bỏ qua
|
||||
if (_rxLength < AREA_FRAME_SIZE)
|
||||
return false;
|
||||
ConsumeRxBuffer(AREA_FRAME_SIZE);
|
||||
return true;
|
||||
}
|
||||
|
||||
return TryParseRangeFrame();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Parse frame dữ liệu quét "HISN" ở đầu rx buffer
|
||||
/// </summary>
|
||||
private bool TryParseRangeFrame()
|
||||
{
|
||||
if (_rxLength < RANGE_FRAME_HEADER_SIZE)
|
||||
return false;
|
||||
|
||||
// Header: các trường uint16 big-endian
|
||||
int startAngle = (_rxBuffer[4] << 8) | _rxBuffer[5]; // độ
|
||||
int endAngle = (_rxBuffer[6] << 8) | _rxBuffer[7]; // độ
|
||||
int dataSize = (_rxBuffer[8] << 8) | _rxBuffer[9]; // số điểm trong frame này
|
||||
int dataPosition = (_rxBuffer[10] << 8) | _rxBuffer[11]; // vị trí điểm hiện tại trong khối
|
||||
int measureSize = (_rxBuffer[12] << 8) | _rxBuffer[13]; // tổng số điểm của khối góc
|
||||
|
||||
// Theo ROS driver: data_size không được vượt quá measure_size
|
||||
if (dataSize > measureSize)
|
||||
dataSize = measureSize;
|
||||
|
||||
int frameSize = RANGE_FRAME_HEADER_SIZE + dataSize * BYTES_PER_POINT;
|
||||
if (_rxLength < frameSize)
|
||||
return false; // Chưa nhận đủ frame
|
||||
|
||||
// Validate header - loại frame lỗi
|
||||
if (measureSize <= 0 || endAngle <= startAngle || endAngle > 360)
|
||||
{
|
||||
ConsumeRxBuffer(RANGE_FRAME_HEADER_SIZE);
|
||||
return true;
|
||||
}
|
||||
|
||||
Interlocked.Increment(ref _framesReceived);
|
||||
|
||||
// Độ phân giải góc và tổng số điểm một vòng quét
|
||||
double angleIncrementDeg = (double)(endAngle - startAngle) / measureSize;
|
||||
int totalPoints = (int)Math.Round(360.0 / angleIncrementDeg);
|
||||
|
||||
if (totalPoints <= 0 || totalPoints > 40000)
|
||||
{
|
||||
ConsumeRxBuffer(frameSize);
|
||||
return true;
|
||||
}
|
||||
|
||||
// Cấp lại buffer tích luỹ khi độ phân giải thay đổi
|
||||
if (_scanRangesRaw == null || _scanRangesRaw.Length != totalPoints)
|
||||
{
|
||||
_scanRangesRaw = new double[totalPoints];
|
||||
_scanIntensities = new double[totalPoints];
|
||||
Array.Fill(_scanRangesRaw, -1.0);
|
||||
_currentScanStartTime = DateTime.UtcNow;
|
||||
}
|
||||
_angleIncrementDeg = angleIncrementDeg;
|
||||
|
||||
// Index của điểm đầu tiên trong frame này (theo công thức của ROS driver)
|
||||
int beginPointIndex = (int)(startAngle / angleIncrementDeg) + dataPosition - dataSize;
|
||||
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
int offset = RANGE_FRAME_HEADER_SIZE + i * BYTES_PER_POINT;
|
||||
|
||||
// Distance và intensity: uint16 little-endian
|
||||
int distanceMm = _rxBuffer[offset] | (_rxBuffer[offset + 1] << 8);
|
||||
int intensity = _rxBuffer[offset + 2] | (_rxBuffer[offset + 3] << 8);
|
||||
|
||||
int index = beginPointIndex + i;
|
||||
if (index < 0 || index >= totalPoints)
|
||||
continue;
|
||||
|
||||
_scanRangesRaw![index] = distanceMm;
|
||||
_scanIntensities![index] = intensity;
|
||||
}
|
||||
|
||||
ConsumeRxBuffer(frameSize);
|
||||
|
||||
// Hoàn thành một vòng quét 360°
|
||||
if (endAngle == 360 && dataPosition == measureSize)
|
||||
{
|
||||
PublishCompletedScan(totalPoints);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Scan Publishing
|
||||
|
||||
private void ResetScanAccumulation()
|
||||
{
|
||||
_scanRangesRaw = null;
|
||||
_scanIntensities = null;
|
||||
_angleIncrementDeg = 0.0;
|
||||
_currentScanStartTime = DateTime.UtcNow;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Build LaserScan từ dữ liệu một vòng quét hoàn chỉnh và fire event
|
||||
/// </summary>
|
||||
private void PublishCompletedScan(int totalPoints)
|
||||
{
|
||||
if (_scanRangesRaw == null || _scanIntensities == null)
|
||||
return;
|
||||
|
||||
var scanStartTime = _currentScanStartTime;
|
||||
var header = new Header(
|
||||
seq: _scanSequenceNumber++,
|
||||
stamp: scanStartTime,
|
||||
frameId: _config.FrameId
|
||||
);
|
||||
|
||||
double angleIncrementRad = 2.0 * Math.PI / totalPoints;
|
||||
|
||||
double[] ranges = new double[totalPoints];
|
||||
double[] intensities = new double[totalPoints];
|
||||
|
||||
// Offset index: xoay dữ liệu theo AngleOffsetDeg và tuỳ chọn Inverted
|
||||
double offsetDeg = _config.AngleOffsetDeg + (_config.Inverted ? 180.0 : 0.0);
|
||||
int indexOffset = (int)Math.Round(offsetDeg / 360.0 * totalPoints);
|
||||
|
||||
for (int i = 0; i < totalPoints; i++)
|
||||
{
|
||||
int srcIndex = i - indexOffset;
|
||||
srcIndex %= totalPoints;
|
||||
if (srcIndex < 0)
|
||||
srcIndex += totalPoints;
|
||||
|
||||
double distanceRawMm = _scanRangesRaw[srcIndex];
|
||||
double distanceM = distanceRawMm / 1000.0;
|
||||
|
||||
if (distanceRawMm <= 0 || distanceRawMm > MAX_DISTANCE_RAW_MM ||
|
||||
distanceM < _config.MinRangeM || distanceM > _config.MaxRangeM)
|
||||
{
|
||||
ranges[i] = -1.0; // No detection (JSON-safe, giống Olei driver)
|
||||
intensities[i] = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
ranges[i] = distanceM;
|
||||
intensities[i] = _scanIntensities[srcIndex];
|
||||
}
|
||||
}
|
||||
|
||||
// Chuẩn bị buffer cho vòng quét kế tiếp
|
||||
Array.Fill(_scanRangesRaw, -1.0);
|
||||
Array.Fill(_scanIntensities, 0.0);
|
||||
_currentScanStartTime = DateTime.UtcNow;
|
||||
|
||||
Interlocked.Increment(ref _scansGenerated);
|
||||
Interlocked.Increment(ref _scansInCurrentSecond);
|
||||
UpdateScanFrequency();
|
||||
|
||||
double scanTime = ScanFrequencyHz.HasValue && ScanFrequencyHz.Value > 0
|
||||
? 1.0 / ScanFrequencyHz.Value
|
||||
: 0.04; // Default 25 Hz
|
||||
|
||||
var scan = new LaserScan
|
||||
{
|
||||
Header = header,
|
||||
AngleMin = 0.0,
|
||||
AngleMax = 2.0 * Math.PI,
|
||||
AngleIncrement = angleIncrementRad,
|
||||
TimeIncrement = scanTime / totalPoints,
|
||||
ScanTime = scanTime,
|
||||
RangeMin = _config.MinRangeM,
|
||||
RangeMax = _config.MaxRangeM,
|
||||
Ranges = ranges,
|
||||
Intensities = intensities
|
||||
};
|
||||
|
||||
lock (_scanLock)
|
||||
{
|
||||
_currentMeasurementData = scan;
|
||||
_lastScanDataTimestamp = scan.Header.Stamp;
|
||||
}
|
||||
|
||||
// Cập nhật UI properties tối đa 1 lần/giây
|
||||
if (_propertyUpdateStopwatch.ElapsedMilliseconds > 1000)
|
||||
{
|
||||
_propertyUpdateStopwatch.Restart();
|
||||
SetProperty("LastScanTime", scan.Header.Stamp.ToString("HH:mm:ss.fff"));
|
||||
SetProperty("ScanFrequency", ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A");
|
||||
SetProperty("AngularResolution", $"{_angleIncrementDeg:F3}°");
|
||||
SetProperty("FramesReceived", Interlocked.Read(ref _framesReceived).ToString());
|
||||
SetProperty("ScansGenerated", Interlocked.Read(ref _scansGenerated).ToString());
|
||||
}
|
||||
|
||||
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scan.Header.Stamp, scan));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Cập nhật tần số quét dựa trên số LaserScan sinh ra mỗi giây
|
||||
/// </summary>
|
||||
private void UpdateScanFrequency()
|
||||
{
|
||||
if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
|
||||
{
|
||||
long scanCount = Interlocked.Exchange(ref _scansInCurrentSecond, 0);
|
||||
double elapsedSeconds = _scanFrequencyStopwatch.ElapsedMilliseconds / 1000.0;
|
||||
ScanFrequencyHz = scanCount / elapsedSeconds;
|
||||
_scanFrequencyStopwatch.Restart();
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Command Builders
|
||||
|
||||
/// <summary>
|
||||
/// Build lệnh cấu hình tham số LiDAR "SCtrl" (12 byte, CRC16-Modbus ở 2 byte cuối)
|
||||
/// </summary>
|
||||
private static byte[] BuildParamCommand(int spinFrequencyHz, string angleIncrementDeg, int noiseFilterLevel)
|
||||
{
|
||||
var command = new byte[12];
|
||||
command[0] = (byte)'S';
|
||||
command[1] = (byte)'C';
|
||||
command[2] = (byte)'t';
|
||||
command[3] = (byte)'r';
|
||||
command[4] = (byte)'l';
|
||||
|
||||
command[5] = 0x00; // run_state: 0x00 = run, 0x01 = stop
|
||||
command[6] = 0x00; // reserved
|
||||
|
||||
command[7] = spinFrequencyHz switch
|
||||
{
|
||||
12 => 0x00, // 12.5 Hz
|
||||
25 => 0x01,
|
||||
50 => 0x02,
|
||||
_ => 0x00
|
||||
};
|
||||
|
||||
command[8] = angleIncrementDeg switch
|
||||
{
|
||||
"0.025" => 0x00,
|
||||
"0.050" => 0x01,
|
||||
"0.100" => 0x02,
|
||||
"0.200" => 0x03,
|
||||
"0.250" => 0x04,
|
||||
"0.500" => 0x05,
|
||||
_ => 0x02
|
||||
};
|
||||
|
||||
command[9] = (byte)Math.Clamp(noiseFilterLevel, 0, 3);
|
||||
|
||||
// CRC16-Modbus trên 10 byte đầu, low byte trước
|
||||
ushort crc = Crc16Modbus(command, 10);
|
||||
command[10] = (byte)(crc & 0x00FF);
|
||||
command[11] = (byte)((crc & 0xFF00) >> 8);
|
||||
|
||||
return command;
|
||||
}
|
||||
|
||||
private static ushort Crc16Modbus(byte[] data, int length)
|
||||
{
|
||||
ushort crc = 0xFFFF;
|
||||
for (int i = 0; i < length; i++)
|
||||
{
|
||||
crc ^= data[i];
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
if ((crc & 0x0001) != 0)
|
||||
crc = (ushort)((crc >> 1) ^ 0xA001);
|
||||
else
|
||||
crc >>= 1;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
Reference in New Issue
Block a user