From 921bb91084bea5e308faa1d219837ebdc9abd6b8 Mon Sep 17 00:00:00 2001 From: QUYVN Date: Tue, 14 Jul 2026 15:15:34 +0700 Subject: [PATCH] update imu, lidar --- .../Drivers/HfiA9IMU/HfiA9IMU.cs | 859 ++++++++ .../Drivers/HfiA9IMU/HfiA9Reader.cs | 503 +++++ .../Drivers/Hinson/HinsonFE35LidarDriver.cs | 867 ++++++++ .../Drivers/WheeltecIMU/WheeltecN100IMU.cs | 1746 +++++++++-------- .../Drivers/WheeltecIMU/WheeltecReader.cs | 59 +- .../RobotNet10.RobotApp/appsettings.json | 31 + 6 files changed, 3197 insertions(+), 868 deletions(-) create mode 100644 srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9IMU.cs create mode 100644 srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9Reader.cs create mode 100644 srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Hinson/HinsonFE35LidarDriver.cs diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9IMU.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9IMU.cs new file mode 100644 index 0000000..2b3beb3 --- /dev/null +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9IMU.cs @@ -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? _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? AccelerationChanged; + public event EventHandler? AngularVelocityChanged; + public event EventHandler? MagnetometerChanged; + public event EventHandler? OrientationChanged; + + // Event thong nhat cho SensorPipeline + public event EventHandler? ImuDataChanged; + + public HfiA9IMU(string deviceId, string deviceName, IConfigurationSection connection) + : base(deviceId, deviceName, DeviceType.Imu) + { + _portName = connection.GetValue("Port") ?? throw new Exception("Port is required"); + _baudRate = connection.GetValue("BaudRate") ?? throw new Exception("BaudRate is required"); + _timeOut = connection.GetValue("TimeOut") ?? throw new Exception("Timeout is required"); + + IMU = new HfiA9Reader(_portName, _baudRate, _timeOut); + + // Continuous IMU data printing (optional) + _printDataEnabled = connection.GetValue("DebugEnabled") ?? false; + _printDataInterval = TimeSpan.FromMilliseconds(connection.GetValue("PrintDataIntervalMs") ?? 200); + + // Doc cau hinh neu co + var autoReconnectEnabled = connection.GetValue("AutoReconnectEnabled"); + var reconnectDelayMs = connection.GetValue("ReconnectDelayMs"); + var maxReconnectAttempts = connection.GetValue("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 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(); + + // 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(); + 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 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); + } + + /// + /// Reader tu reconnect ben trong; bao cho DeviceBase de state machine / UI + /// cap nhat dung trang thai (Connected/Disconnected/Reconnecting) + /// + private void IMU_ConnectionStateChanged(object? sender, bool connected) + { + _ = CheckConnectionAsync(); + } + + /// + /// Reset toan bo trang thai calibration ve gia tri ban dau + /// + 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; + } + + /// + /// 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. + /// + 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"); + } + } + + /// + /// Cap nhat properties hien thi + /// + 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 ReadAccelerationAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedAcceleration; + } + + public async Task ReadAngularVelocityAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedAngularVelocity; + } + + public async Task ReadMagnetometerAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedMagnetometer; + } + + public async Task ReadOrientationAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedOrientation; + } + + public async Task ReadQuaternionAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedQuaternion; + } + + public async Task 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 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(); + 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(); + 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); + } + } +} diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9Reader.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9Reader.cs new file mode 100644 index 0000000..8d20e3c --- /dev/null +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/HfiA9IMU/HfiA9Reader.cs @@ -0,0 +1,503 @@ +using System; +using System.IO; +using System.IO.Ports; +using System.Threading; + +namespace RobotNet10.RobotApp.Drivers.HfiA9IMU +{ + /// + /// 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) + /// + 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? 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; + + /// + /// 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) + /// + 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, + }; + } + + /// + /// Snapshot structure de lay tat ca du lieu cung luc mot cach thread-safe + /// + public struct DataSnapshot + { + public double AccX, AccY, AccZ; + public double Gx, Gy, Gz; + public double MagX, MagY, MagZ; + public double Roll, Pitch, Yaw; + } + + /// + /// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe + /// + 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; + } + + /// + /// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu + /// + 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; + } + + /// + /// Them du lieu moi vao frame buffer va parse cac frame hoan chinh + /// + 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(); + } + } + + /// + /// Xu ly tat ca cac frame hoan chinh trong buffer hien tai + /// + private void ProcessCompleteFramesInBuffer() + { + while (_frameBufferLength > 0) + { + ReadOnlySpan 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 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 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(); + } + } + } +} diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Hinson/HinsonFE35LidarDriver.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Hinson/HinsonFE35LidarDriver.cs new file mode 100644 index 0000000..0aa4d8f --- /dev/null +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Hinson/HinsonFE35LidarDriver.cs @@ -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; + +/// +/// Configuration cho Hinson FE-35 LiDAR Driver +/// +public class HinsonFE35LidarDriverConfig +{ + /// + /// Địa chỉ IP của LiDAR - mặc định: 192.168.1.88 (theo tài liệu Hinson FE) + /// + public string IpAddress { get; set; } = "192.168.1.88"; + + /// + /// Port của LiDAR - mặc định: 8080 + /// + public int Port { get; set; } = 8080; + + /// + /// Sử dụng UDP thay vì TCP - mặc định: false (TCP) + /// + public bool UseUdp { get; set; } = false; + + /// + /// Frame ID cho scan data (ROS-style message headers) + /// + public string FrameId { get; set; } = "laser"; + + /// + /// Tầm quét tối thiểu (mét) - mặc định: 0.05 m + /// + public double MinRangeM { get; set; } = 0.05; + + /// + /// Tầm quét tối đa (mét) - mặc định: 35.0 m (FE-35FB) + /// + public double MaxRangeM { get; set; } = 35.0; + + /// + /// 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ồ. + /// + public double AngleOffsetDeg { get; set; } = 0.0; + + /// + /// LiDAR gắn ngược (upside down) - đảo góc quét (180° - angle) như Olei driver + /// + public bool Inverted { get; set; } = false; + + /// + /// Timeout không nhận được dữ liệu thì coi như mất kết nối (milliseconds) + /// + public int DataTimeoutMs { get; set; } = 2000; + + /// + /// 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 + /// + public bool ChangeParam { get; set; } = false; + + /// + /// Tần số quay (Hz) - hợp lệ: 12 (12.5Hz), 25, 50. Chỉ dùng khi ChangeParam = true + /// + public int SpinFrequencyHz { get; set; } = 25; + + /// + /// Độ 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 + /// + public string AngleIncrementDeg { get; set; } = "0.100"; + + /// + /// Mức lọc nhiễu 0~3 - chỉ dùng khi ChangeParam = true + /// + public int NoiseFilterLevel { get; set; } = 1; +} + +/// +/// 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 +/// +[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(); + + /// + /// Constructor with configuration + /// + 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 + + /// + /// Current measurement data (scan points) + /// + public LaserScan? CurrentMeasurementData => _currentMeasurementData; + + /// + /// Timestamp of the most recent scan data + /// + public DateTime? LastScanDataTimestamp => _lastScanDataTimestamp; + + /// + /// Góc quét tối thiểu (radian) + /// + public double MinAngleRad => 0.0; + + /// + /// Góc quét tối đa (radian) - LiDAR quét đủ 360° + /// + public double MaxAngleRad => 2.0 * Math.PI; + + /// + /// Tầm quét tối thiểu (mét) + /// + public double MinRangeM => _config.MinRangeM; + + /// + /// Tầm quét tối đa (mét) + /// + public double MaxRangeM => _config.MaxRangeM; + + /// + /// Độ phân giải góc (radian) - tính từ dữ liệu thực tế + /// + public double? AngularResolutionRad => + _angleIncrementDeg > 0 ? _angleIncrementDeg * Math.PI / 180.0 : null; + + /// + /// Tần số quét (Hz) - đo từ tốc độ sinh LaserScan thực tế (12.5/25/50 Hz) + /// + public double? ScanFrequencyHz { get; private set; } + + /// + /// Field of View (radians) + /// + public double FieldOfViewRad => MaxAngleRad - MinAngleRad; + + /// + /// Hỗ trợ đo intensity + /// + public bool SupportsIntensity => true; + + /// + /// Độ chính xác đo khoảng cách (mét) + /// + public double? AccuracyM => DEFAULT_ACCURACY_M; + + /// + /// Event raised when new scan data is received + /// + public event EventHandler? 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 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 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; + } + } + + /// + /// Vòng lặp nhận dữ liệu từ LiDAR và parse frame + /// + 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 + + /// + /// 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. + /// + 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; + } + + /// + /// Xoá byteCount byte đầu của rx buffer + /// + 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; + } + + /// + /// 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). + /// + 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(); + } + + /// + /// Parse frame dữ liệu quét "HISN" ở đầu rx buffer + /// + 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; + } + + /// + /// Build LaserScan từ dữ liệu một vòng quét hoàn chỉnh và fire event + /// + 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)); + } + + /// + /// Cập nhật tần số quét dựa trên số LaserScan sinh ra mỗi giây + /// + 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 + + /// + /// Build lệnh cấu hình tham số LiDAR "SCtrl" (12 byte, CRC16-Modbus ở 2 byte cuối) + /// + 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 +} diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecN100IMU.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecN100IMU.cs index 6059184..e401eeb 100644 --- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecN100IMU.cs +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecN100IMU.cs @@ -1,863 +1,883 @@ -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.WheeltecIMU -{ - [Device(DeviceType.Imu, "WheeltecN100IMU", "WheeltecN100IMU", "1.0.0", Description = "IMU Simulation Driver")] - public class WheeltecN100IMU : DeviceBase, IInertialMeasurementUnit - { - private readonly WheeltecReader IMU; - - // Cached data - private AccelStamped _cachedAcceleration; - private Vector3Stamped _cachedAngularVelocity; - private Vector3Stamped _cachedMagnetometer; - private Vector3Stamped _cachedOrientation; - private QuaternionStamped _cachedQuaternion; - private double _cachedTemperature = 25.0; - 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? _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 (firmware drift ~0.009 rad/s) - // 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? AccelerationChanged; - public event EventHandler? AngularVelocityChanged; - public event EventHandler? MagnetometerChanged; - public event EventHandler? OrientationChanged; - - // Event thong nhat cho SensorPipeline - public event EventHandler? ImuDataChanged; - - public WheeltecN100IMU(string deviceId, string deviceName, IConfigurationSection connection) - : base(deviceId, deviceName, DeviceType.Imu) - { - _portName = connection.GetValue("Port") ?? throw new Exception("Port is required"); - _baudRate = connection.GetValue("BaudRate") ?? throw new Exception("BaudRate is required"); - _timeOut = connection.GetValue("TimeOut") ?? throw new Exception("Timeout is required"); - - IMU = new WheeltecReader(_portName, _baudRate, _timeOut); - - // Continuous IMU data printing (optional) - _printDataEnabled = connection.GetValue("DebugEnabled") ?? false; - _printDataInterval = TimeSpan.FromMilliseconds(connection.GetValue("PrintDataIntervalMs") ?? 200); - - // Doc cau hinh neu co - var autoReconnectEnabled = connection.GetValue("AutoReconnectEnabled"); - var reconnectDelayMs = connection.GetValue("ReconnectDelayMs"); - var maxReconnectAttempts = connection.GetValue("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 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 = "100" - }; - - 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" - }; - - yield return new PropertyDescription("Temperature", "Temperature (°C)", "Nhiet do cam bien (°C)") - { - DataType = "number", - IsReadOnly = true, - DisplayOrder = 6, - Category = "Du lieu", - DefaultValue = "25" - }; - } - - 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 temp = CachedTemperature; - var t = ((IInertialMeasurementUnit)this).LastUpdateTime; - var tempStr = temp.HasValue ? temp.Value.ToString("F2") : "NA"; - - 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}) " + - $"temp={tempStr}"); - } - 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(); - - // Dang ky event handler cho DataReceived tu WheeltecReader - IMU.DataReceived += IMU_DataReceived; - - // 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; - - // 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.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); - _cachedTemperature = 25.0; - _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(); - IMU.DataReceived += IMU_DataReceived; - - // 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 OnCheckConnectionAsync(CancellationToken cancellationToken) - { - return Task.FromResult(IMU.IsConnected); - } - - /// - /// Reset toan bo trang thai calibration ve gia tri ban dau - /// - 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; - } - - /// - /// Event handler cho DataReceived tu WheeltecReader. - /// 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. - /// - 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} [WheeltecN100IMU] 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; - } - - // FIXED: 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} [WheeltecN100IMU] 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 - // Firmware AHRS Yaw drift ~0.009 rad/s do tich phan gyro noi bo khong chinh xac - // CalibratedGz sau khi tru bias chi con ~0.00006 rad/s trung binh → giam drift 150 lan - 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} | " + - $"Temp={snapshot.Temp:F2}"); - } - - // 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} [WheeltecN100IMU] 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 - // Struct assignments are NOT atomic — without lock, readers could get partially updated data - lock (_dataLock) - { - previousAcceleration = _cachedAcceleration; - previousOrientation = _cachedOrientation; - - _cachedAcceleration = newAcceleration; - _cachedAngularVelocity = newAngularVelocity; - _cachedMagnetometer = newMagnetometer; - _cachedOrientation = newOrientation; - _cachedQuaternion = newQuaternion; - _cachedTemperature = snapshot.Temp; - _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"); - } - } - - /// - /// Cap nhat properties hien thi - /// - private void UpdateProperties() - { - var accel = _cachedAcceleration; - var angularVel = _cachedAngularVelocity; - var orientation = _cachedOrientation; - var temp = _cachedTemperature; - 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}"); - SetProperty("Temperature", temp.ToString("F2")); - } - - 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 - { - get { lock (_dataLock) { return _cachedTemperature; } } - } - - DateTime IInertialMeasurementUnit.LastUpdateTime - { - get { lock (_dataLock) { return _lastUpdateTime; } } - } - - public async Task ReadAccelerationAsync(CancellationToken cancellationToken = default) - { - await Task.CompletedTask; - return CachedAcceleration; - } - - public async Task ReadAngularVelocityAsync(CancellationToken cancellationToken = default) - { - await Task.CompletedTask; - return CachedAngularVelocity; - } - - public async Task ReadMagnetometerAsync(CancellationToken cancellationToken = default) - { - await Task.CompletedTask; - return CachedMagnetometer; - } - - public async Task ReadOrientationAsync(CancellationToken cancellationToken = default) - { - await Task.CompletedTask; - return CachedOrientation; - } - - public async Task ReadQuaternionAsync(CancellationToken cancellationToken = default) - { - await Task.CompletedTask; - return CachedQuaternion; - } - - public async Task 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 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(); - 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(); - 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.Disconnect(); - IMU.Dispose(); - _printDataTimer?.Dispose(); - _printDataTimer = null; - } - base.Dispose(disposing); - } - } -} +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.WheeltecIMU +{ + [Device(DeviceType.Imu, "WheeltecN100IMU", "WheeltecN100IMU", "1.0.0", Description = "IMU Simulation Driver")] + public class WheeltecN100IMU : DeviceBase, IInertialMeasurementUnit + { + private readonly WheeltecReader IMU; + + // Cached data + private AccelStamped _cachedAcceleration; + private Vector3Stamped _cachedAngularVelocity; + private Vector3Stamped _cachedMagnetometer; + private Vector3Stamped _cachedOrientation; + private QuaternionStamped _cachedQuaternion; + private double _cachedTemperature = 25.0; + 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? _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 (firmware drift ~0.009 rad/s) + // 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? AccelerationChanged; + public event EventHandler? AngularVelocityChanged; + public event EventHandler? MagnetometerChanged; + public event EventHandler? OrientationChanged; + + // Event thong nhat cho SensorPipeline + public event EventHandler? ImuDataChanged; + + public WheeltecN100IMU(string deviceId, string deviceName, IConfigurationSection connection) + : base(deviceId, deviceName, DeviceType.Imu) + { + _portName = connection.GetValue("Port") ?? throw new Exception("Port is required"); + _baudRate = connection.GetValue("BaudRate") ?? throw new Exception("BaudRate is required"); + _timeOut = connection.GetValue("TimeOut") ?? throw new Exception("Timeout is required"); + + IMU = new WheeltecReader(_portName, _baudRate, _timeOut); + + // Continuous IMU data printing (optional) + _printDataEnabled = connection.GetValue("DebugEnabled") ?? false; + _printDataInterval = TimeSpan.FromMilliseconds(connection.GetValue("PrintDataIntervalMs") ?? 200); + + // Doc cau hinh neu co + var autoReconnectEnabled = connection.GetValue("AutoReconnectEnabled"); + var reconnectDelayMs = connection.GetValue("ReconnectDelayMs"); + var maxReconnectAttempts = connection.GetValue("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 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 = "100" + }; + + 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" + }; + + yield return new PropertyDescription("Temperature", "Temperature (°C)", "Nhiet do cam bien (°C)") + { + DataType = "number", + IsReadOnly = true, + DisplayOrder = 6, + Category = "Du lieu", + DefaultValue = "25" + }; + } + + 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 temp = CachedTemperature; + var t = ((IInertialMeasurementUnit)this).LastUpdateTime; + var tempStr = temp.HasValue ? temp.Value.ToString("F2") : "NA"; + + 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}) " + + $"temp={tempStr}"); + } + 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(); + + // 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 + { + 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); + _cachedTemperature = 25.0; + _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(); + 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 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 -> yeu cau co frame gan day + return Task.FromResult(IMU.IsConnected && IMU.HasRecentData); + } + + /// + /// Reader tu reconnect ben trong; bao cho DeviceBase de state machine / UI + /// cap nhat dung trang thai (Connected/Disconnected/Reconnecting) + /// + private void IMU_ConnectionStateChanged(object? sender, bool connected) + { + _ = CheckConnectionAsync(); + } + + /// + /// Reset toan bo trang thai calibration ve gia tri ban dau + /// + 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; + } + + /// + /// Event handler cho DataReceived tu WheeltecReader. + /// 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. + /// + 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} [WheeltecN100IMU] 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; + } + + // FIXED: 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} [WheeltecN100IMU] 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 + // Firmware AHRS Yaw drift ~0.009 rad/s do tich phan gyro noi bo khong chinh xac + // CalibratedGz sau khi tru bias chi con ~0.00006 rad/s trung binh → giam drift 150 lan + 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} | " + + $"Temp={snapshot.Temp:F2}"); + } + + // 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} [WheeltecN100IMU] 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 + // Struct assignments are NOT atomic — without lock, readers could get partially updated data + lock (_dataLock) + { + previousAcceleration = _cachedAcceleration; + previousOrientation = _cachedOrientation; + + _cachedAcceleration = newAcceleration; + _cachedAngularVelocity = newAngularVelocity; + _cachedMagnetometer = newMagnetometer; + _cachedOrientation = newOrientation; + _cachedQuaternion = newQuaternion; + _cachedTemperature = snapshot.Temp; + _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"); + } + } + + /// + /// Cap nhat properties hien thi + /// + private void UpdateProperties() + { + var accel = _cachedAcceleration; + var angularVel = _cachedAngularVelocity; + var orientation = _cachedOrientation; + var temp = _cachedTemperature; + 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}"); + SetProperty("Temperature", temp.ToString("F2")); + } + + 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 + { + get { lock (_dataLock) { return _cachedTemperature; } } + } + + DateTime IInertialMeasurementUnit.LastUpdateTime + { + get { lock (_dataLock) { return _lastUpdateTime; } } + } + + public async Task ReadAccelerationAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedAcceleration; + } + + public async Task ReadAngularVelocityAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedAngularVelocity; + } + + public async Task ReadMagnetometerAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedMagnetometer; + } + + public async Task ReadOrientationAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedOrientation; + } + + public async Task ReadQuaternionAsync(CancellationToken cancellationToken = default) + { + await Task.CompletedTask; + return CachedQuaternion; + } + + public async Task 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 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; + IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged; + ResetCalibrationState(); + + _timerStartUtc = DateTime.UtcNow; + _highPrecisionTimer.Restart(); + + _calibrationCompletedTcs = new TaskCompletionSource(); + IMU.DataReceived += IMU_DataReceived; + IMU.ConnectionStateChanged += IMU_ConnectionStateChanged; + + 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; + IMU.ConnectionStateChanged -= IMU_ConnectionStateChanged; + ResetCalibrationState(); + + _timerStartUtc = DateTime.UtcNow; + _highPrecisionTimer.Restart(); + + _calibrationCompletedTcs = new TaskCompletionSource(); + IMU.DataReceived += IMU_DataReceived; + IMU.ConnectionStateChanged += IMU_ConnectionStateChanged; + + 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); + } + } +} diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs index 06a48aa..bb6cb18 100644 --- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs @@ -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? 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; + /// + /// 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) + /// + 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) { + // 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) { - if (DateTime.UtcNow.Ticks - _lastFrameTicks > DATA_TIMEOUT_TICKS) - { - throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s"); - } 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(); diff --git a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json index e14ca4e..3b2a78e 100644 --- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json +++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/appsettings.json @@ -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,