QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update imu, lidar

Browse Source
This commit is contained in:
QUYVN
2026-07-14 15:15:34 +07:00
parent dddf3fc594
commit 921bb91084
6 changed files with 3197 additions and 868 deletions
Download Patch File Download Diff File Expand all files Collapse all files

859
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9IMU.cs Normal file
View File

@@ -0,0 +1,859 @@
using System.Diagnostics;
using System.Threading;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Sensor;
namespace RobotNet10.RobotApp.Drivers.HfiA9IMU
{
[Device(DeviceType.Imu, "HfiA9IMU", "HfiA9IMU", "1.0.0", Description = "HandsFree HFI-A9 IMU Driver")]
public class HfiA9IMU : DeviceBase, IInertialMeasurementUnit
{
private readonly HfiA9Reader IMU;
// Cached data
private AccelStamped _cachedAcceleration;
private Vector3Stamped _cachedAngularVelocity;
private Vector3Stamped _cachedMagnetometer;
private Vector3Stamped _cachedOrientation;
private QuaternionStamped _cachedQuaternion;
private bool _isCalibrated = false;
private DateTime _lastUpdateTime = DateTime.UtcNow;
private double _sampleRate = 0.0;
private readonly string _portName;
private readonly int _timeOut;
private readonly int _baudRate;
private readonly bool _printDataEnabled;
private readonly TimeSpan _printDataInterval;
private Timer? _printDataTimer;
// Sample rate calculation
private int _sampleCount = 0;
private DateTime _sampleRateStartTime = DateTime.UtcNow;
private readonly TimeSpan _sampleRateWindow = TimeSpan.FromSeconds(1.0);
// Calibration: 2 giay dau sau lan nhan du lieu dau tien de thu thap bias (robot dung yen tuyet doi)
private static readonly TimeSpan CalibrationDuration = TimeSpan.FromSeconds(2.0);
private static readonly TimeSpan CalibrationWaitTimeout = TimeSpan.FromSeconds(5.0);
private const double GravityMps2 = 9.81;
private const double GravityToleranceMps2 = 2.0;
// IMU outlier validation thresholds
private const double MaxValidAccelerationMps2 = 50.0;
private const double MaxValidAngularVelocityRadS = 20.0;
// Thread-safety: Lock for cached sensor data (struct assignments are NOT atomic)
private readonly object _dataLock = new();
// High-precision timestamp using Stopwatch (DateTime.UtcNow has ~10-15ms precision)
private readonly Stopwatch _highPrecisionTimer = Stopwatch.StartNew();
private DateTime _timerStartUtc = DateTime.UtcNow;
private DateTime? _calibrationStartUtc;
private bool _calibrationDone;
private TaskCompletionSource<bool>? _calibrationCompletedTcs;
private double _calibrationSumAccX, _calibrationSumAccY, _calibrationSumAccZ;
private double _calibrationSumGx, _calibrationSumGy, _calibrationSumGz;
private double _calibrationSumRoll, _calibrationSumPitch, _calibrationSumYaw;
private int _calibrationCount;
private double _accBiasX, _accBiasY, _accBiasZ;
private double _gyroBiasX, _gyroBiasY, _gyroBiasZ;
private double _orientationBiasRoll, _orientationBiasPitch, _orientationBiasYaw;
// Yaw integration: tich phan CalibratedGz thay vi dung firmware AHRS Yaw
// Roll/Pitch van dung firmware AHRS vi co gravity reference (khong drift)
private double _integratedYaw;
private DateTime _lastIntegrationTime;
// Diagnostic: log drift moi 5 giay
private DateTime _lastDiagnosticLog = DateTime.MinValue;
private static readonly TimeSpan DiagnosticLogInterval = TimeSpan.FromSeconds(5.0);
// Events (interface)
public event EventHandler<AccelerationChangedEventArgs>? AccelerationChanged;
public event EventHandler<AngularVelocityChangedEventArgs>? AngularVelocityChanged;
public event EventHandler<MagnetometerChangedEventArgs>? MagnetometerChanged;
public event EventHandler<OrientationChangedEventArgs>? OrientationChanged;
// Event thong nhat cho SensorPipeline
public event EventHandler<ImuDataChangedEventArgs>? ImuDataChanged;
public HfiA9IMU(string deviceId, string deviceName, IConfigurationSection connection)
: base(deviceId, deviceName, DeviceType.Imu)
{
_portName = connection.GetValue<string>("Port") ?? throw new Exception("Port is required");
_baudRate = connection.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
_timeOut = connection.GetValue<int?>("TimeOut") ?? throw new Exception("Timeout is required");
IMU = new HfiA9Reader(_portName, _baudRate, _timeOut);
// Continuous IMU data printing (optional)
_printDataEnabled = connection.GetValue<bool?>("DebugEnabled") ?? false;
_printDataInterval = TimeSpan.FromMilliseconds(connection.GetValue<int?>("PrintDataIntervalMs") ?? 200);
// Doc cau hinh neu co
var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
if (autoReconnectEnabled.HasValue)
AutoReconnectEnabled = autoReconnectEnabled.Value;
else
AutoReconnectEnabled = true;
if (reconnectDelayMs.HasValue)
ReconnectDelayMs = reconnectDelayMs.Value;
if (maxReconnectAttempts.HasValue)
MaxReconnectAttempts = maxReconnectAttempts.Value;
_cachedAcceleration = CreateAccelStamped(0, 0, 9.81, DateTime.UtcNow);
_cachedAngularVelocity = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
_cachedMagnetometer = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
_cachedOrientation = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
_cachedQuaternion = CreateQuaternionStamped(1, 0, 0, 0, DateTime.UtcNow);
// Khoi tao gia tri properties
UpdateProperties();
}
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
{
yield return new PropertyDescription("IsCalibrated", "Calibrated", "Trang thai calibrate")
{
DataType = "boolean",
IsReadOnly = true,
DisplayOrder = 1,
Category = "Trang thai",
DefaultValue = "true"
};
yield return new PropertyDescription("SampleRate", "Sample Rate (Hz)", "Tan so lay mau (Hz)")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 2,
Category = "Cau hinh",
DefaultValue = "150"
};
yield return new PropertyDescription("Acceleration", "Acceleration (m/s²)", "Gia toc 3 truc (m/s²)")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 3,
Category = "Du lieu",
DefaultValue = "0, 0, 9.81"
};
yield return new PropertyDescription("AngularVelocity", "Angular Velocity (rad/s)", "Van toc goc 3 truc (rad/s)")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 4,
Category = "Du lieu",
DefaultValue = "0, 0, 0"
};
yield return new PropertyDescription("Orientation", "Orientation (rad)", "Huong Euler angles (rad)")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 5,
Category = "Du lieu",
DefaultValue = "0, 0, 0"
};
}
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
{
IMU.Connect();
await Task.CompletedTask;
}
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
{
// Dam bao port da mo (sau Disconnect can Connect lai)
if (!IMU.IsConnected)
IMU.Connect();
if (_printDataEnabled)
{
_printDataTimer?.Dispose();
_printDataTimer = new Timer(_ =>
{
try
{
var acc = CachedAcceleration.Accel.Linear;
var gyro = CachedAngularVelocity.Vector;
var ori = CachedOrientation.Vector;
var t = ((IInertialMeasurementUnit)this).LastUpdateTime;
Console.WriteLine(
$"{DateTime.Now:HH:mm:ss.ffffff} [IMU_DATA] " +
$"t={t:HH:mm:ss.fff} " +
$"acc=({acc.X:F3},{acc.Y:F3},{acc.Z:F3}) " +
$"gyro=({gyro.X:F5},{gyro.Y:F5},{gyro.Z:F5}) " +
$"rpy=({ori.X:F5},{ori.Y:F5},{ori.Z:F5})");
}
catch { }
}, null, dueTime: TimeSpan.Zero, period: _printDataInterval);
}
// Reset high-precision timer for accurate timestamps
_timerStartUtc = DateTime.UtcNow;
_highPrecisionTimer.Restart();
// Reset calibration de moi lan connect thu thap lai 2 giay dau
ResetCalibrationState();
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
// Dang ky event handler cho DataReceived tu HfiA9Reader
IMU.DataReceived += IMU_DataReceived;
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
// Doi xu ly _calibrationDone (toi da CalibrationWaitTimeout), de connect chi hoan tat sau khi da calibrate
try
{
await Task.WhenAny(
_calibrationCompletedTcs.Task,
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
}
catch (OperationCanceledException)
{
// cancellationToken bi huy
}
_calibrationCompletedTcs = null;
}
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
{
// Huy dang ky event handler
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
// Disconnect IMU de dung processing thread va serial port
IMU.Disconnect();
_printDataTimer?.Dispose();
_printDataTimer = null;
await Task.CompletedTask;
}
protected override async Task OnResetAsync(CancellationToken cancellationToken)
{
// Huy event va disconnect
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
IMU.Disconnect();
_printDataTimer?.Dispose();
_printDataTimer = null;
// Reset high-precision timer
_timerStartUtc = DateTime.UtcNow;
_highPrecisionTimer.Restart();
lock (_dataLock)
{
_cachedAcceleration = CreateAccelStamped(0, 0, 9.81, DateTime.UtcNow);
_cachedAngularVelocity = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
_cachedMagnetometer = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
_cachedOrientation = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
_cachedQuaternion = CreateQuaternionStamped(1, 0, 0, 0, DateTime.UtcNow);
_lastUpdateTime = DateTime.UtcNow;
}
// Reset calibration de sau khi connect lai thu thap 2 giay dau
ResetCalibrationState();
// Reset sample rate
_sampleCount = 0;
_sampleRate = 0.0;
_sampleRateStartTime = DateTime.UtcNow;
// Reconnect va bat dau calibration lai
IMU.Connect();
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
IMU.DataReceived += IMU_DataReceived;
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
// Doi calibration hoan tat
try
{
await Task.WhenAny(
_calibrationCompletedTcs.Task,
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
}
catch (OperationCanceledException)
{
// cancellationToken bi huy
}
_calibrationCompletedTcs = null;
UpdateProperties();
}
protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
{
// Port mo CHUA du de coi la connected: sau khi cam lai USB, port co the mo
// nhung khong co du lieu (ModemManager giu port) -> yeu cau co frame gan day
return Task.FromResult(IMU.IsConnected && IMU.HasRecentData);
}
/// <summary>
/// Reader tu reconnect ben trong; bao cho DeviceBase de state machine / UI
/// cap nhat dung trang thai (Connected/Disconnected/Reconnecting)
/// </summary>
private void IMU_ConnectionStateChanged(object? sender, bool connected)
{
_ = CheckConnectionAsync();
}
/// <summary>
/// Reset toan bo trang thai calibration ve gia tri ban dau
/// </summary>
private void ResetCalibrationState()
{
_calibrationStartUtc = null;
_calibrationDone = false;
_isCalibrated = false;
_calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
_calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
_calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
_calibrationCount = 0;
_accBiasX = _accBiasY = _accBiasZ = 0;
_gyroBiasX = _gyroBiasY = _gyroBiasZ = 0;
_orientationBiasRoll = _orientationBiasPitch = _orientationBiasYaw = 0;
_integratedYaw = 0;
_lastIntegrationTime = DateTime.MinValue;
}
/// <summary>
/// Event handler cho DataReceived tu HfiA9Reader.
/// 2 giay dau ke tu lan nhan du lieu dau tien: chi thu thap mau de tinh bias (robot dung yen tuyet doi).
/// Sau 2 giay: ap dung bias de calibrate, roi moi cap nhat _cached*, fire events, _sampleCount.
/// </summary>
private void IMU_DataReceived(object? sender, EventArgs e)
{
try
{
var snapshot = IMU.GetSnapshot();
// Use high-precision timer instead of DateTime.UtcNow (which has ~10-15ms precision)
var timestamp = _timerStartUtc + _highPrecisionTimer.Elapsed;
// Bat dau cua so calibration khi nhan du lieu lan dau
if (_calibrationStartUtc == null)
{
_calibrationStartUtc = timestamp;
_calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
_calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
_calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
_calibrationCount = 0;
}
if (!_calibrationDone)
{
var calElapsed = timestamp - _calibrationStartUtc.Value;
if (calElapsed < CalibrationDuration)
{
// Trong 2 giay dau: chi tich luy mau, khong cap nhat cache / fire events / sampleCount
_calibrationSumAccX += snapshot.AccX;
_calibrationSumAccY += snapshot.AccY;
_calibrationSumAccZ += snapshot.AccZ;
_calibrationSumGx += snapshot.Gx;
_calibrationSumGy += snapshot.Gy;
_calibrationSumGz += snapshot.Gz;
_calibrationSumRoll += snapshot.Roll;
_calibrationSumPitch += snapshot.Pitch;
_calibrationSumYaw += snapshot.Yaw;
_calibrationCount++;
return;
}
// Het 2 giay: tinh bias va danh dau da calibrate
if (_calibrationCount > 0)
{
double n = _calibrationCount;
double meanAccX = _calibrationSumAccX / n;
double meanAccY = _calibrationSumAccY / n;
double meanAccZ = _calibrationSumAccZ / n;
// Validate gravity magnitude — neu lech qua xa 9.81 thi robot bi rung/di chuyen
double gravityMagnitude = Math.Sqrt(meanAccX * meanAccX + meanAccY * meanAccY + meanAccZ * meanAccZ);
if (Math.Abs(gravityMagnitude - GravityMps2) > GravityToleranceMps2)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9IMU] Calibration REJECTED: " +
$"GravityMag={gravityMagnitude:F4} (expected ~{GravityMps2}, tolerance ±{GravityToleranceMps2}). " +
$"Robot co the dang rung/di chuyen. Thu lai...");
_calibrationStartUtc = null;
_calibrationCount = 0;
_calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
_calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
_calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
return;
}
// Khong tru gravity khoi acceleration data!
// ImuTracker trong Cartographer CAN gravity de estimate orientation.
// Khi dung yen (Z up): acc ~ (0, 0, +9.81) — day la luc phan ung tu mat dat
//
// Chi calibrate bias nho (sensor offset) cho X va Y.
// Voi Z: tinh bias = meanAccZ - expected_gravity
double expectedGravityZ = meanAccZ > 0 ? GravityMps2 : -GravityMps2;
// Bias X,Y: offset khi dung yen (nen ~ 0 neu robot dat phang)
_accBiasX = meanAccX;
_accBiasY = meanAccY;
// Bias Z: chi tru phan offset, GIU NGUYEN gravity
_accBiasZ = meanAccZ - expectedGravityZ;
// Gyro bias: dung — khi dung yen angular velocity = 0
_gyroBiasX = _calibrationSumGx / n;
_gyroBiasY = _calibrationSumGy / n;
_gyroBiasZ = _calibrationSumGz / n;
// Orientation bias: giu lai cho display purposes
_orientationBiasRoll = _calibrationSumRoll / n;
_orientationBiasPitch = _calibrationSumPitch / n;
_orientationBiasYaw = _calibrationSumYaw / n;
_isCalibrated = true;
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9IMU] Calibration done (n={_calibrationCount}): " +
$"MeanAcc=({meanAccX:F4},{meanAccY:F4},{meanAccZ:F4}), GravityMag={gravityMagnitude:F4}, " +
$"AccBias=({_accBiasX:F4},{_accBiasY:F4},{_accBiasZ:F4}), " +
$"GyroBias=({_gyroBiasX:F6},{_gyroBiasY:F6},{_gyroBiasZ:F6})");
}
_calibrationDone = true;
_calibrationCompletedTcs?.TrySetResult(true);
_calibrationCompletedTcs = null;
// Khoi tao yaw integration tu thoi diem calibration xong
_integratedYaw = 0;
_lastIntegrationTime = timestamp;
// Bat dau dem sample rate tu sau calibration
_sampleRateStartTime = timestamp;
_sampleCount = 0;
}
// Ap dung bias: du lieu da calibrate (sau 2 giay moi chay toi day)
double accX = snapshot.AccX - _accBiasX;
double accY = snapshot.AccY - _accBiasY;
double accZ = snapshot.AccZ - _accBiasZ;
double gx = snapshot.Gx - _gyroBiasX;
double gy = snapshot.Gy - _gyroBiasY;
double gz = snapshot.Gz - _gyroBiasZ;
double roll = snapshot.Roll - _orientationBiasRoll;
double pitch = snapshot.Pitch - _orientationBiasPitch;
// Tich phan CalibratedGz de tinh Yaw thay vi dung firmware AHRS Yaw
double yaw;
if (_lastIntegrationTime != DateTime.MinValue)
{
double dt = (timestamp - _lastIntegrationTime).TotalSeconds;
_integratedYaw += gz * dt;
yaw = _integratedYaw;
}
else
{
yaw = 0;
}
_lastIntegrationTime = timestamp;
// Diagnostic log moi 5 giay: theo doi drift
if (timestamp - _lastDiagnosticLog >= DiagnosticLogInterval)
{
_lastDiagnosticLog = timestamp;
var elapsedSec = (timestamp - _timerStartUtc).TotalSeconds;
double firmwareYaw = snapshot.Yaw - _orientationBiasYaw;
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [IMU_DIAG] t={elapsedSec:F1}s | " +
$"CalibratedGz={gz:F6} | " +
$"IntegratedYaw={yaw:F6} FirmwareYaw={firmwareYaw:F6}");
}
// Outlier validation: reject data with unrealistic values
// This prevents ImuTracker corruption from EMI spikes or communication errors
double accMagnitude = Math.Sqrt(accX * accX + accY * accY + accZ * accZ);
double gyroMagnitude = Math.Sqrt(gx * gx + gy * gy + gz * gz);
if (accMagnitude > MaxValidAccelerationMps2 || gyroMagnitude > MaxValidAngularVelocityRadS)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9IMU] OUTLIER REJECTED: " +
$"accMag={accMagnitude:F2} m/s² (max={MaxValidAccelerationMps2}), " +
$"gyroMag={gyroMagnitude:F2} rad/s (max={MaxValidAngularVelocityRadS})");
return;
}
// Quaternion tu goc Euler da calibrate
var cosRoll = Math.Cos(roll / 2);
var sinRoll = Math.Sin(roll / 2);
var cosPitch = Math.Cos(pitch / 2);
var sinPitch = Math.Sin(pitch / 2);
var cosYaw = Math.Cos(yaw / 2);
var sinYaw = Math.Sin(yaw / 2);
var qw = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
var qx = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
var qy = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
var qz = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
var newAcceleration = CreateAccelStamped(accX, accY, accZ, timestamp);
var newAngularVelocity = CreateVector3Stamped(gx, gy, gz, timestamp);
var newMagnetometer = CreateVector3Stamped(snapshot.MagX, snapshot.MagY, snapshot.MagZ, timestamp);
var newOrientation = CreateVector3Stamped(roll, pitch, yaw, timestamp);
var newQuaternion = CreateQuaternionStamped(qw, qx, qy, qz, timestamp);
AccelStamped previousAcceleration;
Vector3Stamped previousOrientation;
// Thread-safe update of cached data using lock
lock (_dataLock)
{
previousAcceleration = _cachedAcceleration;
previousOrientation = _cachedOrientation;
_cachedAcceleration = newAcceleration;
_cachedAngularVelocity = newAngularVelocity;
_cachedMagnetometer = newMagnetometer;
_cachedOrientation = newOrientation;
_cachedQuaternion = newQuaternion;
_lastUpdateTime = timestamp;
}
// Fire unified event (SensorPipeline)
ImuDataChanged?.Invoke(this, new ImuDataChangedEventArgs(
newAcceleration,
newAngularVelocity,
newMagnetometer,
newOrientation,
timestamp));
// Fire individual events (interface — XlocIntegrationService, etc.)
if (Math.Abs(previousAcceleration.Accel.Linear.X - newAcceleration.Accel.Linear.X) > 0.1 ||
Math.Abs(previousAcceleration.Accel.Linear.Y - newAcceleration.Accel.Linear.Y) > 0.1 ||
Math.Abs(previousAcceleration.Accel.Linear.Z - newAcceleration.Accel.Linear.Z) > 0.1)
{
AccelerationChanged?.Invoke(this, new AccelerationChangedEventArgs(newAcceleration));
}
// Xloc requires a continuous IMU stream even when the robot is stationary.
// Emit angular velocity updates every sample instead of threshold-based changes.
AngularVelocityChanged?.Invoke(this, new AngularVelocityChangedEventArgs(newAngularVelocity));
MagnetometerChanged?.Invoke(this, new MagnetometerChangedEventArgs(newMagnetometer));
if (Math.Abs(previousOrientation.Vector.X - newOrientation.Vector.X) > 0.01 ||
Math.Abs(previousOrientation.Vector.Y - newOrientation.Vector.Y) > 0.01 ||
Math.Abs(previousOrientation.Vector.Z - newOrientation.Vector.Z) > 0.01)
{
OrientationChanged?.Invoke(this, new OrientationChangedEventArgs(newOrientation));
}
_sampleCount++;
var elapsed = timestamp - _sampleRateStartTime;
if (elapsed >= _sampleRateWindow)
{
_ = Task.Run(() =>
{
_sampleRate = _sampleCount / elapsed.TotalSeconds;
_sampleCount = 0;
_sampleRateStartTime = timestamp;
UpdateProperties();
});
}
}
catch (Exception ex)
{
LastError = ex;
OnErrorOccurred(ex, "Error updating data from IMU");
}
}
/// <summary>
/// Cap nhat properties hien thi
/// </summary>
private void UpdateProperties()
{
var accel = _cachedAcceleration;
var angularVel = _cachedAngularVelocity;
var orientation = _cachedOrientation;
var sampleRate = _sampleRate;
var isCalibrated = _isCalibrated;
SetProperty("IsCalibrated", isCalibrated.ToString());
SetProperty("SampleRate", sampleRate.ToString("F1"));
SetProperty("Acceleration", $"{accel.Accel.Linear.X:F2}, {accel.Accel.Linear.Y:F2}, {accel.Accel.Linear.Z:F2}");
SetProperty("AngularVelocity", $"{angularVel.Vector.X:F3}, {angularVel.Vector.Y:F3}, {angularVel.Vector.Z:F3}");
SetProperty("Orientation", $"{orientation.Vector.X:F3}, {orientation.Vector.Y:F3}, {orientation.Vector.Z:F3}");
}
private static AccelStamped CreateAccelStamped(double x, double y, double z, DateTime timestamp)
{
return new AccelStamped
{
Header = new Header
{
Stamp = timestamp,
FrameId = "imu_frame"
},
Accel = new Accel
{
Linear = new Vector3(x, y, z),
Angular = new Vector3(0, 0, 0)
}
};
}
private static Vector3Stamped CreateVector3Stamped(double x, double y, double z, DateTime timestamp)
{
return new Vector3Stamped
{
Header = new Header
{
Stamp = timestamp,
FrameId = "imu_frame"
},
Vector = new Vector3(x, y, z)
};
}
private static QuaternionStamped CreateQuaternionStamped(double w, double x, double y, double z, DateTime timestamp)
{
return new QuaternionStamped
{
Header = new Header
{
Stamp = timestamp,
FrameId = "imu_frame"
},
Quaternion = new Quaternion(x, y, z, w)
};
}
#region IInertialMeasurementUnit Implementation
bool IInertialMeasurementUnit.IsConnected => base.IsConnected;
public bool IsCalibrated
{
get { return _isCalibrated; }
}
public double SampleRate
{
get
{
Thread.MemoryBarrier();
return _sampleRate;
}
}
public AccelStamped CachedAcceleration
{
get { lock (_dataLock) { return _cachedAcceleration; } }
}
public Vector3Stamped CachedAngularVelocity
{
get { lock (_dataLock) { return _cachedAngularVelocity; } }
}
public Vector3Stamped? CachedMagnetometer
{
get { lock (_dataLock) { return _cachedMagnetometer; } }
}
public Vector3Stamped CachedOrientation
{
get { lock (_dataLock) { return _cachedOrientation; } }
}
public QuaternionStamped? CachedQuaternion
{
get { lock (_dataLock) { return _cachedQuaternion; } }
}
public double? CachedTemperature
{
// HFI-A9 khong co cam bien nhiet do
get { return null; }
}
DateTime IInertialMeasurementUnit.LastUpdateTime
{
get { lock (_dataLock) { return _lastUpdateTime; } }
}
public async Task<AccelStamped> ReadAccelerationAsync(CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
return CachedAcceleration;
}
public async Task<Vector3Stamped> ReadAngularVelocityAsync(CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
return CachedAngularVelocity;
}
public async Task<Vector3Stamped?> ReadMagnetometerAsync(CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
return CachedMagnetometer;
}
public async Task<Vector3Stamped> ReadOrientationAsync(CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
return CachedOrientation;
}
public async Task<QuaternionStamped?> ReadQuaternionAsync(CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
return CachedQuaternion;
}
public async Task<Imu> ReadAllDataAsync(CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
lock (_dataLock)
{
return CreateImuFromCachedData();
}
}
private Imu CreateImuFromCachedData()
{
var timestamp = _lastUpdateTime != default ? _lastUpdateTime : DateTime.UtcNow;
var orientation = _cachedQuaternion.Quaternion;
var orientationCovariance = new double[Imu.OrientationCovarianceSize];
var angularVelocityCovariance = new double[Imu.AngularVelocityCovarianceSize];
var linearAccelerationCovariance = new double[Imu.LinearAccelerationCovarianceSize];
double gyroVariance = 1e-4;
angularVelocityCovariance[0] = gyroVariance;
angularVelocityCovariance[4] = gyroVariance;
angularVelocityCovariance[8] = gyroVariance;
double accelVariance = 1e-3;
linearAccelerationCovariance[0] = accelVariance;
linearAccelerationCovariance[4] = accelVariance;
linearAccelerationCovariance[8] = accelVariance;
orientationCovariance[0] = 0.001;
orientationCovariance[4] = 0.001;
orientationCovariance[8] = 0.002;
return new Imu(
header: new Header
{
Stamp = timestamp,
FrameId = "imu_frame"
},
orientation: orientation,
orientationCovariance: orientationCovariance,
angularVelocity: _cachedAngularVelocity.Vector,
angularVelocityCovariance: angularVelocityCovariance,
linearAcceleration: _cachedAcceleration.Accel.Linear,
linearAccelerationCovariance: linearAccelerationCovariance
);
}
public async Task<double?> ReadTemperatureAsync(CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
return CachedTemperature;
}
public async Task CalibrateAsync(CancellationToken cancellationToken = default)
{
// Huy event, reset calibration, dang ky lai event va doi calib
IMU.DataReceived -= IMU_DataReceived;
ResetCalibrationState();
_timerStartUtc = DateTime.UtcNow;
_highPrecisionTimer.Restart();
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
IMU.DataReceived += IMU_DataReceived;
try
{
await Task.WhenAny(
_calibrationCompletedTcs.Task,
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
}
catch (OperationCanceledException)
{
// cancellationToken bi huy
}
_calibrationCompletedTcs = null;
UpdateProperties();
}
public async Task CalibrateMagnetometerAsync(CancellationToken cancellationToken = default)
{
await Task.Delay(2000, cancellationToken);
}
public async Task ResetCalibrationAsync(CancellationToken cancellationToken = default)
{
// Huy event, reset state, dang ky lai va doi calib moi
IMU.DataReceived -= IMU_DataReceived;
ResetCalibrationState();
_timerStartUtc = DateTime.UtcNow;
_highPrecisionTimer.Restart();
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
IMU.DataReceived += IMU_DataReceived;
try
{
await Task.WhenAny(
_calibrationCompletedTcs.Task,
Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
}
catch (OperationCanceledException)
{
// cancellationToken bi huy
}
_calibrationCompletedTcs = null;
UpdateProperties();
}
public async Task SetSampleRateAsync(double sampleRate, CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
_sampleRate = Math.Max(1, Math.Min(1000, sampleRate));
UpdateProperties();
}
public async Task SetAccelerometerRangeAsync(double range, CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
}
public async Task SetGyroscopeRangeAsync(double range, CancellationToken cancellationToken = default)
{
await Task.CompletedTask;
}
#endregion
protected override void Dispose(bool disposing)
{
if (disposing)
{
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
IMU.Disconnect();
IMU.Dispose();
_printDataTimer?.Dispose();
_printDataTimer = null;
}
base.Dispose(disposing);
}
}
}

503
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9Reader.cs Normal file
View File

@@ -0,0 +1,503 @@
using System;
using System.IO;
using System.IO.Ports;
using System.Threading;
namespace RobotNet10.RobotApp.Drivers.HfiA9IMU
{
/// <summary>
/// Doc du lieu tu IMU HandsFree HFI-A9 qua serial (CP210x USB-UART, 921600 baud).
/// Giao thuc: frame = 0xAA 0x55 [LEN] [payload LEN bytes] [CRC16-Modbus lo, hi]
/// CRC tinh tren [LEN] + payload.
/// LEN 0x2C: 10 float LE tai payload offset 4:
/// [0]=timestamp, [1:4]=gyro (rad/s), [4:7]=accel (g), [7:10]=mag
/// LEN 0x14: 4 float LE tai payload offset 4: [1:4]=roll/pitch/yaw (do)
/// </summary>
public class HfiA9Reader : IDisposable
{
// Frame lon nhat: 0x2C + 5 = 49 bytes, dung 256 bytes de dam bao an toan
private const int FRAME_BUFFER_SIZE = 256;
private readonly byte[] _frameBuffer = new byte[FRAME_BUFFER_SIZE];
private int _frameBufferLength = 0;
// Non-volatile field de dung voi Volatile.Write lam memory barrier
private int _memoryBarrier = 0;
// Thread doc du lieu tu SerialPort voi priority cao
private Thread? _readingThread;
private volatile bool _shouldRead = false;
private CancellationTokenSource? _readingCts;
// Event de thong bao khi co du lieu moi duoc decode (fire sau frame Euler
// de moi event tuong ung mot chu ky mau day du: gyro/accel/mag + euler)
public event EventHandler? DataReceived;
// Event bao trang thai ket noi thay doi (true = vua mo port, false = mat ket noi)
// de device class bao cho DeviceBase cap nhat state machine / UI
public event EventHandler<bool>? ConnectionStateChanged;
// Properties - lock-free voi memory barriers (volatile khong ho tro double)
public double Roll { get; private set; }
public double Pitch { get; private set; }
public double Yaw { get; private set; }
public double Gx { get; private set; }
public double Gy { get; private set; }
public double Gz { get; private set; }
public double AccX { get; private set; }
public double AccY { get; private set; }
public double AccZ { get; private set; }
public double MagX { get; private set; }
public double MagY { get; private set; }
public double MagZ { get; private set; }
const byte FRAME_HEAD_1 = 0xAA;
const byte FRAME_HEAD_2 = 0x55;
// LEN byte cua tung loai goi
const byte LEN_IMU = 0x2C; // 44: timestamp + gyro + accel + mag (10 float)
const byte LEN_EULER = 0x14; // 20: timestamp + roll/pitch/yaw (4 float)
// HFI-A9 xuat accel theo don vi g; nhan -9.8 theo quy uoc truc cua driver hang
// (dung yen, Z huong len -> AccZ ~ +9.8 m/s², cung quy uoc voi Wheeltec N100)
private const double ACCEL_SCALE = -9.8;
private const double DEG_TO_RAD = Math.PI / 180.0;
private SerialPort? serial;
// Luu thong so port de tao lai SerialPort khi reconnect
private readonly string _portName;
private readonly int _baudRate;
private readonly int _timeOut;
// Thoi gian backoff giua cac lan thu reconnect (ms)
private const int RECONNECT_BACKOFF_MS = 1000;
// Watchdog: neu qua khoang thoi gian nay khong co frame hop le -> coi nhu mat ket noi
// va trigger reconnect. Dung cho truong hop USB "chet mem" khong ne'm exception.
private const long DATA_TIMEOUT_TICKS = 3 * TimeSpan.TicksPerSecond;
private long _lastFrameTicks;
// Kiem tra dinh ky file port con ton tai khong (rut USB -> /dev/ttyUSBx bien mat)
// de phat hien disconnect nhanh hon watchdog
private const long PORT_CHECK_INTERVAL_TICKS = TimeSpan.TicksPerSecond / 2;
private long _lastPortCheckTicks;
public bool IsConnected => serial != null && serial.IsOpen;
/// <summary>
/// Co frame hop le trong DATA_TIMEOUT gan nhat khong — dung de phan biet
/// "port mo nhung khong co du lieu" (vd ModemManager dang giu port sau khi cam lai)
/// </summary>
public bool HasRecentData => DateTime.UtcNow.Ticks - Volatile.Read(ref _lastFrameTicks) < DATA_TIMEOUT_TICKS;
public HfiA9Reader(string portName, int baudRate, int timeOut)
{
_portName = portName;
_baudRate = baudRate;
_timeOut = timeOut;
serial = CreateSerialPort();
}
private SerialPort CreateSerialPort()
{
return new SerialPort()
{
PortName = _portName,
BaudRate = _baudRate,
ReadTimeout = _timeOut,
Parity = Parity.None,
StopBits = StopBits.One,
DataBits = 8,
};
}
/// <summary>
/// Snapshot structure de lay tat ca du lieu cung luc mot cach thread-safe
/// </summary>
public struct DataSnapshot
{
public double AccX, AccY, AccZ;
public double Gx, Gy, Gz;
public double MagX, MagY, MagZ;
public double Roll, Pitch, Yaw;
}
/// <summary>
/// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe
/// </summary>
public DataSnapshot GetSnapshot()
{
Volatile.Read(ref _memoryBarrier);
return new DataSnapshot
{
AccX = AccX,
AccY = AccY,
AccZ = AccZ,
Gx = Gx,
Gy = Gy,
Gz = Gz,
MagX = MagX,
MagY = MagY,
MagZ = MagZ,
Roll = Roll,
Pitch = Pitch,
Yaw = Yaw,
};
}
public void Connect()
{
_frameBufferLength = 0;
Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
// Reading thread se tu mo port trong outer loop va tu reconnect khi mat ket noi
StartReadingThread();
}
private void StartReadingThread()
{
if (_readingThread != null && _readingThread.IsAlive)
return;
_shouldRead = true;
_readingCts?.Dispose();
_readingCts = new CancellationTokenSource();
_readingThread = new Thread(() => ReadingThreadLoop(_readingCts.Token))
{
Name = "HfiA9IMU-Reading",
IsBackground = false,
Priority = ThreadPriority.Highest
};
_readingThread.Start();
}
private void StopReadingThread()
{
_shouldRead = false;
_readingCts?.Cancel();
if (_readingThread != null)
{
if (!_readingThread.Join(1000))
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Reading thread did not stop gracefully");
}
_readingThread = null;
}
_readingCts?.Dispose();
_readingCts = null;
}
/// <summary>
/// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu
/// </summary>
private void ReadingThreadLoop(CancellationToken cancellationToken)
{
Thread.BeginThreadAffinity();
byte[] readBuffer = new byte[256];
while (_shouldRead && !cancellationToken.IsCancellationRequested)
{
try
{
if (serial == null || !serial.IsOpen)
{
SafeCloseSerial();
serial = CreateSerialPort();
serial.Open();
_frameBufferLength = 0;
serial.DiscardInBuffer();
_lastFrameTicks = DateTime.UtcNow.Ticks;
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Connected to {_portName}");
RaiseConnectionStateChanged(true);
}
InnerReadLoop(readBuffer, cancellationToken);
}
catch (OperationCanceledException)
{
break;
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] IMU disconnected: {ex.Message}");
SafeCloseSerial();
RaiseConnectionStateChanged(false);
}
if (_shouldRead && !cancellationToken.IsCancellationRequested)
{
cancellationToken.WaitHandle.WaitOne(RECONNECT_BACKOFF_MS);
}
}
SafeCloseSerial();
Thread.EndThreadAffinity();
}
private void InnerReadLoop(byte[] readBuffer, CancellationToken cancellationToken)
{
while (_shouldRead && !cancellationToken.IsCancellationRequested
&& serial != null && serial.IsOpen)
{
// Watchdog phai kiem tra MOI vong lap: khi rut USB tren Linux,
// BytesToRead co the van > 0 nhung Read() tra ve 0 (EOF) lien tuc,
// neu chi kiem tra trong nhanh bytesToRead <= 0 se khong bao gio phat hien
long nowTicks = DateTime.UtcNow.Ticks;
if (nowTicks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
{
throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
}
// Rut USB -> device file bien mat: phat hien nhanh hon watchdog
if (nowTicks - _lastPortCheckTicks > PORT_CHECK_INTERVAL_TICKS)
{
_lastPortCheckTicks = nowTicks;
if (!File.Exists(_portName))
{
throw new IOException($"Serial port {_portName} no longer exists (USB unplugged?)");
}
}
int bytesToRead = serial.BytesToRead;
if (bytesToRead <= 0)
{
Thread.Sleep(1);
continue;
}
int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
if (bytesRead <= 0)
{
Thread.Sleep(1);
continue;
}
ProcessIncomingData(readBuffer, bytesRead);
}
}
private void SafeCloseSerial()
{
if (serial == null) return;
try
{
serial.Close();
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error closing serial: {ex.Message}");
}
try { serial.Dispose(); }
catch { }
serial = null;
}
/// <summary>
/// Them du lieu moi vao frame buffer va parse cac frame hoan chinh
/// </summary>
private void ProcessIncomingData(byte[] data, int length)
{
int dataOffset = 0;
while (dataOffset < length)
{
ProcessCompleteFramesInBuffer();
int availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;
if (availableSpace == 0)
{
// Buffer day ma khong co frame hop le -> du lieu rac, xoa het
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Buffer full without valid frame, clearing buffer");
_frameBufferLength = 0;
availableSpace = FRAME_BUFFER_SIZE;
}
int bytesToAdd = Math.Min(length - dataOffset, availableSpace);
Array.Copy(data, dataOffset, _frameBuffer, _frameBufferLength, bytesToAdd);
_frameBufferLength += bytesToAdd;
dataOffset += bytesToAdd;
ProcessCompleteFramesInBuffer();
}
}
/// <summary>
/// Xu ly tat ca cac frame hoan chinh trong buffer hien tai
/// </summary>
private void ProcessCompleteFramesInBuffer()
{
while (_frameBufferLength > 0)
{
ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
int headIndex = bufferSpan.IndexOf(FRAME_HEAD_1);
if (headIndex < 0)
{
_frameBufferLength = 0;
break;
}
if (headIndex > 0)
{
ShiftBuffer(headIndex);
continue;
}
// Can toi thieu 3 bytes: AA 55 LEN
if (_frameBufferLength < 3)
break;
if (_frameBuffer[1] != FRAME_HEAD_2)
{
ShiftBuffer(1);
continue;
}
byte payloadLength = _frameBuffer[2];
if (payloadLength != LEN_IMU && payloadLength != LEN_EULER)
{
ShiftBuffer(1);
continue;
}
int totalFrameLength = payloadLength + 5;
if (_frameBufferLength < totalFrameLength)
break;
// CRC16-Modbus tren [LEN + payload], luu little-endian sau payload
ushort crcCalc = Crc16Modbus(_frameBuffer.AsSpan(2, 1 + payloadLength));
ushort crcRecv = (ushort)(_frameBuffer[3 + payloadLength] | (_frameBuffer[4 + payloadLength] << 8));
if (crcCalc != crcRecv)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] CRC16 error: recv={crcRecv:X4}, calc={crcCalc:X4}");
ShiftBuffer(2);
continue;
}
try
{
DecodeFrame(payloadLength, _frameBuffer.AsSpan(3, payloadLength));
_lastFrameTicks = DateTime.UtcNow.Ticks;
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error parsing frame: {ex.Message}");
}
ShiftBuffer(totalFrameLength);
}
}
private void ShiftBuffer(int count)
{
int remaining = _frameBufferLength - count;
if (remaining > 0)
{
Array.Copy(_frameBuffer, count, _frameBuffer, 0, remaining);
}
_frameBufferLength = Math.Max(0, remaining);
}
private void DecodeFrame(byte payloadLength, ReadOnlySpan<byte> payload)
{
if (payloadLength == LEN_IMU)
{
// float [0] la timestamp noi bo, bo qua
Gx = BitConverter.ToSingle(payload.Slice(8, 4));
Gy = BitConverter.ToSingle(payload.Slice(12, 4));
Gz = BitConverter.ToSingle(payload.Slice(16, 4));
AccX = BitConverter.ToSingle(payload.Slice(20, 4)) * ACCEL_SCALE;
AccY = BitConverter.ToSingle(payload.Slice(24, 4)) * ACCEL_SCALE;
AccZ = BitConverter.ToSingle(payload.Slice(28, 4)) * ACCEL_SCALE;
MagX = BitConverter.ToSingle(payload.Slice(32, 4));
MagY = BitConverter.ToSingle(payload.Slice(36, 4));
MagZ = BitConverter.ToSingle(payload.Slice(40, 4));
Volatile.Write(ref _memoryBarrier, 0);
}
else if (payloadLength == LEN_EULER)
{
Roll = BitConverter.ToSingle(payload.Slice(8, 4)) * DEG_TO_RAD;
Pitch = BitConverter.ToSingle(payload.Slice(12, 4)) * DEG_TO_RAD;
Yaw = BitConverter.ToSingle(payload.Slice(16, 4)) * DEG_TO_RAD;
Volatile.Write(ref _memoryBarrier, 0);
// Frame Euler den sau frame IMU trong moi chu ky -> fire event mot lan/chu ky
try
{
DataReceived?.Invoke(this, EventArgs.Empty);
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] DataReceived subscriber error: {ex.Message}");
}
}
}
private void RaiseConnectionStateChanged(bool connected)
{
try
{
ConnectionStateChanged?.Invoke(this, connected);
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] ConnectionStateChanged subscriber error: {ex.Message}");
}
}
private static ushort Crc16Modbus(ReadOnlySpan<byte> data)
{
ushort crc = 0xFFFF;
foreach (byte b in data)
{
crc ^= b;
for (int i = 0; i < 8; i++)
{
if ((crc & 1) != 0)
crc = (ushort)((crc >> 1) ^ 0xA001);
else
crc >>= 1;
}
}
return crc;
}
public void Disconnect()
{
StopReadingThread();
SafeCloseSerial();
_frameBufferLength = 0;
Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
}
public void Dispose()
{
Dispose(true);
GC.SuppressFinalize(this);
}
protected virtual void Dispose(bool disposing)
{
if (disposing)
{
Disconnect();
}
}
}
}

867
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Hinson/HinsonFE35LidarDriver.cs Normal file
View File

@@ -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
}

22
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecN100IMU.cs
View File

@@ -224,6 +224,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
// Dang ky event handler cho DataReceived tu WheeltecReader
IMU.DataReceived += IMU_DataReceived;
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
// Doi xu ly _calibrationDone (toi da CalibrationWaitTimeout), de connect chi hoan tat sau khi da calibrate
try
@@ -243,6 +244,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
// Huy dang ky event handler
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
// Disconnect IMU de dung processing thread va serial port
IMU.Disconnect();
@@ -257,6 +259,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
// Huy event va disconnect
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
IMU.Disconnect();
_printDataTimer?.Dispose();
@@ -289,6 +292,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
IMU.Connect();
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
IMU.DataReceived += IMU_DataReceived;
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
// Doi calibration hoan tat
try
@@ -307,7 +311,18 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
{
return Task.FromResult(IMU.IsConnected);
// Port mo CHUA du de coi la connected: sau khi cam lai USB, port co the mo
// nhung khong co du lieu -> yeu cau co frame gan day
return Task.FromResult(IMU.IsConnected && IMU.HasRecentData);
}
/// <summary>
/// Reader tu reconnect ben trong; bao cho DeviceBase de state machine / UI
/// cap nhat dung trang thai (Connected/Disconnected/Reconnecting)
/// </summary>
private void IMU_ConnectionStateChanged(object? sender, bool connected)
{
_ = CheckConnectionAsync();
}
/// <summary>
@@ -775,6 +790,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
// Huy event, reset calibration, dang ky lai event va doi calib
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
ResetCalibrationState();
_timerStartUtc = DateTime.UtcNow;
@@ -782,6 +798,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
IMU.DataReceived += IMU_DataReceived;
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
try
{
@@ -806,6 +823,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
// Huy event, reset state, dang ky lai va doi calib moi
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
ResetCalibrationState();
_timerStartUtc = DateTime.UtcNow;
@@ -813,6 +831,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
_calibrationCompletedTcs = new TaskCompletionSource<bool>();
IMU.DataReceived += IMU_DataReceived;
IMU.ConnectionStateChanged += IMU_ConnectionStateChanged;
try
{
@@ -852,6 +871,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
if (disposing)
{
IMU.DataReceived -= IMU_DataReceived;
IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged;
IMU.Disconnect();
IMU.Dispose();
_printDataTimer?.Dispose();

59
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs
View File

@@ -1,5 +1,6 @@
using System;
using System.Collections.Generic;
using System.IO;
using System.IO.Ports;
using System.Threading;
@@ -24,6 +25,10 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
// Event de thong bao khi co du lieu moi duoc decode
public event EventHandler? DataReceived;
// Event bao trang thai ket noi thay doi (true = vua mo port, false = mat ket noi)
// de device class bao cho DeviceBase cap nhat state machine / UI
public event EventHandler<bool>? ConnectionStateChanged;
// Properties - lock-free voi memory barriers (volatile khong ho tro double)
public double Roll { get; private set; }
public double Pitch { get; private set; }
@@ -85,8 +90,19 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
private const long DATA_TIMEOUT_TICKS = 3 * TimeSpan.TicksPerSecond;
private long _lastFrameTicks;
// Kiem tra dinh ky file port con ton tai khong (rut USB -> /dev/ttyUSBx bien mat)
// de phat hien disconnect nhanh hon watchdog
private const long PORT_CHECK_INTERVAL_TICKS = TimeSpan.TicksPerSecond / 2;
private long _lastPortCheckTicks;
public bool IsConnected => serial != null && serial.IsOpen;
/// <summary>
/// Co frame hop le trong DATA_TIMEOUT gan nhat khong — dung de phan biet
/// "port mo nhung khong co du lieu" (vd ModemManager dang giu port sau khi cam lai)
/// </summary>
public bool HasRecentData => DateTime.UtcNow.Ticks - Volatile.Read(ref _lastFrameTicks) < DATA_TIMEOUT_TICKS;
public WheeltecReader(string portName, int baudRate, int timeOut)
{
_portName = portName;
@@ -226,6 +242,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
serial.DiscardInBuffer();
_lastFrameTicks = DateTime.UtcNow.Ticks;
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Connected to {_portName}");
RaiseConnectionStateChanged(true);
}
InnerReadLoop(readBuffer, cancellationToken);
@@ -238,6 +255,7 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] IMU disconnected: {ex.Message}");
SafeCloseSerial();
RaiseConnectionStateChanged(false);
}
if (_shouldRead && !cancellationToken.IsCancellationRequested)
@@ -259,19 +277,38 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
while (_shouldRead && !cancellationToken.IsCancellationRequested
&& serial != null && serial.IsOpen)
{
int bytesToRead = serial.BytesToRead;
if (bytesToRead <= 0)
{
if (DateTime.UtcNow.Ticks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
// Watchdog phai kiem tra MOI vong lap: khi rut USB tren Linux,
// BytesToRead co the van > 0 nhung Read() tra ve 0 (EOF) lien tuc,
// neu chi kiem tra trong nhanh bytesToRead <= 0 se khong bao gio phat hien
long nowTicks = DateTime.UtcNow.Ticks;
if (nowTicks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
{
throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
}
// Rut USB -> device file bien mat: phat hien nhanh hon watchdog
if (nowTicks - _lastPortCheckTicks > PORT_CHECK_INTERVAL_TICKS)
{
_lastPortCheckTicks = nowTicks;
if (!File.Exists(_portName))
{
throw new IOException($"Serial port {_portName} no longer exists (USB unplugged?)");
}
}
int bytesToRead = serial.BytesToRead;
if (bytesToRead <= 0)
{
Thread.Sleep(1);
continue;
}
int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
if (bytesRead <= 0) continue;
if (bytesRead <= 0)
{
Thread.Sleep(1);
continue;
}
ProcessIncomingData(readBuffer, bytesRead);
}
@@ -482,6 +519,18 @@ namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
_frameBufferLength = remainingAfterHead;
}
private void RaiseConnectionStateChanged(bool connected)
{
try
{
ConnectionStateChanged?.Invoke(this, connected);
}
catch (Exception ex)
{
Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] ConnectionStateChanged subscriber error: {ex.Message}");
}
}
public void Disconnect()
{
StopReadingThread();

31
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json
View File

@@ -461,6 +461,21 @@
}
}
},
"HinsonLidar":
{
"Enabled": true,
"DeviceId": "lidar-hinson-front",
"DeviceName": "Hinson FE-35FB-01000",
"DeviceType": "Lidar",
"DriverName": "HinsonFE35LidarDriver",
"DriverVersion": "1.0.0",
"Connection": {
"IpAddress": "192.168.100.88",
"Port": 8080,
"FrameId": "hinson-front",
"MaxRangeM": 35.0
}
},
"SickLidar01": {
"DeviceId": "scan_1",
"Enabled": false,
@@ -680,6 +695,22 @@
"PrintDataIntervalMs": 100
}
},
"HfiA9IMU": {
"DeviceId": "imu",
"Enabled": true,
"DeviceName": "HandsFree HFI-A9 IMU",
"DeviceType": "IMU",
"DriverName": "HfiA9IMU",
"DriverVersion": "1.0.0",
"Description": "IMU",
"Connection": {
"Port": "/dev/serial/by-id/usb-Silicon_Labs_HandsFree_IMU_USB_to_UART_Bridge_Controller_0001-if00-port0",
"BaudRate": 921600,
"Timeout": 2000,
"DebugEnabled": true,
"PrintDataIntervalMs": 100
}
},
"HikVisionQr": {
"DeviceId": "hik-qr-001",
"Enabled": false,