Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Hik/HikVisionQr.cs

@@ -0,0 +1,442 @@
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.Shared.Geometry;
+using System.Diagnostics;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+
+namespace RobotNet10.RobotApp.Drivers.Hik;
+
+/// <summary>
+/// Driver Hik cho Camera QR
+/// </summary>
+[Device(DeviceType.CameraQr, "Hik", "HikVisionQr", "1.0.0", Description = "Camera QR Code Detection Hik Driver")]
+public class HikVisionQr : DeviceBase, ICameraQr
+{
+    private readonly ILogger _logger;
+    private readonly Lock _dataLock = new();
+    private Thread? _receiveThread;
+    private CancellationTokenSource? _receiveCts;
+    private UdpClient? _udpClient;
+    private IPEndPoint? _localEndPoint;
+    private IPEndPoint? _remoteEndPoint;
+
+    // Camera parameters
+    private readonly int _cameraWidth;
+    private readonly int _cameraHeight;
+    private readonly double _distanceToQr;
+    private readonly double _fovRangeWidthMm;
+    private readonly double _fovRangeHeightMm;
+    private readonly double _fovRangeDistanceMm;
+    private readonly double _focalLengthPixels;
+    private readonly string _localIp;
+    private readonly int _localPort;
+    private readonly double _qrDataTimeoutSeconds;
+
+    // QR data dictionary (PoseStamped already contains timestamp in Header.Stamp)
+    private readonly Dictionary<string, PoseStamped> _qrDictionary = [];
+
+    // Data rate tracking
+    private int _packetCount = 0;
+    private double _currentDataRate = 0.0;
+
+    public Dictionary<string, PoseStamped> Codes
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                var validCodes = new Dictionary<string, PoseStamped>();
+                var now = DateTime.UtcNow;
+                var expiredKeys = new List<string>();
+
+                foreach (var (code, poseStamped) in _qrDictionary)
+                {
+                    var elapsed = (now - poseStamped.Header.Stamp).TotalSeconds;
+                    if (elapsed <= _qrDataTimeoutSeconds)
+                    {
+                        // Data is still valid
+                        validCodes[code] = poseStamped;
+                    }
+                    else
+                    {
+                        // Data expired, mark for removal
+                        expiredKeys.Add(code);
+                    }
+                }
+
+                // Clean up expired entries
+                foreach (var key in expiredKeys)
+                {
+                    _qrDictionary.Remove(key);
+                }
+
+                return validCodes;
+            }
+        }
+    }
+
+    public HikVisionQr(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
+        : base(deviceId, deviceName, DeviceType.CameraQr)
+    {
+        
+        _logger = serviceProvider.GetRequiredService<ILoggerFactory>().CreateLogger<HikVisionQr>();
+        // Read configuration
+        var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
+        var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
+        var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
+
+        // Camera parameters
+        _cameraWidth = connection.GetValue<int?>("CameraWidth") ?? 1920;
+        _cameraHeight = connection.GetValue<int?>("CameraHeight") ?? 1080;
+        _distanceToQr = connection.GetValue<double?>("DistanceToQr") ?? 0.1;
+        _fovRangeWidthMm = connection.GetValue<double?>("FovRangeWidthMm") ?? 170.0;
+        _fovRangeHeightMm = connection.GetValue<double?>("FovRangeHeightMm") ?? 130.0;
+        _fovRangeDistanceMm = connection.GetValue<double?>("FovRangeDistanceMm") ?? 100.0;
+        _qrDataTimeoutSeconds = connection.GetValue<double?>("QrDataTimeoutSeconds") ?? 3.0;
+
+        // Compute focal length in pixels from FOV range (pinhole camera model)
+        // f = CameraWidth * FovRangeDistance / FovRangeWidth
+        _focalLengthPixels = _cameraWidth * _fovRangeDistanceMm / _fovRangeWidthMm;
+
+        // UDP parameters
+        _localIp = connection.GetValue<string>("LocalIP") ?? "192.168.254.100";
+        _localPort = connection.GetValue<int?>("LocalPort") ?? 1024;
+
+        if (autoReconnectEnabled.HasValue)
+            AutoReconnectEnabled = autoReconnectEnabled.Value;
+
+        if (reconnectDelayMs.HasValue)
+            ReconnectDelayMs = reconnectDelayMs.Value;
+
+        if (maxReconnectAttempts.HasValue)
+            MaxReconnectAttempts = maxReconnectAttempts.Value;
+
+        // Initialize properties
+        UpdateProperties();
+    }
+
+    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
+    {
+        yield return new PropertyDescription("DataRate", "Data Rate", "Tần số nhận dữ liệu (packets/s)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 1,
+            Category = "Trạng thái",
+            DefaultValue = "0"
+        };
+    }
+
+    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
+    {
+        await Task.CompletedTask;
+    }
+
+    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+    {
+        return await Task.FromResult(_udpClient != null && _receiveThread != null && _receiveThread.IsAlive);
+    }
+
+    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
+    {
+        // Create local endpoint and UDP client
+        _localEndPoint = new IPEndPoint(IPAddress.Parse(_localIp), _localPort);
+        _udpClient = new UdpClient(_localEndPoint);
+
+        // Create cancellation token source for receive thread
+        _receiveCts = new CancellationTokenSource();
+
+        // Create and start high-priority thread for receiving data
+        _receiveThread = new Thread(() => ReceiveDataLoop(_receiveCts.Token))
+        {
+            IsBackground = true,
+            Priority = ThreadPriority.Highest,
+            Name = $"HikQr-{DeviceId}-ReceiveThread"
+        };
+        _receiveThread.Start();
+
+        await Task.CompletedTask;
+    }
+
+    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
+    {
+        // Stop receive thread
+        _receiveCts?.Cancel();
+
+        // Wait for thread to finish (with timeout)
+        if (_receiveThread != null && _receiveThread.IsAlive)
+        {
+            var joined = _receiveThread.Join(TimeSpan.FromSeconds(5));
+            if (!joined)
+            {
+                // Thread didn't finish gracefully
+                System.Diagnostics.Debug.WriteLine($"[HikQr] Receive thread did not finish in time");
+            }
+        }
+
+        _receiveCts?.Dispose();
+        _receiveCts = null;
+        _receiveThread = null;
+
+        // Close and dispose UDP client
+        _udpClient?.Close();
+        _udpClient?.Dispose();
+        _udpClient = null;
+        _localEndPoint = null;
+        _remoteEndPoint = null;
+
+        await Task.CompletedTask;
+    }
+
+    protected override async Task OnResetAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            // Clear QR dictionary
+            _qrDictionary.Clear();
+            _packetCount = 0;
+            _currentDataRate = 0.0;
+            UpdateProperties();
+        }
+
+        await Task.CompletedTask;
+    }
+
+    private void ReceiveDataLoop(CancellationToken cancellationToken)
+    {
+        Thread.BeginThreadAffinity();
+        try
+        {
+            var stopwatch = Stopwatch.StartNew();
+            var packetCountInSecond = 0;
+
+            while (!cancellationToken.IsCancellationRequested)
+            {
+                try
+                {
+                    // Check if 1 second has elapsed
+                    if (stopwatch.Elapsed.TotalSeconds >= 1.0)
+                    {
+                        lock (_dataLock)
+                        {
+                            _currentDataRate = packetCountInSecond / stopwatch.Elapsed.TotalSeconds;
+                            UpdateProperties();
+                        }
+
+                        // Reset counter and stopwatch
+                        packetCountInSecond = 0;
+                        stopwatch.Restart();
+                    }
+
+                    // Check if UDP client is available
+                    if (_udpClient == null)
+                        break;
+
+                    // Set receive timeout to allow checking cancellation token
+                    _udpClient.Client.ReceiveTimeout = 100;
+
+                    // Receive UDP packet
+                    var receivedData = _udpClient.Receive(ref _remoteEndPoint);
+
+                    if (receivedData.Length > 0)
+                    {
+                        packetCountInSecond++;
+                        lock (_dataLock)
+                        {
+                            _packetCount++;
+                        }
+
+                        // Process received data
+                        UpdateData(receivedData);
+                    }
+                }
+                catch (SocketException ex) when (ex.SocketErrorCode == SocketError.TimedOut)
+                {
+                    // Timeout is expected, continue loop
+                    continue;
+                }
+                catch (OperationCanceledException)
+                {
+                    break;
+                }
+                catch (Exception ex)
+                {
+                    // Log error and continue
+                    _logger.LogError(ex, "[HikVisionQr] ReceiveDataLoop error");
+                }
+            }
+
+            stopwatch.Stop();
+        }
+        finally
+        {
+            Thread.EndThreadAffinity();
+        }
+    }
+
+    private void UpdateData(byte[] receivedData)
+    {
+        lock (_dataLock)
+        {
+            if (receivedData.Length == 0)
+            {
+                // No data, clear dictionary
+                _qrDictionary.Clear();
+            }
+            else
+            {
+                try
+                {
+                    // Extract message (40 bytes from position 37)
+                    if (receivedData.Length < 77)
+                    {
+                        // Invalid packet size
+                        return;
+                    }
+
+                    byte[] messageBytes = new byte[40];
+                    Array.Copy(receivedData, 37, messageBytes, 0, 40);
+                    string message = Encoding.UTF8.GetString(messageBytes);
+
+                    // Extract QR data (9 bytes)
+                    byte[] dataBytes = new byte[9];
+                    Array.Copy(messageBytes, 0, dataBytes, 0, 9);
+                    string qrCode = Encoding.UTF8.GetString(dataBytes);
+
+                    // Extract X position (3 bytes from position 11)
+                    byte[] xBytes = new byte[3];
+                    Array.Copy(messageBytes, 11, xBytes, 0, 3);
+                    int qrPositionX = int.Parse(Encoding.ASCII.GetString(xBytes));
+
+                    // Extract Y position (3 bytes from position 19)
+                    byte[] yBytes = new byte[3];
+                    Array.Copy(messageBytes, 19, yBytes, 0, 3);
+                    int qrPositionY = int.Parse(Encoding.ASCII.GetString(yBytes));
+
+                    // Extract angle (between ')' and '@')
+                    double qrAngle = 0;
+                    int start = message.IndexOf(')') + 1;
+                    int end = message.IndexOf('@', start);
+                    if (start > 0 && end > start)
+                    {
+                        string numberAngle = message[start..end];
+                        qrAngle = double.Parse(numberAngle);
+                    }
+
+                    // Calculate pose immediately
+                    var pose = CreatePoseFromPixels(
+                        qrPositionX,
+                        qrPositionY,
+                        qrAngle,
+                        _cameraWidth,
+                        _cameraHeight,
+                        _distanceToQr,
+                        _focalLengthPixels);
+
+                    if (pose.HasValue)
+                    {
+                        // Console.WriteLine($"[HikQr] Detected QR Code: {qrCode}, X: {pose.Value.Pose.Position.X}, Y: {pose.Value.Pose.Position.Y}, Angle: {pose.Value.Pose.Orientation.ToYawDegrees()} degrees");
+                        // Add or update dictionary (timestamp is in pose.Header.Stamp)
+                        _qrDictionary[qrCode] = pose.Value;
+                    }
+                }
+                catch (Exception ex)
+                {
+                    System.Diagnostics.Debug.WriteLine($"[HikQr] UpdateData parsing error: {ex.Message}");
+                }
+            }
+
+            UpdateProperties();
+        }
+    }
+
+    private void UpdateProperties()
+    {
+        lock (_dataLock)
+        {
+            SetProperty("DataRate", _currentDataRate.ToString("F2"));
+        }
+    }
+
+    protected override void Dispose(bool disposing)
+    {
+        if (disposing)
+        {
+            _receiveCts?.Cancel();
+            _receiveCts?.Dispose();
+            _udpClient?.Close();
+            _udpClient?.Dispose();
+        }
+        base.Dispose(disposing);
+    }
+
+    #region ICameraQr Implementation
+
+    public PoseStamped? this[string code]
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                if (!_qrDictionary.TryGetValue(code, out var poseStamped))
+                    return null;
+
+                // Check if data is still valid (within timeout)
+                var elapsed = (DateTime.UtcNow - poseStamped.Header.Stamp).TotalSeconds;
+                if (elapsed > _qrDataTimeoutSeconds)
+                {
+                    // Data expired, remove from dictionary
+                    _qrDictionary.Remove(code);
+                    return null;
+                }
+
+                // Data is valid, return pose
+                return poseStamped;
+            }
+        }
+    }
+
+    private static PoseStamped? CreatePoseFromPixels(
+        int? px,
+        int? py,
+        double? angleDeg,
+        int cameraWidth,
+        int cameraHeight,
+        double distanceMeters,
+        double focalLengthPixels)
+    {
+        if (!px.HasValue || !py.HasValue || !angleDeg.HasValue)
+            return null;
+
+        // Convert pixel coordinates to meters using pinhole camera model
+        // Same focal length for both axes (square pixels: 4.8μm × 4.8μm)
+        double xMeters = (px.Value - cameraWidth / 2.0) * distanceMeters / focalLengthPixels;
+        double yMeters = -(py.Value - cameraHeight / 2.0) * distanceMeters / focalLengthPixels; // Invert Y axis
+        double zMeters = distanceMeters;
+
+        // Convert angle to quaternion
+        double angleRad = angleDeg.Value * Math.PI / 180.0;
+        var quat = RobotNet10.Shared.Numbers.Quaternion.FromYawRadian(angleRad);
+
+        // Create header
+        var header = new RobotNet10.Shared.Header
+        {
+            Seq = 0,
+            Stamp = DateTime.UtcNow,
+            FrameId = "camera"
+        };
+
+        // Create pose
+        var pose = new Pose
+        {
+            Position = new Point { X = xMeters, Y = yMeters, Z = zMeters },
+            Orientation = new Quaternion(quat.X, quat.Y, quat.Z, quat.W)
+        };
+
+        return new PoseStamped(header, pose);
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/Olei2dLidarDriver.cs

@@ -0,0 +1,562 @@
+using System.Diagnostics;
+using Olei.LidarSensor;
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.Shared;
+using RobotNet10.Shared.Sensor;
+
+namespace RobotNet10.RobotApp.Drivers.Olei;
+
+/// <summary>
+/// Configuration for Olei 2D LiDAR Driver
+/// </summary>
+public class Olei2dLidarDriverConfig
+{
+    /// <summary>
+    /// UDP port to listen for LiDAR data
+    /// Default: 2368 (typical LiDAR port)
+    /// </summary>
+    public int UdpPort { get; set; } = 2368;
+
+    /// <summary>
+    /// Frame ID for the LiDAR sensor
+    /// Used in ROS-style message headers
+    /// </summary>
+    public string FrameId { get; set; } = "laser";
+
+    /// <summary>
+    /// Whether the LiDAR is mounted inverted (upside down).
+    /// When true, scan angles are mirrored (180° - angle) to correct orientation.
+    /// </summary>
+    public bool Inverted { get; set; } = false;
+}
+
+/// <summary>
+/// Driver for Olei 2D LiDAR Sensor (LR-1F / LR-1BS)
+/// Implements DeviceBase and ILidar interface
+/// Protocol: UDP/IP v2.1
+/// </summary>
+[Device(DeviceType.Lidar, "Olei", "Olei2dLidarDriver", "1.0.0",
+    Description = "2D LiDAR Sensor Communication Data Protocol v2.1")]
+public class Olei2dLidarDriver : DeviceBase, ILidar
+{
+    private readonly Olei2dLidarDriverConfig _config = new();
+    private OleiLidarServer? _lidarServer;
+
+    // Cached measurements
+    private LaserScan? _currentMeasurementData;
+    private DateTime? _lastScanDataTimestamp;
+
+    // Frequency update tracking (for UI properties)
+    private readonly Stopwatch _frequencyUpdateStopwatch = Stopwatch.StartNew();
+
+    // Scan frequency calculation (actual LaserScan generation rate)
+    private readonly Stopwatch _scanFrequencyStopwatch = Stopwatch.StartNew();
+    private long _scansGeneratedInCurrentSecond = 0;
+    private readonly Lock _scanFrequencyLock = new();
+
+    // Calculated LiDAR specifications from actual data
+    private const double _minAngleRad = 0.0;
+    private const double _maxAngleRad = 2.0 * Math.PI;
+    private double _angularResolutionRad = 0.0;  // Calculated from actual packet data
+    private const double DEFAULT_ACCURACY_M = 0.02;   // 2 cm accuracy (fixed by hardware)
+
+    // Hardware range specifications (no filtering applied)
+    private const double HARDWARE_MIN_RANGE_M = 0.0;
+    private const double HARDWARE_MAX_RANGE_M = 30.0;  // 30m max range for Olei LiDAR
+
+    // Packet accumulation for full scan (0° ~ 360°)
+    private readonly List<LidarDataBlock> _accumulatedBlocks = [];
+    private double _lastPacketStartAngle = -1.0;
+    private readonly Lock _accumulationLock = new();
+    private uint _scanSequenceNumber = 0;
+    private DateTime _currentScanStartTime = DateTime.UtcNow;
+
+    /// <summary>
+    /// Constructor with configuration
+    /// </summary>
+    public Olei2dLidarDriver(
+        string deviceId,
+        string deviceName, IConfigurationSection configuration)
+        : base(deviceId, deviceName, DeviceType.Lidar)
+    {
+        configuration.Bind(_config);
+        Description = "Olei 2D LiDAR Sensor (LR-1F/LR-1BS) - UDP Protocol v2.1";
+    }
+
+    #region ILidar Implementation
+
+    /// <summary>
+    /// Current measurement data (scan points)
+    /// </summary>
+    public LaserScan? CurrentMeasurementData => _currentMeasurementData;
+
+    /// <summary>
+    /// Timestamp of the most recent scan data
+    /// </summary>
+    public DateTime? LastScanDataTimestamp => _lastScanDataTimestamp;
+
+    /// <summary>
+    /// Minimum scan angle (radians)
+    /// Calculated from actual LiDAR data
+    /// </summary>
+    public double MinAngleRad => _minAngleRad;
+
+    /// <summary>
+    /// Maximum scan angle (radians)
+    /// Calculated from actual LiDAR data
+    /// </summary>
+    public double MaxAngleRad => _maxAngleRad;
+
+    /// <summary>
+    /// Minimum measurement range (meters)
+    /// Hardware specification (no filtering)
+    /// </summary>
+    public double MinRangeM => HARDWARE_MIN_RANGE_M;
+
+    /// <summary>
+    /// Maximum measurement range (meters)
+    /// Hardware specification (no filtering)
+    /// </summary>
+    public double MaxRangeM => HARDWARE_MAX_RANGE_M;
+
+    /// <summary>
+    /// Angular resolution (radians)
+    /// Calculated from actual LiDAR data
+    /// </summary>
+    public double? AngularResolutionRad => _angularResolutionRad;
+
+    /// <summary>
+    /// Scan frequency (Hz)
+    /// Typically 10-20 Hz for Olei LiDAR
+    /// </summary>
+    public double? ScanFrequencyHz { get; private set; }
+
+    /// <summary>
+    /// Field of View (radians)
+    /// </summary>
+    public double FieldOfViewRad => MaxAngleRad - MinAngleRad;
+
+    /// <summary>
+    /// Supports intensity measurements
+    /// </summary>
+    public bool SupportsIntensity => true;
+
+    /// <summary>
+    /// Measurement accuracy (meters)
+    /// </summary>
+    public double? AccuracyM => DEFAULT_ACCURACY_M;
+
+    /// <summary>
+    /// Event raised when new scan data is received
+    /// </summary>
+    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
+
+    #endregion
+
+    #region DeviceBase Implementation
+
+    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
+    {
+        await Task.Run(() =>
+        {
+            // Initialize LiDAR server
+            _lidarServer = new OleiLidarServer(_config.UdpPort);
+
+            // Subscribe to events
+            _lidarServer.DataReceived += OnLidarDataReceived;
+            _lidarServer.ErrorOccurred += OnLidarErrorOccurred;
+
+            SetProperty("UdpPort", _config.UdpPort.ToString());
+            SetProperty("FrameId", _config.FrameId);
+            SetProperty("MinRange", $"{HARDWARE_MIN_RANGE_M:F2} m");
+            SetProperty("MaxRange", $"{HARDWARE_MAX_RANGE_M:F2} m");
+            SetProperty("FOV", "N/A");  // Will be calculated from actual data
+            SetProperty("AngularResolution", "N/A");  // Will be calculated from actual data
+
+        }, cancellationToken);
+    }
+
+    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
+    {
+        if (_lidarServer == null)
+            throw new InvalidOperationException("LiDAR server not initialized. Call InitializeAsync first.");
+
+        // Start LiDAR server
+        _lidarServer.Start();
+
+        SetProperty("ServerStatus", "Running");
+        SetProperty("Statistics", _lidarServer.GetStatistics());
+    }
+
+    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
+    {
+         _lidarServer?.Stop();
+
+        SetProperty("ServerStatus", "Stopped");
+        SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
+    }
+
+    protected override async Task OnResetAsync(CancellationToken cancellationToken)
+    {
+        // Reset statistics
+        _lidarServer?.ResetStatistics();
+
+        // Reset scan frequency tracking
+        lock (_scanFrequencyLock)
+        {
+            Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
+            _scanFrequencyStopwatch.Restart();
+            ScanFrequencyHz = null;
+        }
+
+        SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
+        SetProperty("ScanFrequency", "N/A");
+    }
+
+    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+    {
+        await Task.Delay(500, cancellationToken);
+        if (_lidarServer == null || !_lidarServer.IsRunning)
+            return false;
+
+        lock (_accumulationLock)
+        {
+            return _lastScanDataTimestamp.HasValue;
+        }
+    }
+
+    protected override List<PropertyDescription> CreatePropertyDescriptions()
+    {
+        return
+        [
+            new PropertyDescription("UdpPort", "UDP Port", "UDP port for receiving LiDAR data"),
+            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("FOV", "Field of View", "Total scanning angle"),
+            new PropertyDescription("AngularResolution", "Angular Resolution", "Angle between scan points"),
+            new PropertyDescription("ServerStatus", "Server Status", "UDP server status"),
+            new PropertyDescription("Statistics", "Statistics", "Server statistics"),
+            new PropertyDescription("LastScanTime", "Last Scan Time", "Timestamp of last scan"),
+            new PropertyDescription("ScanFrequency", "Scan Frequency", "Actual scan rate (Hz)"),
+        ];
+    }
+
+    protected override void Dispose(bool disposing)
+    {
+        if (disposing && _lidarServer != null)
+        {
+            _lidarServer.DataReceived -= OnLidarDataReceived;
+            _lidarServer.ErrorOccurred -= OnLidarErrorOccurred;
+            _lidarServer.Dispose();
+            _lidarServer = null;
+        }
+
+        base.Dispose(disposing);
+    }
+
+    #endregion
+
+    #region Event Handlers
+
+    /// <summary>
+    /// Handle data received from LiDAR server
+    /// Accumulates packets until full 360° scan is complete
+    /// </summary>
+    private void OnLidarDataReceived(object? sender, LidarDataPacket e)
+    {
+        try
+        {
+            var packet = e;
+
+            // Validate packet
+            if (!packet.Header.IsValidFrame)
+                return;
+
+            // Get first valid block's angle to detect scan wrap
+            double packetStartAngle = -1.0;
+            for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
+            {
+                if (packet.DataBlocks[i].IsValid)
+                {
+                    packetStartAngle = packet.DataBlocks[i].GetAngleDegrees();
+                    break;
+                }
+            }
+
+            if (packetStartAngle < 0)
+                return; // No valid blocks in packet
+
+            LaserScan? completedScan = null;
+            bool updateProperties = false;
+
+            lock (_accumulationLock)
+            {
+                // Detect scan completion: angle wrapped back to near 0° after being > 300°
+                if (_lastPacketStartAngle > 300.0 && packetStartAngle < 50.0 && _accumulatedBlocks.Count > 0)
+                {
+                    var scan = CreateLaserScanFromAccumulated(packet.Header, _currentScanStartTime);
+                    completedScan = scan;
+
+                    _currentMeasurementData = scan;
+                    _lastScanDataTimestamp = scan.Header.Stamp;
+
+                    Interlocked.Increment(ref _scansGeneratedInCurrentSecond);
+                    UpdateScanFrequency();
+
+                    if (_frequencyUpdateStopwatch.ElapsedMilliseconds > 1000)
+                    {
+                        updateProperties = true;
+                        _frequencyUpdateStopwatch.Restart();
+                    }
+
+                    _accumulatedBlocks.Clear();
+                    _currentScanStartTime = DateTime.UtcNow;
+                }
+                else if (_accumulatedBlocks.Count == 0)
+                {
+                    _currentScanStartTime = DateTime.UtcNow;
+                }
+
+                // Add current packet's blocks to accumulation buffer
+                for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
+                {
+                    var block = packet.DataBlocks[i];
+                    if (block.IsValid)
+                    {
+                        double angleDeg = block.GetAngleDegrees();
+                        if (angleDeg >= 360.0)
+                            angleDeg %= 360.0;
+                        _accumulatedBlocks.Add(block);
+                    }
+                }
+
+                _lastPacketStartAngle = packetStartAngle;
+            }
+
+            // Fire event and update properties outside the lock to avoid blocking UDP reception
+            if (completedScan is { } publishedScan)
+            {
+                if (updateProperties)
+                {
+                    SetProperty("LastScanTime", publishedScan.Header.Stamp.ToString("HH:mm:ss.fff"));
+                    SetProperty("ScanFrequency", ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A");
+                    SetProperty("FOV", $"{FieldOfViewRad * 180 / Math.PI:F1}°");
+                    SetProperty("AngularResolution", $"{_angularResolutionRad * 180 / Math.PI:F3}°");
+                }
+
+                ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(
+                    publishedScan.Header.Stamp,
+                    publishedScan
+                ));
+            }
+        }
+        catch (Exception ex)
+        {
+            OnErrorOccurred(new Exception($"Error processing LiDAR data: {ex.Message}", ex));
+        }
+    }
+
+    /// <summary>
+    /// Handle errors from LiDAR server
+    /// </summary>
+    private void OnLidarErrorOccurred(object? sender, LidarErrorEventArgs e)
+    {
+        OnErrorOccurred(e.Exception ?? new Exception(e.Message));
+    }
+
+    #endregion
+
+    #region Helper Methods
+
+    /// <summary>
+    /// Update scan frequency based on LaserScan generation rate
+    /// Calculates how many scans are generated per second
+    /// </summary>
+    private void UpdateScanFrequency()
+    {
+        // Check if 1 second has elapsed
+        if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
+        {
+            lock (_scanFrequencyLock)
+            {
+                // Double-check inside lock to prevent race condition
+                if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
+                {
+                    // Calculate frequency (scans per second)
+                    long scanCount = Interlocked.Read(ref _scansGeneratedInCurrentSecond);
+                    double elapsedSeconds = _scanFrequencyStopwatch.ElapsedMilliseconds / 1000.0;
+                    ScanFrequencyHz = scanCount / elapsedSeconds;
+
+                    // Reset for next measurement period
+                    Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
+                    _scanFrequencyStopwatch.Restart();
+                }
+            }
+        }
+    }
+
+    /// <summary>
+    /// Create LaserScan from accumulated blocks (full 360° scan)
+    /// </summary>
+    private LaserScan CreateLaserScanFromAccumulated(LidarHeader lastHeader, DateTime scanStartTime)
+    {
+        var header = new Header(
+            seq: _scanSequenceNumber++,
+            stamp: scanStartTime,
+            frameId: _config.FrameId
+        );
+
+        // Get distance scale from last header
+        byte distanceScale = lastHeader.DistanceScale;
+        double distanceScaleToMeters = distanceScale / 1000.0;
+
+        // Calculate actual angle and range from accumulated data
+        double minAngleDeg = double.MaxValue;
+        double maxAngleDeg = double.MinValue;
+        double minDistanceM = double.MaxValue;
+        double maxDistanceM = double.MinValue;
+
+        // Create sorted list of angles to calculate angular resolution
+        var sortedAngles = new List<double>(_accumulatedBlocks.Count);
+
+        // First pass: Find actual min/max values from data and collect angles
+        foreach (var block in _accumulatedBlocks)
+        {
+            double angleDeg = block.GetAngleDegrees();
+
+            // Normalize angle to 0-360° range
+            if (angleDeg >= 360.0)
+                angleDeg %= 360.0;
+
+            minAngleDeg = Math.Min(minAngleDeg, angleDeg);
+            maxAngleDeg = Math.Max(maxAngleDeg, angleDeg);
+            sortedAngles.Add(angleDeg);
+
+            double distanceM = block.DistanceRaw * distanceScaleToMeters;
+            if (distanceM > 0)  // Only consider valid distances
+            {
+                minDistanceM = Math.Min(minDistanceM, distanceM);
+                maxDistanceM = Math.Max(maxDistanceM, distanceM);
+            }
+        }
+
+        // Fallback to defaults if no valid data
+        if (minAngleDeg == double.MaxValue || maxAngleDeg == double.MinValue)
+        {
+            minAngleDeg = 0.0;
+            maxAngleDeg = 360.0;
+        }
+
+        if (minDistanceM == double.MaxValue || maxDistanceM == double.MinValue)
+        {
+            minDistanceM = HARDWARE_MIN_RANGE_M;
+            maxDistanceM = HARDWARE_MAX_RANGE_M;
+        }
+
+        // Calculate angular resolution from actual data
+        double angularResolutionDeg = 0.225;  // Default fallback
+        if (sortedAngles.Count > 1)
+        {
+            sortedAngles.Sort();
+
+            // Find minimum difference between consecutive angles
+            double minAngleDiff = double.MaxValue;
+            for (int i = 1; i < sortedAngles.Count; i++)
+            {
+                double diff = sortedAngles[i] - sortedAngles[i - 1];
+                if (diff > 0.01)  // Ignore very small differences (noise/duplicates)
+                {
+                    minAngleDiff = Math.Min(minAngleDiff, diff);
+                }
+            }
+
+            if (minAngleDiff < double.MaxValue)
+            {
+                angularResolutionDeg = minAngleDiff;
+            }
+        }
+
+        // Update cached angular resolution for property reporting
+        _angularResolutionRad = angularResolutionDeg * Math.PI / 180.0;
+
+        // Convert angles to radians
+        double angleMinRad = minAngleDeg * Math.PI / 180.0;
+        double angleMaxRad = maxAngleDeg * Math.PI / 180.0;
+
+        // Calculate expected number of points based on calculated angular resolution
+        double angleSpanDeg = maxAngleDeg - minAngleDeg;
+        int expectedPoints = (int)Math.Ceiling(angleSpanDeg / angularResolutionDeg) + 1;
+
+        // Initialize arrays with expected size
+        double[] ranges = new double[expectedPoints];
+        double[] intensities = new double[expectedPoints];
+
+        // Initialize all to -1 (no data, JSON-safe)
+        Array.Fill(ranges, -1.0);
+        Array.Fill(intensities, 0.0);
+
+        // Second pass: Map accumulated blocks to array indices based on angle
+        int validCount = 0;
+        foreach (var block in _accumulatedBlocks)
+        {
+            double angleDeg = _config.Inverted
+                ? (block.GetAngleDegrees() + 180.0) % 360.0
+                : block.GetAngleDegrees();
+
+            // Normalize angle to 0-360° range
+            if (angleDeg < 0)
+                angleDeg += 360.0;
+            else if (angleDeg >= 360.0)
+                angleDeg %= 360.0;
+
+            // Calculate array index from relative angle position using calculated angular resolution
+            double relativeAngle = angleDeg - minAngleDeg;
+            int index = (int)Math.Round(relativeAngle / angularResolutionDeg);
+
+            // Clamp index to valid range
+            if (index >= 0 && index < expectedPoints)
+            {
+                double distanceM = block.DistanceRaw * distanceScaleToMeters;
+                if (distanceM <= 0 || distanceM >= HARDWARE_MAX_RANGE_M)
+                {
+                    ranges[index] = -1.0;  // No detection / max-range return
+                    intensities[index] = 0.0;
+                    continue;
+                }
+                // Store all distance values (no range filtering)
+                ranges[index] = distanceM;
+                intensities[index] = block.SignalStrength;
+                validCount++;
+            }
+        }
+
+        // Calculate angle increment from actual data
+        double angleIncrementRad = expectedPoints > 1
+            ? (angleMaxRad - angleMinRad) / (expectedPoints - 1)
+            : angularResolutionDeg * Math.PI / 180.0;  // Use calculated resolution
+
+        // Calculate scan timing (estimate based on rotation rate if available)
+        double scanTime = ScanFrequencyHz.HasValue && ScanFrequencyHz.Value > 0
+            ? 1.0 / ScanFrequencyHz.Value
+            : 0.1; // Default 10 Hz
+        double timeIncrement = scanTime / expectedPoints;
+
+        return new LaserScan
+        {
+            Header = header,
+            AngleMin = angleMinRad,       // From actual data
+            AngleMax = angleMaxRad,       // From actual data
+            AngleIncrement = angleIncrementRad,  // Calculated from data
+            TimeIncrement = timeIncrement,
+            ScanTime = scanTime,
+            RangeMin = HARDWARE_MIN_RANGE_M,
+            RangeMax = HARDWARE_MAX_RANGE_M,
+            Ranges = ranges,
+            Intensities = intensities
+        };
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiGS15Driver.cs

@@ -0,0 +1,919 @@
+using System.Buffers.Binary;
+using System.Linq;
+using System.Net;
+using System.Net.NetworkInformation;
+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.Lidar;
+
+/// <summary>
+/// UDP connection for Olei GS1-5 (V3 protocol). Packet size 1136 bytes, not 1240.
+/// </summary>
+internal sealed class OleiGS15UdpConnection
+{
+    public const int ExpectedPacketSize = 1136;
+
+    private readonly Queue<Gs15Packet> _packetQueue = new();
+    private readonly object _packetLock = new();
+
+    public string device_IP { get; set; } = "192.168.110.22";
+    public int device_port { get; set; } = 2468;
+    public string local_ip { get; set; } = "192.168.110.114";
+    public UdpClient? udp;
+    public IPEndPoint check_ip_device = new(IPAddress.Any, 0);
+
+    public bool openPort()
+    {
+        try
+        {
+            if (!IpExists(local_ip))
+            {
+                Console.WriteLine($"[ERROR] OleiGS15: local_ip {local_ip} does NOT exist on any NIC.");
+                return false;
+            }
+            if (IsPortBusy(device_port))
+            {
+                Console.WriteLine($"[ERROR] OleiGS15: Port {device_port} is already in use!");
+                return false;
+            }
+            udp = new UdpClient(new IPEndPoint(IPAddress.Parse(local_ip), device_port));
+            check_ip_device = new IPEndPoint(IPAddress.Parse(local_ip), device_port);
+            udp.Client.ReceiveTimeout = 1000;
+            Console.WriteLine("[INFO] OleiGS15 UDP port opened.");
+            return true;
+        }
+        catch (Exception ex)
+        {
+            Console.WriteLine($"[ERROR] OleiGS15: Cannot open UDP {local_ip}:{device_port} - {ex.Message}");
+            return false;
+        }
+    }
+
+    public void readRawdata()
+    {
+        if (udp == null) return;
+        try
+        {
+            byte[] data = udp.Receive(ref check_ip_device);
+            if (check_ip_device.Address == null || check_ip_device.Address.Equals(IPAddress.Any) || check_ip_device.Address.Equals(IPAddress.None) || check_ip_device.Port == 0)
+                return;
+            if (check_ip_device.Address.ToString() != device_IP)
+                return;
+            if (data.Length != ExpectedPacketSize)
+                return;
+
+            var packet = new Gs15Packet { stamp = DateTime.UtcNow, data = data };
+            lock (_packetLock)
+            {
+                _packetQueue.Enqueue(packet);
+            }
+        }
+        catch (SocketException ex)
+        {
+            if (ex.SocketErrorCode != SocketError.TimedOut)
+                Console.WriteLine($"[ERROR] OleiGS15 socket: {ex.Message}");
+        }
+        catch (Exception) { }
+    }
+
+    public Gs15Packet? TryDequeuePacket()
+    {
+        lock (_packetLock)
+        {
+            if (_packetQueue.Count == 0) return null;
+            return _packetQueue.Dequeue();
+        }
+    }
+
+    private static bool IpExists(string ip)
+    {
+        foreach (var ni in NetworkInterface.GetAllNetworkInterfaces())
+        {
+            foreach (var ua in ni.GetIPProperties().UnicastAddresses)
+            {
+                if (ua.Address.ToString() == ip) return true;
+            }
+        }
+        return false;
+    }
+
+    private static bool IsPortBusy(int port)
+    {
+        var listeners = IPGlobalProperties.GetIPGlobalProperties().GetActiveUdpListeners();
+        return listeners.Any(ep => ep.Port == port);
+    }
+}
+
+internal sealed class Gs15Packet
+{
+    public DateTime stamp;
+    public byte[] data = Array.Empty<byte>();
+}
+
+public sealed class OleiGS15DriverConfig
+{
+    public int Version { get; set; } = 3;
+    public string ScannerIp { get; set; } = "192.168.110.22";
+    public string LocalIp { get; set; } = "192.168.110.114";
+    public int Port { get; set; } = 2468;
+    public string Transport { get; set; } = "udp";
+    public string FrameId { get; set; } = "olelidar";
+    public string ScanTopic { get; set; } = "scan";
+    public bool Inverted { get; set; } = false;
+    public bool TimeFromLidar { get; set; } = true;
+    public bool Ntp { get; set; } = false;
+    public string PacketType { get; set; } = "B";
+    public double RangeMin { get; set; } = 0.08;
+    public double RangeMax { get; set; } = 50.0;
+    public bool AutoReconnectEnabled { get; set; } = true;
+    public int ReconnectDelayMs { get; set; } = 3000;
+    public int MaxReconnectAttempts { get; set; } = 0;
+
+    public static OleiGS15DriverConfig Parse(IConfigurationSection connection)
+    {
+        var cfg = new OleiGS15DriverConfig
+        {
+            Version = connection.GetValue<int?>("Version")
+                ?? connection.GetValue<int?>("version")
+                ?? 3,
+            ScannerIp = connection["ScannerIp"]
+                ?? connection["scanner_ip"]
+                ?? connection["DeviceIp"]
+                ?? "192.168.110.22",
+            LocalIp = connection["LocalIp"]
+                ?? connection["local_ip"]
+                ?? "192.168.110.114",
+            Transport = connection["Transport"]
+                ?? connection["transport"]
+                ?? "udp",
+            FrameId = connection["FrameId"]
+                ?? connection["frame_id"]
+                ?? "olelidar",
+            ScanTopic = connection["ScanTopic"]
+                ?? connection["scan_topic"]
+                ?? "scan",
+            PacketType = connection["PacketType"]
+                ?? connection["packet_type"]
+                ?? "B",
+            Inverted = connection.GetValue<bool?>("Inverted")
+                ?? connection.GetValue<bool?>("inverted")
+                ?? false,
+            TimeFromLidar = connection.GetValue<bool?>("TimeFromLidar")
+                ?? connection.GetValue<bool?>("timeFromLidar")
+                ?? true,
+            Ntp = connection.GetValue<bool?>("Ntp")
+                ?? connection.GetValue<bool?>("ntp")
+                ?? false,
+            RangeMin = connection.GetValue<double?>("RangeMin")
+                ?? connection.GetValue<double?>("range_min")
+                ?? 0.08,
+            RangeMax = connection.GetValue<double?>("RangeMax")
+                ?? connection.GetValue<double?>("range_max")
+                ?? 50.0,
+            AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true,
+            ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 3000,
+            MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0
+        };
+
+        var port = connection.GetValue<int?>("Port")
+            ?? connection.GetValue<int?>("port")
+            ?? connection.GetValue<int?>("DevicePort")
+            ?? 2368;
+        if (port is < 1 or > 65535)
+        {
+            throw new InvalidOperationException($"Invalid Port: {port}. Must be between 1 and 65535");
+        }
+
+        cfg.Port = port;
+
+        if (!string.Equals(cfg.Transport, "udp", StringComparison.OrdinalIgnoreCase))
+        {
+            throw new InvalidOperationException("OleiGS15Driver currently supports transport=udp only");
+        }
+
+        if (cfg.RangeMin < 0)
+        {
+            throw new InvalidOperationException($"RangeMin must be >= 0. Current value: {cfg.RangeMin}");
+        }
+
+        if (cfg.RangeMax <= cfg.RangeMin)
+        {
+            throw new InvalidOperationException(
+                $"RangeMax must be greater than RangeMin. RangeMin={cfg.RangeMin}, RangeMax={cfg.RangeMax}");
+        }
+
+        return cfg;
+    }
+}
+
+[Device(DeviceType.Lidar, "Olei", "OleiGS15Driver", "1.0.0",
+    Description = "Olei GS1-5 LiDAR driver (ROS version3-compatible UDP parser)")]
+public class OleiGS15Driver : DeviceBase, ILidar
+{
+    private const int V3HeaderSize = 48;
+    private const ushort V3Magic = 0xFEAC;
+
+    private readonly OleiGS15DriverConfig _config;
+    private readonly OleiGS15UdpConnection _connection = new();
+    private readonly Lock _dataLock = new();
+
+    private CancellationTokenSource? _updateCts;
+    private Task? _updateTask;
+
+    private LaserScan? _lastLaserScan;
+    private DateTime? _lastScanDataTimestamp;
+    private int _lastPointCount;
+    private double? _lastScanFrequencyHz;
+    private bool _portOpened;
+
+    private V3ScanAccumulator? _scanAccumulator;
+    private ushort? _lastFirstIndex;
+
+    public OleiGS15Driver(string deviceId, string deviceName, IConfigurationSection connection)
+        : base(deviceId, deviceName, DeviceType.Lidar)
+    {
+        _config = OleiGS15DriverConfig.Parse(connection);
+
+        _connection.device_IP = _config.ScannerIp;
+        _connection.local_ip = _config.LocalIp;
+        _connection.device_port = _config.Port;
+
+        AutoReconnectEnabled = _config.AutoReconnectEnabled;
+        ReconnectDelayMs = _config.ReconnectDelayMs;
+        MaxReconnectAttempts = _config.MaxReconnectAttempts;
+
+        UpdateProperties();
+    }
+
+    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
+    {
+        yield return new PropertyDescription("Version", "Version", "Phiên bản protocol")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 1,
+            Category = "Cấu hình",
+            DefaultValue = "3"
+        };
+
+        yield return new PropertyDescription("PacketType", "Packet Type", "Loại packet A/B/C")
+        {
+            DataType = "string",
+            IsReadOnly = true,
+            DisplayOrder = 2,
+            Category = "Cấu hình",
+            DefaultValue = "B"
+        };
+
+        yield return new PropertyDescription("ScannerIp", "Scanner IP", "IP của Olei GS1-5")
+        {
+            DataType = "string",
+            IsReadOnly = true,
+            DisplayOrder = 3,
+            Category = "Kết nối",
+            DefaultValue = ""
+        };
+
+        yield return new PropertyDescription("LocalIp", "Local IP", "IP local nhận UDP packet")
+        {
+            DataType = "string",
+            IsReadOnly = true,
+            DisplayOrder = 4,
+            Category = "Kết nối",
+            DefaultValue = ""
+        };
+
+        yield return new PropertyDescription("Port", "Port", "UDP port của lidar")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 5,
+            Category = "Kết nối",
+            DefaultValue = "2368"
+        };
+
+        yield return new PropertyDescription("FrameId", "Frame ID", "Frame ID của LaserScan")
+        {
+            DataType = "string",
+            IsReadOnly = true,
+            DisplayOrder = 6,
+            Category = "Cấu hình",
+            DefaultValue = "olelidar"
+        };
+
+        yield return new PropertyDescription("RangeMin", "Range Min (m)", "Khoảng cách tối thiểu")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 7,
+            Category = "Cấu hình",
+            DefaultValue = "0.08"
+        };
+
+        yield return new PropertyDescription("RangeMax", "Range Max (m)", "Khoảng cách tối đa")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 8,
+            Category = "Cấu hình",
+            DefaultValue = "50.0"
+        };
+
+        yield return new PropertyDescription("ConnectionStatus", "Connection Status", "Trạng thái kết nối lidar")
+        {
+            DataType = "string",
+            IsReadOnly = true,
+            DisplayOrder = 9,
+            Category = "Trạng thái",
+            DefaultValue = "Disconnected"
+        };
+
+        yield return new PropertyDescription("PointCount", "Point Count", "Số điểm scan gần nhất")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 10,
+            Category = "Trạng thái",
+            DefaultValue = "0"
+        };
+
+        yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan gần nhất")
+        {
+            DataType = "string",
+            IsReadOnly = true,
+            DisplayOrder = 11,
+            Category = "Trạng thái",
+            DefaultValue = ""
+        };
+    }
+
+    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            _portOpened = _connection.openPort();
+            if (!_portOpened)
+            {
+                throw new InvalidOperationException(
+                    $"Failed to open Olei GS1-5 UDP port at {_config.LocalIp}:{_config.Port}");
+            }
+        }
+
+        await Task.CompletedTask;
+    }
+
+    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
+    {
+        StartUpdateLoop();
+        SetProperty("ConnectionStatus", "Connected");
+        await Task.CompletedTask;
+    }
+
+    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
+    {
+        StopUpdateLoop();
+        CloseUdpPort();
+        SetProperty("ConnectionStatus", "Disconnected");
+        await Task.CompletedTask;
+    }
+
+    protected override async Task OnResetAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            _lastLaserScan = null;
+            _lastScanDataTimestamp = null;
+            _lastPointCount = 0;
+            _lastScanFrequencyHz = null;
+            _scanAccumulator = null;
+            _lastFirstIndex = null;
+        }
+
+        UpdateProperties();
+        await Task.CompletedTask;
+    }
+
+    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            if (!_portOpened)
+            {
+                return false;
+            }
+
+            if (_updateTask == null || _updateTask.IsCompleted)
+            {
+                return false;
+            }
+
+            if (_lastScanDataTimestamp == null)
+            {
+                return true;
+            }
+
+            return (DateTime.UtcNow - _lastScanDataTimestamp.Value).TotalSeconds < 5.0;
+        }
+    }
+
+    private void StartUpdateLoop()
+    {
+        lock (_dataLock)
+        {
+            if (_updateTask != null && !_updateTask.IsCompleted)
+            {
+                return;
+            }
+
+            _updateCts = new CancellationTokenSource();
+            _updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token), _updateCts.Token);
+        }
+    }
+
+    private void StopUpdateLoop()
+    {
+        Task? updateTask;
+        CancellationTokenSource? cts;
+
+        lock (_dataLock)
+        {
+            updateTask = _updateTask;
+            cts = _updateCts;
+            _updateTask = null;
+            _updateCts = null;
+        }
+
+        try
+        {
+            cts?.Cancel();
+            updateTask?.Wait(TimeSpan.FromSeconds(2));
+        }
+        catch
+        {
+        }
+        finally
+        {
+            cts?.Dispose();
+        }
+    }
+
+    private async Task UpdateLoopAsync(CancellationToken cancellationToken)
+    {
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                _connection.readRawdata();
+
+                var processedAnyPacket = false;
+                while (TryProcessOnePacket())
+                {
+                    processedAnyPacket = true;
+                }
+
+                if (!processedAnyPacket)
+                {
+                    await Task.Delay(5, cancellationToken);
+                }
+            }
+            catch (OperationCanceledException)
+            {
+                break;
+            }
+            catch (Exception ex)
+            {
+                LastError = ex;
+                OnErrorOccurred(ex, "OleiGS15 update loop error");
+                await Task.Delay(100, cancellationToken);
+            }
+        }
+    }
+
+    private bool TryProcessOnePacket()
+    {
+        var packet = _connection.TryDequeuePacket();
+        if (packet == null)
+        {
+            return false;
+        }
+
+        if (!TryParseV3Packet(packet.data, out var header, out var rangesMm, out var intensities, out var hasIntensity))
+        {
+            return true;
+        }
+
+        var packetTimestamp = _config.TimeFromLidar
+            ? (packet.stamp.Kind == DateTimeKind.Utc ? packet.stamp : packet.stamp.ToUniversalTime())
+            : DateTime.UtcNow;
+
+        V3ScanAccumulator? completedScan = null;
+
+        lock (_dataLock)
+        {
+            if (_lastFirstIndex.HasValue && _lastFirstIndex.Value > header.FirstIndex)
+            {
+                _scanAccumulator = null;
+            }
+            _lastFirstIndex = header.FirstIndex;
+
+            if (_scanAccumulator == null)
+            {
+                _scanAccumulator = new V3ScanAccumulator(header, _config);
+            }
+
+            _scanAccumulator.AddPacketData(rangesMm, intensities, hasIntensity, _config);
+
+            if (_scanAccumulator.IsCompleted)
+            {
+                completedScan = _scanAccumulator;
+                _scanAccumulator = null;
+            }
+        }
+
+        if (completedScan != null)
+        {
+            PublishCompletedScan(completedScan, packetTimestamp);
+        }
+
+        return true;
+    }
+
+    private void PublishCompletedScan(V3ScanAccumulator completed, DateTime packetTimestamp)
+    {
+        var scanTime = completed.ScanTimeSeconds > 0 ? completed.ScanTimeSeconds : 0.1f;
+        var scanStamp = packetTimestamp - TimeSpan.FromSeconds(scanTime);
+
+        var laserScan = new LaserScan
+        {
+            Header = new RobotNet10.Shared.Header
+            {
+                Seq = 0,
+                Stamp = scanStamp,
+                FrameId = _config.FrameId
+            },
+            AngleMin = completed.AngleMin,
+            AngleMax = completed.AngleMax,
+            AngleIncrement = completed.AngleIncrement,
+            TimeIncrement = completed.TimeIncrement,
+            ScanTime = scanTime,
+            RangeMin = _config.RangeMin,
+            RangeMax = _config.RangeMax,
+            Ranges = Array.ConvertAll(completed.Ranges, x => (double)x),
+            Intensities = Array.ConvertAll(completed.Intensities, x => (double)x)
+        };
+
+        lock (_dataLock)
+        {
+            _lastLaserScan = laserScan;
+            _lastScanDataTimestamp = scanStamp;
+            _lastPointCount = completed.Ranges.Length;
+            _lastScanFrequencyHz = completed.ScanFrequencyHz > 0 ? completed.ScanFrequencyHz : null;
+        }
+
+        ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
+        UpdateProperties();
+    }
+
+    private static bool TryParseV3Packet(
+        byte[] data,
+        out V3HeaderLite header,
+        out ushort[] rangesMm,
+        out ushort[] intensities,
+        out bool hasIntensity)
+    {
+        header = default;
+        rangesMm = Array.Empty<ushort>();
+        intensities = Array.Empty<ushort>();
+        hasIntensity = false;
+
+        if (data.Length < V3HeaderSize)
+        {
+            return false;
+        }
+
+        var magic = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(0, 2));
+        if (magic != V3Magic)
+        {
+            return false;
+        }
+
+        header = new V3HeaderLite
+        {
+            Magic = magic,
+            Version = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(2, 2)),
+            PacketSize = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(4, 4)),
+            HeaderSize = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(8, 2)),
+            DistanceRatio = data[10],
+            Types = data[11],
+            ScanNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(12, 2)),
+            PacketNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(14, 2)),
+            TimestampDecimal = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(16, 4)),
+            TimestampInteger = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(20, 4)),
+            ScanFrequencyRaw = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(24, 2)),
+            NumPointsScan = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(26, 2)),
+            InputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(28, 2)),
+            OutputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(30, 2)),
+            FieldStatus = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(32, 4)),
+            StartIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(36, 2)),
+            EndIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(38, 2)),
+            FirstIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(40, 2)),
+            NumPointsPacket = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(42, 2)),
+            StatusFlags = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(44, 4))
+        };
+
+        var packetSize = header.PacketSize == 0 ? data.Length : (int)Math.Min(header.PacketSize, data.Length);
+        var headerSize = header.HeaderSize == 0 ? V3HeaderSize : header.HeaderSize;
+        if (headerSize < V3HeaderSize || headerSize > packetSize)
+        {
+            return false;
+        }
+
+        var bytesPerPoint = header.Types switch
+        {
+            0x00 => 2,
+            0x01 => 4,
+            0x10 => 4,
+            _ => 0
+        };
+
+        if (bytesPerPoint == 0)
+        {
+            return false;
+        }
+
+        var payloadBytes = packetSize - headerSize;
+        var pointCount = header.NumPointsPacket;
+        if (pointCount == 0 || pointCount * bytesPerPoint > payloadBytes)
+        {
+            pointCount = (ushort)(payloadBytes / bytesPerPoint);
+        }
+
+        if (pointCount == 0)
+        {
+            return false;
+        }
+
+        rangesMm = new ushort[pointCount];
+
+        if (header.Types == 0x01)
+        {
+            hasIntensity = true;
+            intensities = new ushort[pointCount];
+        }
+
+        var offset = headerSize;
+        for (int i = 0; i < pointCount; i++)
+        {
+            switch (header.Types)
+            {
+                case 0x00:
+                    rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
+                    offset += 2;
+                    break;
+                case 0x01:
+                    rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
+                    intensities[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
+                    offset += 4;
+                    break;
+                case 0x10:
+                    rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
+                    offset += 4;
+                    break;
+            }
+        }
+
+        return true;
+    }
+
+    private void UpdateProperties()
+    {
+        DateTime? timestamp;
+        int pointCount;
+
+        lock (_dataLock)
+        {
+            timestamp = _lastScanDataTimestamp;
+            pointCount = _lastPointCount;
+        }
+
+        SetProperty("Version", _config.Version.ToString());
+        SetProperty("PacketType", _config.PacketType);
+        SetProperty("ScannerIp", _config.ScannerIp);
+        SetProperty("LocalIp", _config.LocalIp);
+        SetProperty("Port", _config.Port.ToString());
+        SetProperty("FrameId", _config.FrameId);
+        SetProperty("RangeMin", _config.RangeMin.ToString("F3"));
+        SetProperty("RangeMax", _config.RangeMax.ToString("F3"));
+        SetProperty("PointCount", pointCount.ToString());
+        SetProperty("LastScanTime", timestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
+    }
+
+    private void CloseUdpPort()
+    {
+        lock (_dataLock)
+        {
+            _portOpened = false;
+
+            try
+            {
+                _connection.udp?.Close();
+            }
+            catch
+            {
+            }
+
+            try
+            {
+                _connection.udp?.Dispose();
+            }
+            catch
+            {
+            }
+        }
+    }
+
+    public LaserScan? CurrentMeasurementData
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                return _lastLaserScan;
+            }
+        }
+    }
+
+    public DateTime? LastScanDataTimestamp
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                return _lastScanDataTimestamp;
+            }
+        }
+    }
+
+    public double MinAngleRad
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                return _lastLaserScan?.AngleMin ?? -Math.PI;
+            }
+        }
+    }
+
+    public double MaxAngleRad
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                return _lastLaserScan?.AngleMax ?? Math.PI;
+            }
+        }
+    }
+
+    public double MinRangeM => _config.RangeMin;
+    public double MaxRangeM => _config.RangeMax;
+
+    public double? AngularResolutionRad
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                var scan = _lastLaserScan;
+                if (scan?.Ranges == null || scan.Value.Ranges.Length < 2)
+                {
+                    return null;
+                }
+
+                return scan.Value.AngleIncrement;
+            }
+        }
+    }
+
+    public double? ScanFrequencyHz
+    {
+        get
+        {
+            lock (_dataLock)
+            {
+                return _lastScanFrequencyHz;
+            }
+        }
+    }
+
+    public double FieldOfViewRad => Math.Abs(MaxAngleRad - MinAngleRad);
+    public bool SupportsIntensity => true;
+    public double? AccuracyM => null;
+
+    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
+
+    protected override void Dispose(bool disposing)
+    {
+        if (disposing)
+        {
+            StopUpdateLoop();
+            CloseUdpPort();
+        }
+
+        base.Dispose(disposing);
+    }
+
+    private readonly struct V3HeaderLite
+    {
+        public ushort Magic { get; init; }
+        public ushort Version { get; init; }
+        public uint PacketSize { get; init; }
+        public ushort HeaderSize { get; init; }
+        public byte DistanceRatio { get; init; }
+        public byte Types { get; init; }
+        public ushort ScanNumber { get; init; }
+        public ushort PacketNumber { get; init; }
+        public uint TimestampDecimal { get; init; }
+        public uint TimestampInteger { get; init; }
+        public ushort ScanFrequencyRaw { get; init; }
+        public ushort NumPointsScan { get; init; }
+        public ushort InputStatus { get; init; }
+        public ushort OutputStatus { get; init; }
+        public uint FieldStatus { get; init; }
+        public ushort StartIndex { get; init; }
+        public ushort EndIndex { get; init; }
+        public ushort FirstIndex { get; init; }
+        public ushort NumPointsPacket { get; init; }
+        public uint StatusFlags { get; init; }
+    }
+
+    private sealed class V3ScanAccumulator
+    {
+        private int _writeIndex;
+
+        public float[] Ranges { get; }
+        public float[] Intensities { get; }
+        public bool IsCompleted => _writeIndex >= Ranges.Length;
+        public float AngleMin { get; }
+        public float AngleMax { get; }
+        public float AngleIncrement { get; }
+        public float TimeIncrement { get; }
+        public float ScanTimeSeconds { get; }
+        public double ScanFrequencyHz { get; }
+
+        public V3ScanAccumulator(V3HeaderLite header, OleiGS15DriverConfig config)
+        {
+            var actualNum = Math.Max(1, (int)(header.EndIndex - header.StartIndex));
+            var pointsPerScan = Math.Max(1, (int)header.NumPointsScan);
+
+            Ranges = new float[actualNum];
+            for (int i = 0; i < Ranges.Length; i++)
+            {
+                Ranges[i] = 0.0f;
+            }
+
+            Intensities = new float[actualNum];
+
+            AngleIncrement = (float)((360.0 / pointsPerScan) * Math.PI / 180.0);
+            AngleMin = (float)(header.StartIndex * AngleIncrement - Math.PI);
+            AngleMax = (float)(header.EndIndex * AngleIncrement - Math.PI);
+
+            var rpm = header.ScanFrequencyRaw & 0x7FFF;
+            ScanFrequencyHz = rpm > 0 ? rpm / 60.0 : 0.0;
+            ScanTimeSeconds = rpm > 0 ? (float)(60.0 / rpm) : 0.1f;
+            TimeIncrement = ScanTimeSeconds / pointsPerScan;
+        }
+
+        public void AddPacketData(ushort[] rangesMm, ushort[] intensities, bool hasIntensity, OleiGS15DriverConfig config)
+        {
+            for (int i = 0; i < rangesMm.Length && _writeIndex < Ranges.Length; i++)
+            {
+                var rangeM = rangesMm[i] / 1000.0f;
+                if (rangeM < config.RangeMin || rangeM > config.RangeMax || rangeM <= 0)
+                {
+                    Ranges[_writeIndex] = 0.0f;
+                }
+                else
+                {
+                    Ranges[_writeIndex] = rangeM;
+                }
+
+                if (hasIntensity && i < intensities.Length)
+                {
+                    Intensities[_writeIndex] = intensities[i];
+                }
+                else
+                {
+                    Intensities[_writeIndex] = 0.0f;
+                }
+
+                _writeIndex++;
+            }
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiLidarDriver.cs

@@ -0,0 +1,390 @@
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.Shared.Sensor;
+
+namespace RobotNet10.RobotApp.Drivers.Lidar;
+
+/// <summary>
+/// Olei 2D LiDAR driver (LR-1BS5 / olelidar ROS protocol, 1240-byte UDP packets).
+/// </summary>
+[Device(DeviceType.Lidar, "Olei", "OleiLidarDriver", "1.0.0")]
+public class OleiLidarDriver : DeviceBase, ILidar
+{
+    private readonly Connect _connect = new();
+    private readonly DecodeLidar _decode = new();
+
+    private readonly double _minRangeM;
+    private readonly double _maxRangeM;
+    private readonly double? _angularResolutionRad;
+    private readonly double? _scanFrequencyHz;
+    private readonly bool _supportsIntensity;
+    private readonly double? _accuracyM;
+    private readonly double _startAngleRad;
+    private readonly double _endAngleRad;
+    private readonly string _frameId;
+    private readonly int _angleMinHundredths;
+    private readonly int _angleMaxHundredths;
+    private readonly int _poly;
+
+    private DateTime? _lastScanDataTimestamp;
+    private LaserScan? _lastLaserScan;
+    private bool _portOpened;
+
+    private readonly Lock _dataLock = new();
+    private CancellationTokenSource? _updateCts;
+    private Task? _updateTask;
+    private uint _scanSequence;
+
+    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
+
+    public OleiLidarDriver(string deviceId, string deviceName, IConfigurationSection connection)
+        : base(deviceId, deviceName, DeviceType.Lidar)
+    {
+        _connect.device_IP = connection.GetValue<string>("DeviceIp") ?? "192.168.254.13";
+        _connect.device_port = connection.GetValue<int?>("DevicePort")
+            ?? connection.GetValue<int?>("UdpPort")
+            ?? 2368;
+        _connect.local_ip = connection.GetValue<string>("LocalIp") ?? string.Empty;
+        _connect.multiaddr_ip = connection.GetValue<string>("Multiaddr") ?? string.Empty;
+
+        var minAngleDeg = connection.GetValue<double?>("MinAngle") ?? -135.0;
+        var maxAngleDeg = connection.GetValue<double?>("MaxAngle") ?? 135.0;
+        _startAngleRad = minAngleDeg * Math.PI / 180.0;
+        _endAngleRad = maxAngleDeg * Math.PI / 180.0;
+
+        _angleMinHundredths = (int)(minAngleDeg * 100 + 18000);
+        _angleMaxHundredths = (int)(maxAngleDeg * 100 + 18000);
+
+        _minRangeM = connection.GetValue<double?>("MinRangeM") ?? 0.2;
+        _maxRangeM = connection.GetValue<double?>("MaxRangeM") ?? 50.0;
+        _scanFrequencyHz = connection.GetValue<double?>("ScanFrequencyHz");
+        _supportsIntensity = connection.GetValue<bool?>("SupportsIntensity") ?? true;
+        _accuracyM = connection.GetValue<double?>("AccuracyM") ?? 0.02;
+        _frameId = connection.GetValue<string>("FrameId") ?? "olei_lidar_frame";
+        _poly = connection.GetValue<int?>("Poly") ?? 1;
+
+        var stepDeg = connection.GetValue<double?>("StepDeg") ?? 0.225;
+        _angularResolutionRad = stepDeg * Math.PI / 180.0;
+
+        var decoderConfig = new DecoderConfig
+        {
+            AngleMin = minAngleDeg,
+            AngleMax = maxAngleDeg,
+            RangeMin = _minRangeM,
+            RangeMax = _maxRangeM,
+            Poly = _poly,
+            Inverted = connection.GetValue<bool?>("Inverted") ?? false,
+            StepDeg = stepDeg,
+            FrameId = _frameId
+        };
+        _decode.SetConfig(decoderConfig);
+
+        if (connection.GetValue<bool?>("AutoReconnectEnabled") is bool autoReconnect)
+            AutoReconnectEnabled = autoReconnect;
+        else
+            AutoReconnectEnabled = true;
+
+        if (connection.GetValue<int?>("ReconnectDelayMs") is int reconnectDelay)
+            ReconnectDelayMs = reconnectDelay;
+
+        if (connection.GetValue<int?>("MaxReconnectAttempts") is int maxAttempts)
+            MaxReconnectAttempts = maxAttempts;
+
+        UpdateProperties();
+    }
+
+    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
+    {
+        yield return new PropertyDescription("NumberOfPoints", "Number of Points", "Số điểm scan")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 1,
+            Category = "Cấu hình",
+            DefaultValue = "0"
+        };
+
+        yield return new PropertyDescription("StartAngle", "Start Angle (deg)", "Góc bắt đầu (độ)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 2,
+            Category = "Cấu hình",
+            DefaultValue = (_startAngleRad * 180.0 / Math.PI).ToString("F1")
+        };
+
+        yield return new PropertyDescription("EndAngle", "End Angle (deg)", "Góc kết thúc (độ)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 3,
+            Category = "Cấu hình",
+            DefaultValue = (_endAngleRad * 180.0 / Math.PI).ToString("F1")
+        };
+
+        yield return new PropertyDescription("MaxRange", "Max Range (m)", "Tầm quét tối đa (m)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 4,
+            Category = "Cấu hình",
+            DefaultValue = _maxRangeM.ToString("F1")
+        };
+
+        yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan cuối cùng")
+        {
+            DataType = "string",
+            IsReadOnly = true,
+            DisplayOrder = 5,
+            Category = "Trạng thái",
+            DefaultValue = ""
+        };
+    }
+
+    protected override Task OnInitializeAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            _portOpened = _connect.openPort();
+            if (!_portOpened)
+                throw new InvalidOperationException(
+                    $"Failed to open Olei UDP port {_connect.device_port} on local {_connect.local_ip}");
+        }
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnConnectAsync(CancellationToken cancellationToken)
+    {
+        StartUpdateLoop();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
+    {
+        StopUpdateLoop();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnResetAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            _lastLaserScan = null;
+            _lastScanDataTimestamp = null;
+            _scanSequence = 0;
+        }
+        UpdateProperties();
+        return Task.CompletedTask;
+    }
+
+    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+    {
+        if (!_portOpened || !_connect.IsOpen)
+            return false;
+
+        // LiDAR may need a few seconds after motor start before UDP data arrives.
+        for (var i = 0; i < 40; i++)
+        {
+            if (_lastScanDataTimestamp.HasValue)
+                return true;
+
+            if (_connect.TotalPacketsReceived > 0 || _connect.GetPacketCount() > 0)
+                return true;
+
+            await Task.Delay(250, cancellationToken);
+        }
+
+        Console.WriteLine(
+            $"[WARN] Olei LiDAR connection check failed: packets={_connect.TotalPacketsReceived}, " +
+            $"queued={_connect.GetPacketCount()}, wrongSrc={_connect.DroppedWrongSource}, " +
+            $"wrongSize={_connect.DroppedWrongSize}, device={_connect.device_IP}:{_connect.device_port}");
+
+        return false;
+    }
+
+    private void StartUpdateLoop()
+    {
+        lock (_dataLock)
+        {
+            if (_updateCts != null)
+                return;
+
+            _updateCts = new CancellationTokenSource();
+            _updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token));
+        }
+    }
+
+    private void StopUpdateLoop()
+    {
+        CancellationTokenSource? cts;
+        lock (_dataLock)
+        {
+            cts = _updateCts;
+            _updateCts = null;
+            _updateTask = null;
+        }
+        cts?.Cancel();
+        cts?.Dispose();
+    }
+
+    private async Task UpdateLoopAsync(CancellationToken cancellationToken)
+    {
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                var framePublished = false;
+                while (_connect.TryDequeuePacket() is { } pkt)
+                {
+                    if (_decode.PacketCb(pkt))
+                        framePublished |= TryPublishScan(pkt.stamp);
+                }
+
+                if (!framePublished)
+                    await Task.Delay(1, cancellationToken);
+            }
+            catch (OperationCanceledException)
+            {
+                break;
+            }
+            catch (Exception ex)
+            {
+                LastError = ex;
+                OnErrorOccurred(ex, "Olei LiDAR update loop error");
+                await Task.Delay(500, cancellationToken);
+            }
+        }
+    }
+
+    private bool TryPublishScan(DateTime packetReceiveStamp)
+    {
+        var angles = _decode.scanAngleInVec;
+        var rangesMm = _decode.scanRangeInVec;
+        var intensitiesRaw = _decode.scanIntensityInVec;
+
+        if (angles.Count == 0 ||
+            angles.Count != rangesMm.Count ||
+            angles.Count != intensitiesRaw.Count)
+            return false;
+
+        var rangeList = new List<double>();
+        var intensityList = new List<double>();
+        var angleRadList = new List<double>();
+
+        for (var i = 0; i < angles.Count; i++)
+        {
+            if (i % _poly != 0)
+                continue;
+
+            var angleHundredths = angles[i];
+            if (angleHundredths < _angleMinHundredths || angleHundredths > _angleMaxHundredths)
+                continue;
+
+            var angleDeg = (angleHundredths - 18000) / 100.0;
+            var angleRad = angleDeg * Math.PI / 180.0;
+            angleRadList.Add(angleRad);
+
+            var distanceM = rangesMm[i] * OleiConstants.DistanceResolution;
+            var isValid = distanceM >= _minRangeM && distanceM <= _maxRangeM;
+            rangeList.Add(isValid ? distanceM : double.NaN);
+            intensityList.Add(intensitiesRaw[i]);
+        }
+
+        if (rangeList.Count == 0)
+            return false;
+
+        var frequency = _decode.Frequency > 0.001f
+            ? _decode.Frequency
+            : (float)(_scanFrequencyHz ?? 10.0);
+        var scanTime = 1.0 / frequency;
+        var numberOfPoints = rangeList.Count;
+
+        var angleMin = angleRadList[0];
+        var angleMax = angleRadList[^1];
+        var angleIncrement = numberOfPoints > 1
+            ? (angleMax - angleMin) / (numberOfPoints - 1)
+            : _decode.StepDeg * Math.PI / 180.0;
+
+        var receiveUtc = packetReceiveStamp.Kind == DateTimeKind.Utc
+            ? packetReceiveStamp
+            : packetReceiveStamp.ToUniversalTime();
+        var lidarEndUtc = _decode.LastCompletedScanLidarEndUtc ?? receiveUtc;
+        var scanStamp = lidarEndUtc - TimeSpan.FromSeconds(scanTime);
+
+        var laserScan = new LaserScan
+        {
+            Header = new RobotNet10.Shared.Header
+            {
+                Seq = _scanSequence++,
+                Stamp = scanStamp,
+                FrameId = _frameId
+            },
+            AngleMin = angleMin,
+            AngleMax = angleMax,
+            AngleIncrement = angleIncrement,
+            TimeIncrement = scanTime / numberOfPoints,
+            ScanTime = scanTime,
+            RangeMin = (float)_minRangeM,
+            RangeMax = (float)_maxRangeM,
+            Ranges = rangeList.ToArray(),
+            Intensities = intensityList.ToArray()
+        };
+
+        lock (_dataLock)
+        {
+            _lastLaserScan = laserScan;
+            _lastScanDataTimestamp = scanStamp;
+        }
+
+        ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
+        UpdateProperties();
+        return true;
+    }
+
+    private void UpdateProperties()
+    {
+        lock (_dataLock)
+        {
+            var pointCount = _lastLaserScan?.Ranges.Length ?? 0;
+            SetProperty("NumberOfPoints", pointCount.ToString());
+            SetProperty("StartAngle", (_startAngleRad * 180.0 / Math.PI).ToString("F1"));
+            SetProperty("EndAngle", (_endAngleRad * 180.0 / Math.PI).ToString("F1"));
+            SetProperty("MaxRange", _maxRangeM.ToString("F1"));
+            SetProperty("LastScanTime", _lastScanDataTimestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
+        }
+    }
+
+    #region ILidar
+
+    public LaserScan? CurrentMeasurementData
+    {
+        get { lock (_dataLock) { return _lastLaserScan; } }
+    }
+
+    public DateTime? LastScanDataTimestamp
+    {
+        get { lock (_dataLock) { return _lastScanDataTimestamp; } }
+    }
+
+    public double MinAngleRad => _startAngleRad;
+    public double MaxAngleRad => _endAngleRad;
+    public double MinRangeM => _minRangeM;
+    public double MaxRangeM => _maxRangeM;
+    public double? AngularResolutionRad => _angularResolutionRad;
+    public double? ScanFrequencyHz => _decode.Frequency > 0.001f ? _decode.Frequency : _scanFrequencyHz;
+    public double FieldOfViewRad => Math.Abs(_endAngleRad - _startAngleRad);
+    public bool SupportsIntensity => _supportsIntensity;
+    public double? AccuracyM => _accuracyM;
+
+    #endregion
+
+    protected override void Dispose(bool disposing)
+    {
+        if (disposing)
+        {
+            StopUpdateLoop();
+            _connect.Dispose();
+        }
+        base.Dispose(disposing);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiLidarHandler.cs

@@ -0,0 +1,437 @@
+using System.Buffers.Binary;
+using System.Net;
+using System.Net.Sockets;
+
+namespace RobotNet10.RobotApp.Drivers.Lidar;
+
+/// <summary>
+/// UDP transport + packet decode aligned with olelidar ROS driver (driver.cpp / decoder.cpp).
+/// </summary>
+public sealed class Connect : IDisposable
+{
+    public const int kPacketSize = OleiConstants.PacketSize;
+
+    public string device_IP { get; set; } = "192.168.254.13";
+    public int device_port { get; set; } = 2368;
+    public string local_ip { get; set; } = "192.168.254.10";
+    public string multiaddr_ip { get; set; } = string.Empty;
+
+    private readonly Queue<oleiPackage> _packetList = new();
+    private readonly object _packetListLock = new();
+
+    private UdpClient? _udp;
+    private Thread? _receiveThread;
+    private volatile bool _running;
+    private IPAddress? _deviceIpAddress;
+
+    private long _totalPacketsReceived;
+    private long _droppedWrongSource;
+    private long _droppedWrongSize;
+
+    public long TotalPacketsReceived => Interlocked.Read(ref _totalPacketsReceived);
+    public long DroppedWrongSource => Interlocked.Read(ref _droppedWrongSource);
+    public long DroppedWrongSize => Interlocked.Read(ref _droppedWrongSize);
+
+    public bool IsOpen => _udp != null && _running;
+
+    public bool openPort()
+    {
+        try
+        {
+            _deviceIpAddress = IPAddress.Parse(device_IP).MapToIPv4();
+
+            var bindAddress = string.IsNullOrWhiteSpace(local_ip)
+                ? IPAddress.Any
+                : IPAddress.Parse(local_ip).MapToIPv4();
+            var bindEp = new IPEndPoint(bindAddress, device_port);
+
+            _udp = new UdpClient();
+            _udp.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
+            _udp.Client.Bind(bindEp);
+            _udp.Client.ReceiveTimeout = 500;
+
+            if (!string.IsNullOrWhiteSpace(multiaddr_ip))
+            {
+                try
+                {
+                    _udp.JoinMulticastGroup(IPAddress.Parse(multiaddr_ip));
+                }
+                catch (Exception ex)
+                {
+                    Console.WriteLine($"[WARN] Olei multicast join failed ({multiaddr_ip}): {ex.Message}");
+                }
+            }
+
+            StartReceiveLoop();
+            Console.WriteLine($"[INFO] Olei UDP listening on {bindEp.Address}:{bindEp.Port}, device {device_IP}");
+            return true;
+        }
+        catch (Exception ex)
+        {
+            Console.WriteLine($"[ERROR] Cannot open Olei UDP port: {ex.Message}");
+            return false;
+        }
+    }
+
+    public void closePort()
+    {
+        _running = false;
+        try
+        {
+            _receiveThread?.Join(TimeSpan.FromSeconds(2));
+        }
+        catch { /* ignore */ }
+
+        _receiveThread = null;
+        _udp?.Close();
+        _udp?.Dispose();
+        _udp = null;
+    }
+
+    private void StartReceiveLoop()
+    {
+        _running = true;
+        _receiveThread = new Thread(ReceiveLoop)
+        {
+            IsBackground = true,
+            Name = $"OleiLidar-UDP-{device_port}",
+            Priority = ThreadPriority.Highest
+        };
+        _receiveThread.Start();
+    }
+
+    private void ReceiveLoop()
+    {
+        while (_running && _udp != null)
+        {
+            try
+            {
+                var remote = new IPEndPoint(IPAddress.Any, 0);
+                var data = _udp.Receive(ref remote);
+
+                var senderIp = remote.Address.MapToIPv4();
+                if (_deviceIpAddress != null && !senderIp.Equals(_deviceIpAddress))
+                {
+                    Interlocked.Increment(ref _droppedWrongSource);
+                    continue;
+                }
+
+                if (data.Length < kPacketSize)
+                {
+                    Interlocked.Increment(ref _droppedWrongSize);
+                    continue;
+                }
+
+                Interlocked.Increment(ref _totalPacketsReceived);
+
+                var packet = new oleiPackage
+                {
+                    stamp = DateTime.UtcNow,
+                    data = new byte[kPacketSize]
+                };
+                Array.Copy(data, packet.data, kPacketSize);
+
+                lock (_packetListLock)
+                {
+                    _packetList.Enqueue(packet);
+                }
+            }
+            catch (SocketException ex) when (ex.SocketErrorCode == SocketError.TimedOut)
+            {
+                // Normal when no packets (ROS poll timeout).
+            }
+            catch (SocketException ex) when (!_running)
+            {
+                break;
+            }
+            catch (ObjectDisposedException)
+            {
+                break;
+            }
+            catch (Exception ex)
+            {
+                if (_running)
+                    Console.WriteLine($"[ERROR] Olei UDP receive: {ex.Message}");
+            }
+        }
+    }
+
+    public int GetPacketCount()
+    {
+        lock (_packetListLock)
+            return _packetList.Count;
+    }
+
+    public oleiPackage? TryDequeuePacket()
+    {
+        lock (_packetListLock)
+            return _packetList.Count == 0 ? null : _packetList.Dequeue();
+    }
+
+    public void Dispose() => closePort();
+}
+
+public sealed class oleiPackage
+{
+    public DateTime stamp { get; set; }
+    public byte[] data { get; set; } = new byte[OleiConstants.PacketSize];
+}
+
+public sealed class DecoderConfig
+{
+    public double AngleMin { get; set; } = 0.0;
+    public double AngleMax { get; set; } = 360.0;
+    public double RangeMin { get; set; } = 0.2;
+    public double RangeMax { get; set; } = 30.0;
+    public int Poly { get; set; } = 1;
+    public bool Inverted { get; set; }
+    public double StepDeg { get; set; } = 0.225;
+    public string FrameId { get; set; } = "olelidar";
+}
+
+public static class OleiConstants
+{
+    public const int DataHeadSize = 40;
+    public const int PointBytes = 8;
+    public const int BlocksPerPacket = 150;
+    public const int PacketSize = DataHeadSize + BlocksPerPacket * PointBytes;
+    public const float DistanceResolution = 0.001f;
+    public const float AzimuthResolutionDeg = 0.01f;
+}
+
+public readonly struct DataPoint
+{
+    public readonly ushort Azimuth;
+    public readonly ushort Distance;
+    public readonly ushort Reflectivity;
+
+    public DataPoint(ReadOnlySpan<byte> span)
+    {
+        Azimuth = BinaryPrimitives.ReadUInt16LittleEndian(span);
+        Distance = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(2));
+        Reflectivity = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(4));
+    }
+}
+
+public readonly struct DataBlock
+{
+    public readonly DataPoint Point;
+    public DataBlock(ReadOnlySpan<byte> span) => Point = new DataPoint(span);
+}
+
+public readonly struct DataHeader
+{
+    public readonly byte[] Code;
+    public readonly uint Timestamp;
+    public readonly ushort Rpm;
+    public readonly uint Rsv;
+
+    public DataHeader(ReadOnlySpan<byte> span)
+    {
+        Code = span.Slice(22, 2).ToArray();
+        Timestamp = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(28));
+        Rpm = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(32));
+        Rsv = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(36));
+    }
+}
+
+public readonly struct Packet
+{
+    public readonly DataHeader Header;
+    public readonly DataBlock[] Blocks;
+
+    public Packet(ReadOnlySpan<byte> span)
+    {
+        if (span.Length < OleiConstants.PacketSize)
+            throw new ArgumentException($"packet must be {OleiConstants.PacketSize} bytes");
+
+        Header = new DataHeader(span.Slice(0, OleiConstants.DataHeadSize));
+        Blocks = new DataBlock[OleiConstants.BlocksPerPacket];
+        var offset = OleiConstants.DataHeadSize;
+        for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
+        {
+            Blocks[i] = new DataBlock(span.Slice(offset, OleiConstants.PointBytes));
+            offset += OleiConstants.PointBytes;
+        }
+    }
+}
+
+/// <summary>
+/// Packet decoder — port of olelidar/src/decoder.cpp PacketCb / DecodeAndFill / PublishMsg.
+/// </summary>
+public sealed class DecodeLidar
+{
+    public readonly List<ushort> scanAngleInVec = new();
+    public readonly List<ushort> scanRangeInVec = new();
+    public readonly List<ushort> scanIntensityInVec = new();
+
+    private readonly List<ushort> _scanAngleVec = new();
+    private readonly List<ushort> _scanRangeVec = new();
+    private readonly List<ushort> _scanIntensityVec = new();
+
+    public ushort AzimuthLast { get; private set; }
+    public ushort AzimuthNow { get; private set; }
+    public ushort AzimuthFirst { get; private set; } = 0xFFFF;
+
+    private DateTime _machineTimeBase;
+    private uint _innerTimestampBaseMs;
+    private bool _isTimeBase;
+    private uint _lastStampMs;
+
+    /// <summary>End-of-scan time mapped from lidar internal clock (UTC).</summary>
+    public DateTime? LastCompletedScanLidarEndUtc { get; private set; }
+
+    public float Frequency { get; private set; }
+    public byte LidarType { get; private set; } = 0x01;
+    public int Direction { get; private set; }
+    public double StepDeg { get; private set; } = 0.225;
+
+    private DecoderConfig _config = new();
+    private readonly object _locker = new();
+
+    public void SetConfig(DecoderConfig config)
+    {
+        _config = config;
+        StepDeg = config.StepDeg;
+    }
+
+    public bool PacketCb(oleiPackage dataMsg)
+    {
+        if (dataMsg.data.Length < OleiConstants.PacketSize)
+            return false;
+
+        var pkt = new Packet(dataMsg.data);
+        AzimuthNow = pkt.Blocks[0].Point.Azimuth;
+
+        if (AzimuthFirst == 0xFFFF)
+        {
+            LidarType = pkt.Header.Code.Length > 1 ? pkt.Header.Code[1] : (byte)0x01;
+            AzimuthFirst = AzimuthNow;
+            var rpm = pkt.Header.Rpm & 0x7FFF;
+            Direction = pkt.Header.Rpm >> 15;
+            if (_config.Inverted)
+                Direction = 1 - Direction;
+
+            if (Frequency < 0.001f && rpm > 0)
+            {
+                Frequency = rpm / 60.0f;
+                if (LidarType == 0x01)
+                    StepDeg = 0.225;
+            }
+        }
+
+        if (AzimuthLast < AzimuthNow)
+        {
+            DecodeAndFill(pkt);
+            AzimuthLast = AzimuthNow;
+            return false;
+        }
+
+        AzimuthLast = AzimuthNow;
+
+        if (AzimuthFirst >= 200)
+        {
+            AzimuthFirst = AzimuthNow;
+            return false;
+        }
+
+        var nowStampMs = pkt.Header.Timestamp;
+        if (!_isTimeBase)
+        {
+            _machineTimeBase = dataMsg.stamp.Kind == DateTimeKind.Utc
+                ? dataMsg.stamp
+                : dataMsg.stamp.ToUniversalTime();
+            _innerTimestampBaseMs = nowStampMs;
+            _isTimeBase = true;
+        }
+
+        _lastStampMs = nowStampMs;
+        LastCompletedScanLidarEndUtc = ComputeLidarTimeUtc(nowStampMs, dataMsg.stamp);
+
+        if (Frequency < 0.001f)
+        {
+            if (LidarType == 0x01)
+            {
+                var rpm = pkt.Header.Rpm & 0x7FFF;
+                Frequency = rpm / 60.0f;
+                StepDeg = 0.225;
+            }
+            else if (_scanAngleVec.Count > 2)
+            {
+                StepDeg = (_scanAngleVec[1] - _scanAngleVec[0]) / 100.0;
+                Frequency = (float)(StepDeg * 10000.0 / 60.0);
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        lock (_locker)
+        {
+            scanAngleInVec.Clear();
+            scanRangeInVec.Clear();
+            scanIntensityInVec.Clear();
+
+            scanAngleInVec.AddRange(_scanAngleVec);
+            scanRangeInVec.AddRange(_scanRangeVec);
+            scanIntensityInVec.AddRange(_scanIntensityVec);
+
+            if (Direction == 0)
+            {
+                scanRangeInVec.Reverse();
+                scanIntensityInVec.Reverse();
+            }
+
+            _scanAngleVec.Clear();
+            _scanRangeVec.Clear();
+            _scanIntensityVec.Clear();
+        }
+
+        DecodeAndFill(pkt);
+        return scanAngleInVec.Count > 0;
+    }
+
+    private DateTime ComputeLidarTimeUtc(uint lidarTimestampMs, DateTime receiveStamp)
+    {
+        var receiveUtc = receiveStamp.Kind == DateTimeKind.Utc
+            ? receiveStamp
+            : receiveStamp.ToUniversalTime();
+
+        if (!_isTimeBase)
+            return receiveUtc;
+
+        var deltaMs = lidarTimestampMs - _innerTimestampBaseMs;
+        return _machineTimeBase.AddMilliseconds(deltaMs);
+    }
+
+    private void DecodeAndFill(Packet pkt)
+    {
+        var rangeMaxMm = (ushort)(_config.RangeMax * 1000);
+        var rangeMinMm = (ushort)(_config.RangeMin * 1000);
+
+        for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
+        {
+            var dp = pkt.Blocks[i].Point;
+            var azimuth = dp.Azimuth;
+            var range = dp.Distance;
+            var intensity = dp.Reflectivity;
+
+            if (range > rangeMaxMm || range < rangeMinMm)
+            {
+                range = 0;
+                intensity = 0;
+            }
+
+            if (azimuth < 0xFF00)
+            {
+                lock (_locker)
+                {
+                    _scanAngleVec.Add(azimuth);
+                    _scanRangeVec.Add(range);
+                    _scanIntensityVec.Add(intensity);
+                }
+            }
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/PhenikaaX/CiA402Helper.cs

@@ -0,0 +1,165 @@
+namespace RobotNet10.RobotApp.Drivers.PhenikaaX;
+
+public static class CiA402Helper
+{
+
+    /// <summary>
+    /// Extract value từ PDO data theo bit offset và bit length
+    /// </summary>
+    public static byte[] ExtractValueFromPdoData(byte[] pdoData, int bitOffset, byte bitLength)
+    {
+        int byteOffset = bitOffset / 8;
+        int bitOffsetInByte = bitOffset % 8;
+        int byteLength = (bitLength + 7) / 8; // Round up
+
+        if (byteOffset + byteLength > pdoData.Length)
+        {
+            throw new ArgumentException($"PDO data too short for extraction at bit offset {bitOffset}, length {bitLength}");
+        }
+
+        byte[] result = new byte[byteLength];
+
+        if (bitOffsetInByte == 0 && bitLength % 8 == 0)
+        {
+            // Aligned extraction - simple copy
+            Array.Copy(pdoData, byteOffset, result, 0, byteLength);
+        }
+        else
+        {
+            // Unaligned extraction - need bit manipulation
+            ulong value = 0;
+            int bitsRead = 0;
+            int currentByteOffset = byteOffset;
+            int currentBitOffset = bitOffsetInByte;
+
+            while (bitsRead < bitLength && currentByteOffset < pdoData.Length)
+            {
+                int bitsToRead = Math.Min(8 - currentBitOffset, bitLength - bitsRead);
+                byte mask = (byte)((1 << bitsToRead) - 1);
+                byte byteValue = (byte)((pdoData[currentByteOffset] >> currentBitOffset) & mask);
+
+                value |= (ulong)byteValue << bitsRead;
+
+                bitsRead += bitsToRead;
+                currentByteOffset++;
+                currentBitOffset = 0;
+            }
+
+            // Convert ulong to byte array
+            for (int i = 0; i < byteLength; i++)
+            {
+                result[i] = (byte)(value >> (i * 8));
+            }
+        }
+
+        return result;
+    }
+
+
+    /// <summary>
+    /// Parse object index từ string (hỗ trợ hex format: 0x6040 hoặc decimal: 24640)
+    /// </summary>
+    public static bool TryParseObjectIndex(string indexStr, out ushort index)
+    {
+        index = 0;
+        if (string.IsNullOrWhiteSpace(indexStr))
+            return false;
+
+        indexStr = indexStr.Trim();
+        if (indexStr.StartsWith("0x", StringComparison.OrdinalIgnoreCase))
+        {
+            return ushort.TryParse(indexStr.AsSpan(2), System.Globalization.NumberStyles.HexNumber, null, out index);
+        }
+        else
+        {
+            return ushort.TryParse(indexStr, out index);
+        }
+    }
+
+    /// <summary>
+    /// Convert bytes to value type với sign extension nếu cần
+    /// </summary>
+    public static T ConvertBytesToValue<T>(byte[] bytes, byte bitLength) where T : struct
+    {
+        ulong value = 0;
+        for (int i = 0; i < Math.Min(bytes.Length, 8); i++)
+        {
+            value |= (ulong)bytes[i] << (i * 8);
+        }
+
+        var type = typeof(T);
+        if (type == typeof(ushort))
+            return (T)(object)(ushort)value;
+        if (type == typeof(short))
+        {
+            if (bitLength < 16 && (value & (1UL << (bitLength - 1))) != 0)
+                value |= 0xFFFFUL << bitLength;
+            return (T)(object)(short)value;
+        }
+        if (type == typeof(uint))
+            return (T)(object)(uint)value;
+        if (type == typeof(int))
+        {
+            if (bitLength < 32 && (value & (1UL << (bitLength - 1))) != 0)
+                value |= 0xFFFFFFFFUL << bitLength;
+            return (T)(object)(int)value;
+        }
+        if (type == typeof(byte))
+            return (T)(object)(byte)value;
+        if (type == typeof(sbyte))
+        {
+            if (bitLength < 8 && (value & (1UL << (bitLength - 1))) != 0)
+                value |= 0xFFUL << bitLength;
+            return (T)(object)(sbyte)value;
+        }
+
+        throw new NotSupportedException($"Type {type.Name} is not supported");
+    }
+
+    /// <summary>
+    /// Convert value to bytes theo bitLength được map trong PDO
+    /// Mục đích của bitLength:
+    /// 1. Truncate/mask giá trị nếu vượt quá số bits được map
+    /// 2. Đảm bảo chỉ gửi đúng số bytes cần thiết
+    /// 3. Xử lý các trường hợp partial mapping (ví dụ: map 8 bits của ushort 16 bits)
+    /// </summary>
+    public static byte[] ConvertValueToBytes<T>(T value, byte bitLength) where T : struct
+    {
+        var type = typeof(T);
+        int byteLength = (bitLength + 7) / 8; // Round up to nearest byte
+
+        // Convert value to ulong để xử lý mask
+        ulong rawValue = 0;
+        if (type == typeof(ushort))
+            rawValue = (ushort)(object)value!;
+        else if (type == typeof(short))
+            rawValue = (ulong)(ushort)(short)(object)value!; // Convert signed to unsigned
+        else if (type == typeof(uint))
+            rawValue = (uint)(object)value!;
+        else if (type == typeof(int))
+            rawValue = (ulong)(uint)(int)(object)value!; // Convert signed to unsigned
+        else if (type == typeof(byte))
+            rawValue = (byte)(object)value!;
+        else if (type == typeof(sbyte))
+            rawValue = (ulong)(byte)(sbyte)(object)value!;
+        else
+            throw new NotSupportedException($"Type {type.Name} is not supported");
+
+        // Mask để chỉ lấy số bits được map
+        if (bitLength < 64)
+        {
+            ulong mask = (1UL << bitLength) - 1;
+            rawValue &= mask;
+        }
+
+        // Convert to byte array với đúng số bytes
+        byte[] result = new byte[byteLength];
+        for (int i = 0; i < byteLength && i < 8; i++)
+        {
+            result[i] = (byte)(rawValue >> (i * 8));
+        }
+
+        return result;
+    }
+
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Sick/README.md

@@ -0,0 +1,3 @@
+1. In ra log LaserScan
+    $./run-quiet.sh
+    $./run.sh 2>&1 | grep -A 30 "===== LaserScan"

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Tada/TadaBattery.cs

@@ -0,0 +1,428 @@
+using System.Timers;
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.Shared.Sensor;
+using RobotNet10.Shared;
+
+namespace RobotNet10.RobotApp.Drivers.Tada;
+
+/// <summary>
+/// BMU driver cho TADA RS485
+/// </summary>
+[Device(DeviceType.Battery, "Tada", "TadaBattery", "1.0.0", Description = "Battery Tada Driver")]
+public class TadaBattery : DeviceBase, IBattery
+{
+    private readonly ILogger<TadaBattery> _logger;
+    private readonly Lock _dataLock = new();
+
+    private TadaRs485Client? _client;
+
+    // Polling
+    private System.Timers.Timer? _pollingTimer;
+
+    // Cached (THEO TÀI LIỆU RS485)
+    private double _cachedChargeLevel;         // SOC %
+    private double _cachedVoltage;             // V
+    private double _cachedCurrent;             // A
+    private bool _cachedCharging;              // Current > 0
+
+    private double? _cachedHealth;             // SOH %
+    private double? _cachedTemperature;        // Celsius
+    private double? _cachedRemainingCapacity;  // Ah
+    private double? _cachedFullCapacity;       // Wh  (RemainEnergy)
+
+    private int? _cachedChargeTime;            // minutes (time to full)
+    private int? _cachedDischargeTime;         // minutes (time to empty)
+    private int? _cachedStatusRaw;             // raw bit flags
+
+    private DateTime _lastUpdateTime = DateTime.MinValue;
+    private BatteryState? _cachedBatteryState;
+
+    // IBattery Implementation
+    public BatteryState? CurrentBatteryState
+    {
+        get { lock (_dataLock) { return _cachedBatteryState; } }
+    }
+
+    private readonly string _portName;
+    private readonly int _baud;
+
+    public TadaBattery(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
+        : base(deviceId, deviceName, DeviceType.Battery)
+    {
+        
+        var loggerFactory = serviceProvider.GetRequiredService<ILoggerFactory>();
+        _logger = loggerFactory.CreateLogger<TadaBattery>();
+
+        _portName = connection.GetValue<string>("Port") ?? throw new Exception("Port is required");
+        _baud = connection.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
+
+        AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true;
+        ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 2000;
+        MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0;
+
+        UpdateProperties();
+    }
+
+    // DeviceBase overrides
+    protected override Task OnInitializeAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            _client?.Dispose();
+            _client = new TadaRs485Client(_logger, _portName, _baud);
+        }
+        UpdateProperties();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnConnectAsync(CancellationToken cancellationToken)
+    {
+        StartPollingLoop();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
+    {
+        StopPollingLoop();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnResetAsync(CancellationToken cancellationToken)
+    {
+        StopPollingLoop();
+        lock (_dataLock)
+        {
+            _client?.ForceReconnect(); // now exists
+            ResetCache();
+        }
+        UpdateProperties();
+        StartPollingLoop();
+        return Task.CompletedTask;
+    }
+
+    protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+            return Task.FromResult(_client != null && !_client.IsFaulted);
+    }
+
+    // Polling
+    private void StartPollingLoop()
+    {
+        lock (_dataLock)
+        {
+            if (_pollingTimer != null && _pollingTimer.Enabled)
+                return;
+
+            StopPollingLoop(); // Đảm bảo không có timer nào đang chạy
+
+            _pollingTimer = new System.Timers.Timer(500) // 2Hz = 500ms interval
+            {
+                AutoReset = true,
+                Enabled = true
+            };
+            _pollingTimer.Elapsed += PollTimer_Elapsed;
+        }
+    }
+
+    private void StopPollingLoop()
+    {
+        lock (_dataLock)
+        {
+            if (_pollingTimer != null)
+            {
+                _pollingTimer.Stop();
+                _pollingTimer.Elapsed -= PollTimer_Elapsed;
+                _pollingTimer.Dispose();
+                _pollingTimer = null;
+            }
+        }
+    }
+
+    private void PollTimer_Elapsed(object? sender, ElapsedEventArgs e)
+    {
+        try
+        {
+            _client?.RequestStatus();
+            var data = _client?.ReadResponse();
+            if (data != null)
+                UpdateCache(data);
+        }
+        catch (Exception ex)
+        {
+            LastError = ex;
+            OnErrorOccurred(ex, "RS485 polling error");
+        }
+    }
+
+    // UpdateCache
+    private void UpdateCache(Dictionary<string, double> data)
+    {
+        lock (_dataLock)
+        {
+            var now = DateTime.UtcNow;
+            
+            // CHARGE LEVEL
+            if (data.TryGetValue("SOC", out var soc))
+            {
+                if (Math.Abs(soc - _cachedChargeLevel) > 0.1)
+                {
+                    _cachedChargeLevel = soc;
+                }
+            }
+
+            // VOLTAGE
+            if (data.TryGetValue("Voltage", out var volt))
+            {
+                if (Math.Abs(volt - _cachedVoltage) > 0.05)
+                {
+                    _cachedVoltage = volt;
+                }
+            }
+
+            // CURRENT
+            if (data.TryGetValue("Current", out var curr))
+            {
+                if (Math.Abs(curr - _cachedCurrent) > 0.05)
+                {
+                    _cachedCurrent = curr;
+                }
+            }
+
+            // CHARGING
+            bool newCharging = _cachedCurrent > 0;
+            if (newCharging != _cachedCharging)
+            {
+                _cachedCharging = newCharging;
+            }
+
+            // TEMPERATURE
+            if (data.TryGetValue("Temp", out var t))
+            {
+                if (_cachedTemperature == null || Math.Abs(t - _cachedTemperature.Value) > 0.1)
+                {
+                    _cachedTemperature = t;
+                }
+            }
+
+            // Optional fields (per document)
+            _cachedHealth = data.TryGetValue("SOH", out var soh) ? soh : null;
+            _cachedRemainingCapacity = data.TryGetValue("RemainCapacity", out var rc) ? rc : null;
+            _cachedFullCapacity = data.TryGetValue("RemainEnergy", out var re) ? re : null;
+
+            _cachedChargeTime = data.TryGetValue("ChargeTime", out var ct) ? (int)ct : null;
+            _cachedDischargeTime = data.TryGetValue("DischargeTime", out var dt) ? (int)dt : null;
+            _cachedStatusRaw = data.TryGetValue("Status", out var st) ? (int)st : null;
+
+            _lastUpdateTime = now;
+
+            // Update cached BatteryState
+            _cachedBatteryState = CreateBatteryStateFromCache();
+
+            UpdateProperties();
+        }
+    }
+
+    private void ResetCache()
+    {
+        _cachedChargeLevel = 0;
+        _cachedVoltage = 0;
+        _cachedCurrent = 0;
+        _cachedCharging = false;
+
+        _cachedHealth = null;
+        _cachedTemperature = null;
+        _cachedRemainingCapacity = null;
+        _cachedFullCapacity = null;
+
+        _cachedChargeTime = null;
+        _cachedDischargeTime = null;
+        _cachedStatusRaw = null;
+
+        _lastUpdateTime = DateTime.MinValue;
+        _cachedBatteryState = null;
+    }
+
+    // IBattery Implementation
+    public Task<BatteryState> ReadBatteryStateAsync(CancellationToken cancellationToken = default)
+    {
+        lock (_dataLock)
+        {
+            if (_cachedBatteryState.HasValue)
+            {
+                return Task.FromResult(_cachedBatteryState.Value);
+            }
+            
+            return Task.FromResult(CreateBatteryStateFromCache());
+        }
+    }
+
+    // Helper method to create BatteryState from cached values
+    private BatteryState CreateBatteryStateFromCache()
+    {
+        // Map PowerSupplyStatus from charging state
+        byte powerSupplyStatus = BatteryState.PowerSupplyStatusUnknown;
+        if (_cachedCharging)
+            powerSupplyStatus = BatteryState.PowerSupplyStatusCharging;
+        else if (_cachedCurrent < 0)
+            powerSupplyStatus = BatteryState.PowerSupplyStatusDischarging;
+        else if (_cachedCurrent == 0)
+            powerSupplyStatus = BatteryState.PowerSupplyStatusNotCharging;
+
+        // Map PowerSupplyHealth from health percentage
+        byte powerSupplyHealth = BatteryState.PowerSupplyHealthUnknown;
+        if (_cachedHealth.HasValue)
+        {
+            if (_cachedHealth.Value >= 80)
+                powerSupplyHealth = BatteryState.PowerSupplyHealthGood;
+            else if (_cachedHealth.Value < 20)
+                powerSupplyHealth = BatteryState.PowerSupplyHealthDead;
+        }
+
+        return new BatteryState
+        {
+            Header = new Header
+            {
+                Stamp = _lastUpdateTime != DateTime.MinValue ? _lastUpdateTime : DateTime.UtcNow,
+                FrameId = "battery_frame"
+            },
+            Voltage = _cachedVoltage,
+            Current = _cachedCurrent,
+            Charge = _cachedRemainingCapacity.HasValue ? _cachedRemainingCapacity.Value : double.NaN,
+            Capacity = _cachedFullCapacity.HasValue ? _cachedFullCapacity.Value : double.NaN,
+            DesignCapacity = double.NaN, // Not provided by TADA BMU
+            Percentage = _cachedChargeLevel,
+            PowerSupplyStatus = powerSupplyStatus,
+            PowerSupplyHealth = powerSupplyHealth,
+            PowerSupplyTechnology = BatteryState.PowerSupplyTechnologyUnknown, // Not provided by TADA BMU
+            Present = true,
+            CellVoltage = Array.Empty<double>(), // Not provided by TADA BMU
+            CellTemperature = _cachedTemperature.HasValue ? new[] { _cachedTemperature.Value } : Array.Empty<double>(),
+            Location = string.Empty,
+            SerialNumber = string.Empty
+        };
+    }
+
+    // UpdateProperties
+    private void UpdateProperties()
+    {
+        lock (_dataLock)
+        {
+            SetProperty("ChargeLevel", _cachedChargeLevel.ToString("F1"));
+            SetProperty("Voltage", _cachedVoltage.ToString("F2"));
+            SetProperty("Current", _cachedCurrent.ToString("F2"));
+            SetProperty("Charging", _cachedCharging.ToString());
+            SetProperty("Temperature", _cachedTemperature?.ToString("F1") ?? "0");
+            SetProperty("Health", _cachedHealth?.ToString("F0") ?? "0");
+            SetProperty("RemainCapacity", _cachedRemainingCapacity?.ToString("F2") ?? "0");
+            SetProperty("FullCapacity", _cachedFullCapacity?.ToString("F2") ?? "0");
+
+            SetProperty("ChargeTime", _cachedChargeTime?.ToString() ?? "0");
+            SetProperty("DischargeTime", _cachedDischargeTime?.ToString() ?? "0");
+            SetProperty("StatusRaw", _cachedStatusRaw?.ToString() ?? "0");
+        }
+    }
+
+    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
+    {
+        yield return new PropertyDescription("ChargeLevel", "Charge Level (%)", "Mức pin (%)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 1,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Voltage", "Voltage (V)", "Điện áp (V)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 2,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Current", "Current (A)", "Dòng điện (A)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 3,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Charging", "Charging", "Đang sạc?")
+        {
+            DataType = "boolean",
+            IsReadOnly = true,
+            DisplayOrder = 4,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Temperature", "Temperature (°C)", "Nhiệt độ")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 5,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Health", "SOH (%)", "Sức khỏe pin")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 6,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("RemainCapacity", "Remaining Capacity (Ah)", "Dung lượng còn lại")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 7,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("FullCapacity", "Remaining Energy (Wh)", "Năng lượng còn lại (Wh)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 8,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("ChargeTime", "Charge Time (min)", "Thời gian còn lại để sạc đầy")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 9,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("DischargeTime", "Discharge Time (min)", "Thời gian còn lại để xả hết")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 10,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("StatusRaw", "Status Flags", "Trạng thái bit (BMU Flags)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 11,
+            Category = "Status"
+        };
+    }
+
+    protected override void Dispose(bool disposing)
+    {
+        if (disposing)
+        {
+            StopPollingLoop();
+            _client?.Dispose();
+        }
+
+        base.Dispose(disposing);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Tada/TadaRs485Client.cs

@@ -0,0 +1,359 @@
+using System;
+using System.Collections.Generic;
+using System.IO.Ports;
+using System.Linq;
+using System.Text;
+using System.Threading;
+
+namespace RobotNet10.RobotApp.Drivers.Tada;
+
+[Flags]
+public enum DataKind1 : byte
+{
+    Voltage = 1 << 0,
+    Current = 1 << 1,
+    SOC = 1 << 2,
+    Status = 1 << 3,
+    ChargeTime = 1 << 4,
+    DischargeTime = 1 << 5,
+    Temp = 1 << 6
+}
+
+[Flags]
+public enum DataKind2 : byte
+{
+    SOH = 1 << 0,
+    RemainCapacity = 1 << 1,
+    RemainEnergy = 1 << 2
+}
+public class TadaRs485Client : IDisposable
+{
+    private readonly ILogger _logger;
+    private SerialPort _port;
+    private readonly string _portName;
+    private readonly int _baud;
+    private readonly Parity _parity;
+    private readonly int _dataBits;
+    private readonly StopBits _stopBits;
+    private readonly int _readTimeoutMs;
+    private readonly int _writeTimeoutMs;
+    public bool IsFaulted => _faulted;
+
+    private readonly Lock _lock = new();
+    private bool _faulted = false;
+    private DateTime _lastRetry = DateTime.MinValue;
+    private int _retryDelayMs = 1000; // backoff min = 1s
+    private DateTime _lastDataTime = DateTime.MinValue;
+    private readonly int _dataTimeoutMs = 10_000; // 10 giây
+    private int _consecutiveFails = 0;
+    private readonly int _maxFails = 3; // sau 3 lần fail liên tiếp thì coi như lost
+
+    public TadaRs485Client(
+        ILogger logger,
+        string portName,
+        int baud = 19200,
+        Parity parity = Parity.None,
+        int dataBits = 8,
+        StopBits stopBits = StopBits.One,
+        int readTimeoutMs = 2000,
+        int writeTimeoutMs = 1000)
+    {
+        _logger = logger;
+        _portName = portName;
+        _baud = baud;
+        _parity = parity;
+        _dataBits = dataBits;
+        _stopBits = stopBits;
+        _readTimeoutMs = readTimeoutMs;
+        _writeTimeoutMs = writeTimeoutMs;
+        _lastDataTime = DateTime.Now;
+        _consecutiveFails = 0;
+        _port = null!;
+        EnsureConnected();
+    }
+
+    private void CheckDataTimeout()
+    {
+        if (_lastDataTime != DateTime.MinValue &&
+            (DateTime.Now - _lastDataTime).TotalMilliseconds > _dataTimeoutMs)
+        {
+            _logger.LogError("[TadaRs485Client] Communication lost (data timeout).");
+            _faulted = true;
+            _lastDataTime = DateTime.MinValue; // reset để tránh spam log
+        }
+    }
+
+    private void EnsureConnected()
+    {
+        lock (_lock)
+        {
+            if (_port != null && _port.IsOpen && !_faulted) return;
+
+            if ((DateTime.Now - _lastRetry).TotalMilliseconds < _retryDelayMs)
+                return;
+            _lastRetry = DateTime.Now;
+
+            try
+            {
+                _port?.Dispose();
+                _port = new SerialPort(_portName, _baud, _parity, _dataBits, _stopBits)
+                {
+                    ReadTimeout = _readTimeoutMs,
+                    WriteTimeout = _writeTimeoutMs
+                };
+                _port.Open();
+                _faulted = false;
+                _retryDelayMs = 1000;
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError("[TadaRs485Client] Connect failed: {ex.Message}", ex.Message);
+                _faulted = true;
+                // exponential backoff up to 30s
+                _retryDelayMs = Math.Min(_retryDelayMs * 2, 30_000);
+            }
+        }
+    }
+
+    private static byte Checksum(byte[] data, int start, int len)
+    {
+        int sum = 0;
+        for (int i = start; i < start + len; i++) sum += data[i];
+        return (byte)(sum & 0xFF);
+    }
+
+    private static string ToHex(byte[] data, int len)
+    {
+        var sb = new StringBuilder();
+        for (int i = 0; i < len; i++)
+        {
+            sb.Append(data[i].ToString("X2"));
+            if (i < len - 1) sb.Append('-');
+        }
+        return sb.ToString();
+    }
+
+    public void RequestStatus(byte address = 0x60,
+        DataKind1 kind1 = DataKind1.Voltage | DataKind1.Current | DataKind1.SOC | DataKind1.Status |
+                          DataKind1.ChargeTime | DataKind1.DischargeTime | DataKind1.Temp,
+        DataKind2 kind2 = DataKind2.SOH | DataKind2.RemainCapacity | DataKind2.RemainEnergy)
+    {
+        EnsureConnected();
+        if (_port == null || !_port.IsOpen || _faulted) return;
+
+        try
+        {
+            byte kind1Byte = (byte)kind1;
+            byte kind2Byte = (byte)kind2;
+
+            byte[] frame =
+            [
+                0xAF, 0xFA,
+                address,
+                0x05,
+                0x01,
+                address,
+                kind1Byte, kind2Byte,
+                0x00,
+                0xAF, 0xA0
+            ];
+
+            frame[8] = Checksum(frame, 2, 6);
+            _port.DiscardInBuffer();
+            _port.DiscardOutBuffer();
+            _port.Write(frame, 0, frame.Length);
+            Thread.Sleep(50);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError("[TadaRs485Client] Write error: {ex.Message}", ex.Message);
+            _faulted = true;
+        }
+    }
+
+    private byte[] ReadFrame()
+    {
+        EnsureConnected();
+        if (_port == null || !_port.IsOpen || _faulted) return [];
+
+        var buffer = new List<byte>();
+        int expectedLen = -1;
+        var start = DateTime.Now;
+
+        try
+        {
+            while ((DateTime.Now - start).TotalMilliseconds < _readTimeoutMs)
+            {
+                int bytesAvailable = _port.BytesToRead;
+                if (bytesAvailable > 0)
+                {
+                    byte[] tempBuffer = new byte[bytesAvailable];
+                    int bytesRead = _port.Read(tempBuffer, 0, bytesAvailable);
+                    buffer.AddRange(tempBuffer.Take(bytesRead));
+
+                    // check start marker (chuẩn AF FA hoặc bản partial 4D)
+                    if (buffer.Count >= 3 && expectedLen == -1)
+                    {
+                        if (buffer[0] == 0xAF && buffer[1] == 0xFA)
+                        {
+                            expectedLen = buffer[3] + 6;
+                        }
+                        else if (buffer[0] == 0x4D)
+                        {
+                            // frame thiếu AF FA -> vẫn tính chiều dài như thường
+                            expectedLen = buffer[2] + 5; // vì mất 2 byte start
+                        }
+                    }
+
+                    if (expectedLen > 0 && buffer.Count >= expectedLen)
+                    {
+                        if (buffer[^2] == 0xAF && buffer[^1] == 0xA0)
+                        {
+                            return [.. buffer];
+                        }
+                        else
+                        {
+                            buffer.Clear();
+                            expectedLen = -1;
+                        }
+                    }
+                }
+                else
+                {
+                    Thread.Sleep(2);
+                }
+            }
+
+            // hết thời gian chờ
+            if (buffer.Count > 0)
+                _logger.LogWarning("[TadaRs485Client] Timeout / partial frame ({buffer.Count} bytes): {frame}", buffer.Count, ToHex([.. buffer], buffer.Count));
+            return [];
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError("[TadaRs485Client] Read error: {ex.Message}", ex.Message);
+            _faulted = true;
+            return [];
+        }
+    }
+
+    public Dictionary<string, double>? ReadResponse()
+    {
+        var frame = ReadFrame();
+        if (frame == null || frame.Length < 9)
+        {
+            _consecutiveFails++;
+            if (_consecutiveFails >= _maxFails)
+            {
+                _logger.LogError("[TadaRs485Client] Communication lost (too many failed reads).");
+                _faulted = true;
+            }
+            CheckDataTimeout();
+            return null;
+        }
+
+        if (frame[0] == 0x4D)
+        {
+            // Partial frame - fix it by prepending AF FA
+            var newFrame = new byte[frame.Length + 1];
+            newFrame[0] = 0xAF; newFrame[1] = 0xFA;
+            Array.Copy(frame, 1, newFrame, 2, frame.Length - 1);
+            frame = newFrame;
+        }
+        else if (frame[0] == 0xAF && frame.Length > 1 && frame[1] == 0xFA)
+        {
+            // Valid full frame - continue processing
+        }
+        else
+        {
+            // Invalid frame format
+            _consecutiveFails++;
+            if (_consecutiveFails >= _maxFails)
+            {   
+                _logger.LogError("[TadaRs485Client] Communication lost (invalid frame format).");
+                _faulted = true;
+            }
+            CheckDataTimeout();
+            return null;
+        }
+
+        if (frame[^2] != 0xAF || frame[^1] != 0xA0)
+        {
+            _logger.LogError("[TadaRs485Client] Footer mismatch");
+            throw new Exception("invalid frame: footer");
+        }
+        if (frame[4] != 0x03)
+        {
+            _logger.LogError("[TadaRs485Client] Command code mismatch");
+            throw new Exception("invalid frame: command");
+        }
+
+        var result = new Dictionary<string, double>();
+        int dataLen = frame[3] - 3;
+        int dataStart = 6;
+
+        for (int i = 0; i + 1 < dataLen; i += 2)
+        {
+            int idx = dataStart + i;
+            if (idx + 1 >= frame.Length) break;
+
+            ushort raw = (ushort)((frame[idx] << 8) | frame[idx + 1]);
+            int index = i / 2;
+
+            switch (index)
+            {
+                case 0: result["Voltage"] = raw / 100.0; break;
+                case 1: result["Current"] = (short)raw / 10.0; break;
+                case 2: result["SOC"] = raw; break;
+                case 3: result["Status"] = raw; break;
+                case 4: result["ChargeTime"] = raw; break;
+                case 5: result["DischargeTime"] = raw; break;
+                case 6: result["Temp"] = (short)raw / 10.0; break;
+                case 7: result["SOH"] = raw; break;
+                case 8: result["RemainCapacity"] = raw / 100.0; break;
+                case 9: result["RemainEnergy"] = raw / 10.0; break;
+            }
+        }
+
+        _lastDataTime = DateTime.Now;  // reset watchdog
+        _consecutiveFails = 0;         // reset fail counter
+        return result;
+    }
+
+    public void ForceReconnect()
+    {
+        lock (_lock)
+        {
+            try
+            {
+                _port?.Close();
+            }
+            catch { }
+            _faulted = false;
+            _lastRetry = DateTime.MinValue;
+            _retryDelayMs = 1000;
+            EnsureConnected();
+        }
+    }
+
+    public void Dispose()
+    {
+        try
+        {
+            if (_port != null)
+            {
+                if (_port.IsOpen)
+                {
+                    _port.Close();
+                }
+                _port.Dispose();
+                _port = null!;
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError("[TadaRs485Client] Error in Dispose: {ex.Message}", ex.Message);
+        }
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Varta/VartaBattery.cs

@@ -0,0 +1,593 @@
+using System.Timers;
+using RobotNet10.CANOpen;
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.Shared;
+using RobotNet10.Shared.Sensor;
+
+namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
+
+[Device(DeviceType.Battery, "Varta", "VartaBattery", "1.0.0", Description = "Battery Varta CAN Driver")]
+public class VartaBattery : DeviceBase, IBattery
+{
+    private readonly ILogger<VartaBattery> _logger;
+    private readonly ICanOpenManager _canOpenManager;
+    private readonly Lock _dataLock = new();
+
+    private VartaCanClient? _client;
+    private System.Timers.Timer? _pollingTimer;
+
+    private readonly string _canInterface;
+    private readonly int _readTimeoutMs;
+    private readonly int _pollingIntervalMs;
+    private readonly int _connectionTimeoutMs;
+
+    private double _cachedChargeLevel;
+    private double _cachedVoltage;
+    private double _cachedCurrent;
+    private bool _cachedCharging;
+
+    private double? _cachedFetTemperature;
+    private double? _cachedCellTemperature;
+    private double? _cachedChargeReqVoltage;
+    private double? _cachedChargeReqCurrent;
+
+    private double? _cachedNominalCapacityMah;
+    private double? _cachedFullCapacityMah;
+    private double? _cachedRemainingCapacityMah;
+    private double? _cachedHealth;
+
+    private int? _cachedInfo;
+    private int? _cachedWarn;
+    private int? _cachedError;
+    private int? _cachedChargeCtrl;
+
+    private DateTime _lastUpdateTime = DateTime.MinValue;
+    private DateTime _connectedAt = DateTime.MinValue;
+    private bool _connectionLossSignaled;
+    private BatteryState? _cachedBatteryState;
+
+    public BatteryState? CurrentBatteryState
+    {
+        get { lock (_dataLock) { return _cachedBatteryState; } }
+    }
+
+    public VartaBattery(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
+        : base(deviceId, deviceName, DeviceType.Battery)
+    {
+        var loggerFactory = serviceProvider.GetRequiredService<ILoggerFactory>();
+        _logger = loggerFactory.CreateLogger<VartaBattery>();
+        _canOpenManager = serviceProvider.GetRequiredService<ICanOpenManager>();
+
+        _canInterface = connection.GetValue<string>("CanInterface")
+            ?? connection.GetValue<string>("Interface")
+            ?? "can0";
+
+        _readTimeoutMs = connection.GetValue<int?>("ReadTimeoutMs") ?? 200;
+        _pollingIntervalMs = connection.GetValue<int?>("PollingIntervalMs") ?? 500;
+        _connectionTimeoutMs = connection.GetValue<int?>("ConnectionTimeoutMs")
+            ?? Math.Max(5000, _pollingIntervalMs * 10);
+
+        AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true;
+        ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 2000;
+        MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0;
+
+        UpdateProperties();
+    }
+
+    protected override Task OnInitializeAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            _client?.Dispose();
+            _client = new VartaCanClient(_logger, _canOpenManager, _canInterface, _readTimeoutMs);
+        }
+
+        UpdateProperties();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnConnectAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            if (_client == null)
+            {
+                _client = new VartaCanClient(_logger, _canOpenManager, _canInterface, _readTimeoutMs);
+            }
+            else
+            {
+                _client.ForceReconnect();
+            }
+
+            _connectedAt = DateTime.UtcNow;
+            _connectionLossSignaled = false;
+        }
+
+        StartPollingLoop();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
+    {
+        StopPollingLoop();
+        lock (_dataLock)
+        {
+            _connectionLossSignaled = false;
+            _connectedAt = DateTime.MinValue;
+        }
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnResetAsync(CancellationToken cancellationToken)
+    {
+        StopPollingLoop();
+
+        lock (_dataLock)
+        {
+            _client?.ForceReconnect();
+            ResetCache();
+            _connectedAt = DateTime.UtcNow;
+            _connectionLossSignaled = false;
+        }
+
+        UpdateProperties();
+        StartPollingLoop();
+        return Task.CompletedTask;
+    }
+
+    protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+    {
+        lock (_dataLock)
+        {
+            if (_client == null || _client.IsFaulted)
+            {
+                return Task.FromResult(false);
+            }
+
+            var referenceTime = _lastUpdateTime != DateTime.MinValue
+                ? _lastUpdateTime
+                : _connectedAt;
+
+            if (referenceTime == DateTime.MinValue)
+            {
+                return Task.FromResult(false);
+            }
+
+            var isRecent = (DateTime.UtcNow - referenceTime).TotalMilliseconds <= _connectionTimeoutMs;
+            return Task.FromResult(isRecent);
+        }
+    }
+
+    private void StartPollingLoop()
+    {
+        lock (_dataLock)
+        {
+            if (_pollingTimer != null && _pollingTimer.Enabled)
+            {
+                return;
+            }
+
+            StopPollingLoop();
+
+            _pollingTimer = new System.Timers.Timer(_pollingIntervalMs)
+            {
+                AutoReset = true,
+                Enabled = true
+            };
+            _pollingTimer.Elapsed += PollTimer_Elapsed;
+        }
+    }
+
+    private void StopPollingLoop()
+    {
+        lock (_dataLock)
+        {
+            if (_pollingTimer == null)
+            {
+                return;
+            }
+
+            _pollingTimer.Stop();
+            _pollingTimer.Elapsed -= PollTimer_Elapsed;
+            _pollingTimer.Dispose();
+            _pollingTimer = null;
+        }
+    }
+
+    private void PollTimer_Elapsed(object? sender, ElapsedEventArgs e)
+    {
+        try
+        {
+            var data = _client?.ReadResponse();
+            if (data != null)
+            {
+                UpdateCache(data);
+                return;
+            }
+
+            var noDataMs = _lastUpdateTime != DateTime.MinValue
+                ? (DateTime.UtcNow - _lastUpdateTime).TotalMilliseconds
+                : 0;
+            _logger.LogWarning("[VartaBattery] No CAN data received (last update {NoDataMs:F0}ms ago, IsFaulted={IsFaulted})",
+                noDataMs, _client?.IsFaulted);
+
+            if (_client?.IsFaulted == true)
+            {
+                NotifyConnectionLost("Varta CAN socket faulted");
+                return;
+            }
+
+            if (_lastUpdateTime != DateTime.MinValue && noDataMs > _connectionTimeoutMs)
+            {
+                NotifyConnectionLost($"No Varta CAN data for more than {_connectionTimeoutMs}ms");
+            }
+        }
+        catch (Exception ex)
+        {
+            NotifyConnectionLost("Varta CAN polling exception", ex);
+        }
+    }
+
+    private void NotifyConnectionLost(string reason, Exception? cause = null)
+    {
+        lock (_dataLock)
+        {
+            if (_connectionLossSignaled)
+            {
+                return;
+            }
+
+            _connectionLossSignaled = true;
+        }
+
+        LastError = cause ?? new TimeoutException(reason);
+        OnErrorOccurred(LastError, reason);
+        _ = CheckConnectionAsync();
+    }
+
+    private void UpdateCache(Dictionary<string, double> data)
+    {
+        lock (_dataLock)
+        {
+            if (data.TryGetValue("SOC", out var soc))
+            {
+                _cachedChargeLevel = soc;
+            }
+
+            if (data.TryGetValue("Voltage", out var voltage))
+            {
+                _cachedVoltage = voltage;
+            }
+
+            if (data.TryGetValue("Current", out var current))
+            {
+                _cachedCurrent = current;
+                _cachedCharging = current > 0;
+            }
+
+            _cachedFetTemperature = data.TryGetValue("FetTemp", out var fetTemp) ? fetTemp : _cachedFetTemperature;
+            _cachedCellTemperature = data.TryGetValue("CellTemp", out var cellTemp) ? cellTemp : _cachedCellTemperature;
+            _cachedChargeReqVoltage = data.TryGetValue("ChargeReqVoltage", out var chargeReqVoltage) ? chargeReqVoltage : _cachedChargeReqVoltage;
+            _cachedChargeReqCurrent = data.TryGetValue("ChargeReqCurrent", out var chargeReqCurrent) ? chargeReqCurrent : _cachedChargeReqCurrent;
+
+            _cachedNominalCapacityMah = data.TryGetValue("NominalCapacityMah", out var nominalCap) ? nominalCap : _cachedNominalCapacityMah;
+            _cachedFullCapacityMah = data.TryGetValue("FullCapacityMah", out var fullCap) ? fullCap : _cachedFullCapacityMah;
+            _cachedRemainingCapacityMah = data.TryGetValue("RemainingCapacityMah", out var remainingCap) ? remainingCap : _cachedRemainingCapacityMah;
+
+            // If device provides SOH value directly, use it. Otherwise compute from capacities when available
+            if (data.TryGetValue("SOH", out var sohVal))
+            {
+                _cachedHealth = sohVal;
+            }
+            else if (_cachedFullCapacityMah.HasValue && _cachedNominalCapacityMah.HasValue && _cachedNominalCapacityMah.Value > 0)
+            {
+                try
+                {
+                    _cachedHealth = (_cachedFullCapacityMah.Value / _cachedNominalCapacityMah.Value) * 100.0;
+                }
+                catch
+                {
+                    _cachedHealth = null;
+                }
+            }
+            else
+            {
+                _cachedHealth = null;
+            }
+
+            _cachedInfo = data.TryGetValue("Info", out var info) ? (int)info : _cachedInfo;
+            _cachedWarn = data.TryGetValue("Warn", out var warn) ? (int)warn : _cachedWarn;
+            _cachedError = data.TryGetValue("Error", out var error) ? (int)error : _cachedError;
+            _cachedChargeCtrl = data.TryGetValue("ChargeCtrl", out var chargeCtrl) ? (int)chargeCtrl : _cachedChargeCtrl;
+
+            _lastUpdateTime = DateTime.UtcNow;
+            _connectionLossSignaled = false;
+            _cachedBatteryState = CreateBatteryStateFromCache();
+
+            UpdateProperties();
+        }
+    }
+
+    private void ResetCache()
+    {
+        _cachedChargeLevel = 0;
+        _cachedVoltage = 0;
+        _cachedCurrent = 0;
+        _cachedCharging = false;
+
+        _cachedFetTemperature = null;
+        _cachedCellTemperature = null;
+        _cachedChargeReqVoltage = null;
+        _cachedChargeReqCurrent = null;
+
+        _cachedNominalCapacityMah = null;
+        _cachedFullCapacityMah = null;
+        _cachedRemainingCapacityMah = null;
+
+        _cachedInfo = null;
+        _cachedWarn = null;
+        _cachedError = null;
+        _cachedChargeCtrl = null;
+
+        _lastUpdateTime = DateTime.MinValue;
+        _connectedAt = DateTime.MinValue;
+        _connectionLossSignaled = false;
+        _cachedBatteryState = null;
+    }
+
+    public Task<BatteryState> ReadBatteryStateAsync(CancellationToken cancellationToken = default)
+    {
+        lock (_dataLock)
+        {
+            if (_cachedBatteryState.HasValue)
+            {
+                return Task.FromResult(_cachedBatteryState.Value);
+            }
+
+            return Task.FromResult(CreateBatteryStateFromCache());
+        }
+    }
+
+    private BatteryState CreateBatteryStateFromCache()
+    {
+        byte powerSupplyStatus = BatteryState.PowerSupplyStatusUnknown;
+        if (_cachedCharging)
+        {
+            powerSupplyStatus = BatteryState.PowerSupplyStatusCharging;
+        }
+        else if (_cachedCurrent < 0)
+        {
+            powerSupplyStatus = BatteryState.PowerSupplyStatusDischarging;
+        }
+        else if (_cachedCurrent == 0)
+        {
+            powerSupplyStatus = BatteryState.PowerSupplyStatusNotCharging;
+        }
+
+        var cellTemperature = _cachedCellTemperature.HasValue
+            ? new[] { _cachedCellTemperature.Value }
+            : Array.Empty<double>();
+
+        // Map PowerSupplyHealth from computed SOH where possible
+        byte powerSupplyHealth = BatteryState.PowerSupplyHealthUnknown;
+        if (_cachedHealth.HasValue)
+        {
+            if (_cachedHealth.Value >= 80.0)
+                powerSupplyHealth = BatteryState.PowerSupplyHealthGood;
+            else if (_cachedHealth.Value < 20.0)
+                powerSupplyHealth = BatteryState.PowerSupplyHealthDead;
+            else
+                powerSupplyHealth = BatteryState.PowerSupplyHealthUnknown;
+        }
+
+        return new BatteryState
+        {
+            Header = new Header
+            {
+                Stamp = _lastUpdateTime != DateTime.MinValue ? _lastUpdateTime : DateTime.UtcNow,
+                FrameId = "battery_frame"
+            },
+            Voltage = (float)_cachedVoltage,
+            Current = (float)_cachedCurrent,
+            Charge = _cachedRemainingCapacityMah.HasValue ? (float)(_cachedRemainingCapacityMah.Value / 1000.0) : float.NaN,
+            Capacity = _cachedFullCapacityMah.HasValue ? (float)(_cachedFullCapacityMah.Value / 1000.0) : float.NaN,
+            DesignCapacity = _cachedNominalCapacityMah.HasValue ? (float)(_cachedNominalCapacityMah.Value / 1000.0) : float.NaN,
+            Percentage = (float)_cachedChargeLevel,
+            PowerSupplyStatus = powerSupplyStatus,
+            PowerSupplyHealth = powerSupplyHealth,
+            PowerSupplyTechnology = BatteryState.PowerSupplyTechnologyUnknown,
+            Present = true,
+            CellVoltage = [],
+            CellTemperature = cellTemperature,
+            Location = string.Empty,
+            SerialNumber = string.Empty
+        };
+    }
+
+    private void UpdateProperties()
+    {
+        lock (_dataLock)
+        {
+            SetProperty("CanInterface", _canInterface);
+            SetProperty("ConnectionTimeoutMs", _connectionTimeoutMs.ToString());
+            SetProperty("ChargeLevel", _cachedChargeLevel.ToString("F1"));
+            SetProperty("Voltage", _cachedVoltage.ToString("F2"));
+            SetProperty("Current", _cachedCurrent.ToString("F2"));
+            SetProperty("Charging", _cachedCharging.ToString());
+
+            SetProperty("FetTemperature", _cachedFetTemperature?.ToString("F1") ?? "0");
+            SetProperty("CellTemperature", _cachedCellTemperature?.ToString("F1") ?? "0");
+            SetProperty("ChargeReqVoltage", _cachedChargeReqVoltage?.ToString("F2") ?? "0");
+            SetProperty("ChargeReqCurrent", _cachedChargeReqCurrent?.ToString("F2") ?? "0");
+
+            SetProperty("NominalCapacityMah", _cachedNominalCapacityMah?.ToString("F0") ?? "0");
+            SetProperty("FullCapacityMah", _cachedFullCapacityMah?.ToString("F0") ?? "0");
+            SetProperty("RemainingCapacityMah", _cachedRemainingCapacityMah?.ToString("F0") ?? "0");
+            SetProperty("Health", _cachedHealth?.ToString("F0") ?? "0");
+
+            SetProperty("Info", _cachedInfo?.ToString() ?? "0");
+            SetProperty("Warn", _cachedWarn?.ToString() ?? "0");
+            SetProperty("Error", _cachedError?.ToString() ?? "0");
+            SetProperty("ChargeCtrl", _cachedChargeCtrl?.ToString() ?? "0");
+        }
+    }
+
+    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
+    {
+        yield return new PropertyDescription("CanInterface", "CAN Interface", "CAN interface của pin")
+        {
+            DataType = "text",
+            IsReadOnly = true,
+            DisplayOrder = 1,
+            Category = "Config"
+        };
+
+        yield return new PropertyDescription("ConnectionTimeoutMs", "Connection Timeout (ms)", "Ngưỡng timeout phát hiện mất kết nối")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 2,
+            Category = "Config"
+        };
+
+        yield return new PropertyDescription("ChargeLevel", "Charge Level (%)", "Mức pin (%)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 3,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Voltage", "Voltage (V)", "Điện áp (V)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 4,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Current", "Current (A)", "Dòng điện (A)")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 5,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Charging", "Charging", "Đang sạc?")
+        {
+            DataType = "boolean",
+            IsReadOnly = true,
+            DisplayOrder = 6,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("FetTemperature", "FET Temp (C)", "Nhiệt độ FET")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 7,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("CellTemperature", "Cell Temp (C)", "Nhiệt độ cell")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 8,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("ChargeReqVoltage", "Charge Req Voltage (V)", "Điện áp sạc yêu cầu")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 9,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("ChargeReqCurrent", "Charge Req Current (A)", "Dòng sạc yêu cầu")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 10,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("NominalCapacityMah", "Nominal Capacity (mAh)", "Dung lượng danh định")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 11,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("FullCapacityMah", "Full Capacity (mAh)", "Dung lượng đầy")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 12,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("RemainingCapacityMah", "Remaining Capacity (mAh)", "Dung lượng còn lại")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 13,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Health", "SOH (%)", "Sức khỏe pin")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 14,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Info", "Info Flags", "Cờ thông tin")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 15,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Warn", "Warn Flags", "Cờ cảnh báo")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 16,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("Error", "Error Flags", "Cờ lỗi")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 17,
+            Category = "Status"
+        };
+
+        yield return new PropertyDescription("ChargeCtrl", "Charge Control", "Trạng thái điều khiển sạc")
+        {
+            DataType = "number",
+            IsReadOnly = true,
+            DisplayOrder = 18,
+            Category = "Status"
+        };
+    }
+
+    protected override void Dispose(bool disposing)
+    {
+        if (disposing)
+        {
+            StopPollingLoop();
+            _client?.Dispose();
+        }
+
+        base.Dispose(disposing);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Varta/VartaCanClient.cs

@@ -0,0 +1,285 @@
+using Microsoft.Extensions.Logging;
+using RobotNet10.CANOpen;
+using RobotNet10.CANOpen.Interfaces;
+using System.Collections.Concurrent;
+using System.Runtime.InteropServices;
+
+// using 
+namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
+
+/// <summary>
+/// Varta CAN client — đọc dữ liệu pin qua CANopen PDO.
+/// Dùng SocketCAN transport có sẵn trong RobotNet10.CANOpen.
+/// Protocol (CAN 11-bit):
+///   0x19B -> Voltage, Current
+///   0x281 -> FetTemp, CellTemp, ChargeReqVoltage, ChargeReqCurrent
+///   0x381 -> NominalCapacity, FullCapacity, RemainingCapacity, SOC
+///   0x481/0x581 -> Info, Warn, Error, ChargeCtrl
+/// </summary>
+public sealed class VartaCanClient : IDisposable
+{
+    private readonly ILogger _logger;
+    private readonly ICanOpenManager _canOpenManager;
+    private readonly string _canInterface;
+    private readonly int _readTimeoutMs;
+    private readonly Lock _stateLock = new();
+    private readonly ConcurrentQueue<CanFrameReceivedEventArgs> _rxQueue = new();
+    private readonly SemaphoreSlim _rxSignal = new(0);
+    private ICanBus? _bus;
+    private bool _disposed;
+    private bool _isFaulted;
+
+    public bool IsFaulted
+    {
+        get
+        {
+            lock (_stateLock)
+            {
+                return _isFaulted;
+            }
+        }
+    }
+
+    public VartaCanClient(ILogger logger, ICanOpenManager canOpenManager, string canInterface, int readTimeoutMs = 200)
+    {
+        _logger = logger;
+        _canOpenManager = canOpenManager;
+        _canInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
+        _readTimeoutMs = Math.Max(10, readTimeoutMs);
+
+        OpenBus();
+    }
+
+
+    private void OpenBus()
+    {
+        lock (_stateLock)
+        {
+            _isFaulted = false;
+        }
+
+        try
+        {
+            // Xóa bus cũ khỏi cache của CanOpenManager trước khi tạo lại,
+            // tránh GetOrCreateCanBusAsync trả về bus đã chết do caching.
+            _logger.LogInformation("[VartaCanClient] Removing old CAN bus from cache for {Iface}", _canInterface);
+            _canOpenManager.RemoveCanBusAsync(_canInterface).GetAwaiter().GetResult();
+
+            var bus = _canOpenManager.GetOrCreateCanBusAsync(_canInterface).GetAwaiter().GetResult();
+            _logger.LogInformation("[VartaCanClient] CAN bus recreated for {Iface}, IsConnected={IsConnected}", _canInterface, bus.IsConnected);
+
+            lock (_stateLock)
+            {
+                if (_bus != null)
+                {
+                    _bus.FrameReceived -= OnFrameReceived;
+                }
+
+                _bus = bus;
+                _bus.FrameReceived += OnFrameReceived;
+            }
+        }
+        catch (Exception ex)
+        {
+            lock (_stateLock)
+            {
+                _isFaulted = true;
+            }
+            // _logger.LogError(ex, "[VartaCanClient] Không thể mở SocketCAN trên interface {Iface}", _canInterface);
+        }
+    }
+
+    public void ForceReconnect()
+    {
+        if (_disposed)
+        {
+            return;
+        }
+
+        _logger.LogInformation("[VartaCanClient] ForceReconnect start {Iface}", _canInterface);
+
+        lock (_stateLock)
+        {
+            try
+            {
+                if (_bus != null)
+                {
+                    _bus.FrameReceived -= OnFrameReceived;
+                }
+            }
+            catch (Exception ex)
+            {
+                _logger.LogError(ex, "[VartaCanClient] No data in {Iface}", _canInterface);
+            }
+
+            while (_rxQueue.TryDequeue(out _)) { }
+            while (_rxSignal.Wait(0)) { }
+        }
+
+        OpenBus();
+        _logger.LogInformation("[VartaCanClient] ForceReconnect Finished, IsFaulted={IsFaulted}", _isFaulted);
+    }
+
+    /// <summary>
+    /// Đọc frame mới nhất từ queue receive và decode theo protocol Varta.
+    /// Drain toàn bộ queue, chỉ giữ frame mới nhất cho mỗi CAN ID để tránh trễ dữ liệu.
+    /// </summary>
+    public Dictionary<string, double>? ReadResponse(int maxFrames = 30)
+    {
+        if (_disposed || IsFaulted || _bus == null || !_bus.IsConnected)
+        {
+            return null;
+        }
+
+        // Nếu queue rỗng, chờ frame mới đến
+        if (_rxQueue.IsEmpty)
+        {
+            try
+            {
+                if (!_rxSignal.Wait(_readTimeoutMs))
+                {
+                    return null;
+                    ForceReconnect();
+                }
+            }
+            catch
+            {
+                return null;
+            }
+        }
+
+        // Drain toàn bộ queue, chỉ giữ frame mới nhất cho mỗi CAN ID
+        var latestFrames = new Dictionary<uint, CanFrameReceivedEventArgs>();
+        while (_rxQueue.TryDequeue(out var frame))
+        {
+            latestFrames[frame.CanId] = frame;
+            // Drain semaphore để khớp với số frame bị loại bỏ
+            _rxSignal.Wait(0);
+        }
+
+        if (latestFrames.Count == 0)
+        {
+            return null;
+        }
+
+        var result = new Dictionary<string, double>(StringComparer.OrdinalIgnoreCase);
+        foreach (var frame in latestFrames.Values)
+        {
+            DecodeFrame(frame, result);
+        }
+
+        return result.Count == 0 ? null : result;
+    }
+
+    private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
+    {
+        if (_disposed)
+        {
+            return;
+        }
+
+        _rxQueue.Enqueue(e);
+        try
+        {
+            _rxSignal.Release();
+        }
+        catch (SemaphoreFullException)
+        {
+        }
+    }
+
+    private static void DecodeFrame(CanFrameReceivedEventArgs frame, Dictionary<string, double> result)
+    {
+        // uint canId = frame.CanId & 0x7FFu;
+        var d = frame.Data;
+
+        if (d == null)
+        {
+            return;
+        }
+
+        // var canBase = canId & 0x780u;
+
+        switch (frame.CanId)
+        {
+            case 0x181: // TPDO1: 0x180 + NodeId
+            {
+                uint voltage = BitConverter.ToUInt32(d, 0);
+                int current = BitConverter.ToInt32(d, 4);
+                double volts = voltage / 1000.0;
+                double amps = current / 1000.0;
+                result["Voltage"] = Math.Round(volts, 1, MidpointRounding.ToZero);
+                result["Current"] = Math.Round(amps, 1, MidpointRounding.ToZero);
+                // Console.WriteLine($"[VartaCanClient] Received TPDO1: Voltage={volts} V, Current={amps} A, Timestamps: {DateTime.Now:HH:mm:ss.fff} s");
+                break;
+            }
+
+            case 0x281: // TPDO2: 0x280 + NodeId
+                result["FetTemp"] = BitConverter.ToInt16(d, 0) / 10.0;
+                result["CellTemp"] = BitConverter.ToInt16(d, 2) / 10.0;
+                result["ChargeReqVoltage"] = BitConverter.ToUInt16(d, 4) / 1000.0;
+                result["ChargeReqCurrent"] = BitConverter.ToUInt16(d, 6) / 1000.0;
+                // Console.WriteLine($"[VartaCanClient] Received TPDO2: FetTemp={result["FetTemp"]} °C, CellTemp={result["CellTemp"]} °C, ChargeReqVoltage={result["ChargeReqVoltage"]} V, ChargeReqCurrent={result["ChargeReqCurrent"]} A");
+                break;
+
+            case 0x381: // TPDO3: 0x380 + NodeId
+            {
+                var nominal = BitConverter.ToUInt16(d, 0);
+                var full = BitConverter.ToUInt16(d, 2);
+                var remaining = BitConverter.ToUInt16(d, 4);
+                result["NominalCapacityMah"] = nominal;
+                result["FullCapacityMah"] = full;
+                result["RemainingCapacityMah"] = remaining;
+                result["SOC"] = full == 0 ? 0 : remaining * 100.0 / full;
+                result["SOH"] = nominal == 0 ? 0 : full * 100.0 / nominal;
+                // Console.WriteLine($"[VartaCanClient] Received TPDO3: Nominal={nominal} mAh, Full={full} mAh, Remaining={remaining} mAh, SOC={result["SOC"]} %, SOH={result["SOH"]} %");
+                break;
+            }
+
+            case 0x481: // TPDO4: 0x480 + NodeId
+
+            case 0x581: // SDO response: 0x580 + NodeId (some firmware puts status words here)
+                result["Info"] = BitConverter.ToUInt16(d, 0);
+                result["Warn"] = BitConverter.ToUInt16(d, 2);
+                result["Error"] = BitConverter.ToUInt16(d, 4);
+                result["ChargeCtrl"] = BitConverter.ToUInt16(d, 6);
+                // Console.WriteLine($"[VartaCanClient] Received TPDO4/SDO: Info={result["Info"]}, Warn={result["Warn"]}, Error={result["Error"]}, ChargeCtrl={result["ChargeCtrl"]}");
+                break;
+
+            case 0x264:
+                result["ChargeControl"] = d[0];                                    // byte 0: uint8
+                result["SOC"] = d[1];                                              // byte 1: uint8, %
+                // byte 2: không sử dụng
+                result["ChargeVoltageRequest"] = BitConverter.ToUInt16(d, 3) / 256.0;  // bytes 3-4: uint16, 1/256 V
+                result["ChargeCurrentRequest"] = BitConverter.ToUInt16(d, 5) / 16.0;  // bytes 5-6: uint16, 1/16 A
+                result["BatteryStatus"] = d[7];                                    // byte 7: uint8
+                // Console.WriteLine($"[VartaCanClient] Received 0x264: ChargeControl={result["ChargeControl"]}, SOC={result["SOC"]} %, ChargeVoltageRequest={result["ChargeVoltageRequest"]:F4} V, ChargeCurrentRequest={result["ChargeCurrentRequest"]:F4} A, BatteryStatus={result["BatteryStatus"]}");
+                break;
+        }
+    }
+
+    public void Dispose()
+    {
+        lock (_stateLock)
+        {
+            if (_disposed)
+            {
+                return;
+            }
+            _disposed = true;
+        }
+
+        try
+        {
+            _bus?.FrameReceived -= OnFrameReceived;
+        }
+        catch
+        {
+        }
+        finally
+        {
+            _rxSignal.Dispose();
+            while (_rxQueue.TryDequeue(out _)) { }
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Varta/Vartaservice/VartaChargerSimulatorService.cs

@@ -0,0 +1,165 @@
+using Microsoft.Extensions.Configuration;
+using Microsoft.Extensions.Hosting;
+using Microsoft.Extensions.Logging;
+using RobotNet10.CANOpen;
+
+namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
+
+public sealed class VartaChargerSimulatorService : IHostedService, IDisposable
+{
+    private readonly ILogger<VartaChargerSimulatorService> _logger;
+    private readonly ICanOpenManager _canOpenManager;
+    private readonly VartaChargerSimulatorOptions _options;
+    private readonly SemaphoreSlim _controlLock = new(1, 1);
+
+    private Task? _runTask;
+    private CancellationTokenSource? _runCts;
+    private string _currentInterface = "can0";
+    private bool _disposed;
+
+    public bool IsRunning => _runTask is { IsCompleted: false };
+    public string CurrentInterface => _currentInterface;
+
+    public VartaChargerSimulatorService(
+        IConfiguration configuration,
+        ILogger<VartaChargerSimulatorService> logger,
+        ICanOpenManager canOpenManager)
+    {
+        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
+        _canOpenManager = canOpenManager ?? throw new ArgumentNullException(nameof(canOpenManager));
+
+        _options = new VartaChargerSimulatorOptions();
+        configuration.GetSection("Varta:Charger59VSimulator").Bind(_options);
+    }
+
+    public async Task StartAsync(CancellationToken cancellationToken)
+    {
+        if (_options.Enabled)
+        {
+            await StartSimulatorAsync(_options.CanInterface, cancellationToken);
+            return;
+        }
+
+        _logger.LogInformation("Varta Charger59V simulator is disabled at startup.");
+    }
+
+    public async Task StopAsync(CancellationToken cancellationToken)
+    {
+        await StopSimulatorAsync(cancellationToken);
+    }
+
+    public async Task<bool> StartSimulatorAsync(string? canInterface = null, CancellationToken cancellationToken = default)
+    {
+        ThrowIfDisposed();
+
+        await _controlLock.WaitAsync(cancellationToken);
+        try
+        {
+            if (_runTask is { IsCompleted: false })
+            {
+                return false;
+            }
+
+            _currentInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
+            _runCts = new CancellationTokenSource();
+            _runTask = RunSimulatorAsync(_currentInterface, _runCts.Token);
+            return true;
+        }
+        finally
+        {
+            _controlLock.Release();
+        }
+    }
+
+    public async Task<bool> StopSimulatorAsync(CancellationToken cancellationToken = default)
+    {
+        ThrowIfDisposed();
+
+        Task? runTask;
+        CancellationTokenSource? runCts;
+
+        await _controlLock.WaitAsync(cancellationToken);
+        try
+        {
+            if (_runTask is not { IsCompleted: false } || _runCts is null)
+            {
+                return false;
+            }
+
+            runTask = _runTask;
+            runCts = _runCts;
+            _runTask = null;
+            _runCts = null;
+        }
+        finally
+        {
+            _controlLock.Release();
+        }
+
+        runCts.Cancel();
+        try
+        {
+            await runTask.WaitAsync(cancellationToken);
+        }
+        catch (OperationCanceledException)
+        {
+        }
+        finally
+        {
+            runCts.Dispose();
+        }
+
+        return true;
+    }
+
+    private async Task RunSimulatorAsync(string canInterface, CancellationToken token)
+    {
+        _logger.LogInformation("Starting Varta Charger59V simulator on CAN interface {CanInterface}", canInterface);
+
+        try
+        {
+            var simulator = new Charger59V(_canOpenManager, canInterface, token);
+            await simulator.RunAsync();
+        }
+        catch (OperationCanceledException) when (token.IsCancellationRequested)
+        {
+            _logger.LogInformation("Varta Charger59V simulator stopped.");
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "Varta Charger59V simulator crashed.");
+        }
+    }
+
+    public void Dispose()
+    {
+        if (_disposed)
+        {
+            return;
+        }
+
+        _disposed = true;
+        _runCts?.Cancel();
+        _runCts?.Dispose();
+        _controlLock.Dispose();
+    }
+
+    private void ThrowIfDisposed()
+    {
+        if (_disposed)
+        {
+            throw new ObjectDisposedException(nameof(VartaChargerSimulatorService));
+        }
+    }
+}
+
+public sealed class VartaChargerSimulatorOptions
+{
+    public bool Enabled { get; set; }
+    public string CanInterface { get; set; } = "can0";
+}
+
+public sealed class VartaChargerSimulatorStartRequest
+{
+    public string? CanInterface { get; set; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Varta/Vartaservice/Vartaservice.cs

@@ -0,0 +1,488 @@
+using RobotNet10.CANOpen;
+using RobotNet10.CANOpen.Interfaces;
+using System.Collections.Concurrent;
+
+namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
+/// <summary>
+/// Charger Simulator cho VARTA EasyBlade 59V
+/// Máy tính đóng vai Charger, giao tiếp với pin thật qua USB-CAN adapter.
+///
+/// ── Thông số cố định (theo Technical Spec V1.8) ──────────────────────
+///   Baud rate        : 250 kbit/s
+///   Charger Node ID  : 100 (0x64)
+///   Max voltage      : 58.8 V  (4 × 12V × 1.225 cells)
+///   Max current      : 25 A
+///   Heartbeat        : mỗi 1000 ms  →  COB-ID 0x764
+///   RPDO1            : mỗi 200 ms   →  COB-ID 0x1E4
+/// ─────────────────────────────────────────────────────────────────────
+/// </summary>
+public class Charger59V
+{
+    // ════════════════════════════════════════════════════════════════
+    //  ⚙️  THÔNG SỐ HARDCODE – CHỈNH TẠI ĐÂY NẾU CẦN
+    // ════════════════════════════════════════════════════════════════
+
+    // Điện áp tối đa charger có thể cung cấp (V)
+    // EasyBlade 59V: pin lithium 13S → max 54.6V, để an toàn dùng 58.8V
+    private const double MAX_VOLTAGE_V   = 58.8;
+
+    // Dòng tối đa charger có thể cung cấp (A)
+    private const double MAX_CURRENT_A   = 25.0;
+
+    // Điện áp thực đo được (báo lại pin trong RPDO1, byte 2-3)
+    // Lúc chưa sạc thực thì đặt bằng Max hoặc giá trị đo thực của nguồn
+    private const double ACTUAL_VOLTAGE_V = 54.0;
+
+    // Dòng thực đo được (báo lại pin trong RPDO1, byte 0-1)
+    private const double ACTUAL_CURRENT_A = 10.0;
+
+    // COB-ID (không đổi theo spec)
+    private const uint COB_HEARTBEAT = 0x764;   // gửi
+    private const uint COB_RPDO1     = 0x1E4;   // gửi
+    private const uint COB_SDO_TX    = 0x5E4;   // gửi (response về battery)
+    private const uint COB_SDO_RX    = 0x664;   // nhận (request từ battery)
+    private const uint COB_TPDO9     = 0x264;   // nhận (SoC, VReq, IReq)
+    private const uint COB_TPDO8     = 0x49B;   // nhận (charge control status)
+
+    // ── Giá trị raw (Q8 = ×256, Q4 = ×16) ───────────────────────────
+    private static readonly ushort RAW_MAX_VOLTAGE = (ushort)(MAX_VOLTAGE_V * 256);
+    private static readonly ushort RAW_MAX_CURRENT = (ushort)(MAX_CURRENT_A * 16);
+    private static readonly ushort RAW_ACT_VOLTAGE = (ushort)(ACTUAL_VOLTAGE_V * 256);
+    private static readonly ushort RAW_ACT_CURRENT = (ushort)(ACTUAL_CURRENT_A * 256);
+
+    // ════════════════════════════════════════════════════════════════
+    //  State
+    // ════════════════════════════════════════════════════════════════
+    private bool   _sdoInitDone      = false;
+    private bool   _chargeActive     = false;  // true sau khi set Bit12
+    private bool   _relayOpen        = true;   // true = relay mở, không có điện ra
+    private bool   _batteryCharging  = false;  // true khi pin báo đang vào trạng thái sạc
+
+    // Lưu lại giá trị SDO battery ghi vào charger
+    private byte   _batteryStatus    = 0;   // Object 0x6000
+    private byte   _chargeControl    = 0;   // Object 0x4200
+    private ushort _voltageReqRaw    = 0;   // Object 0x2276
+    private ushort _currentReqRaw    = 0;   // Object 0x6070
+
+    private readonly ICanOpenManager _canOpenManager;
+    private readonly string _canInterface;
+    private ICanBus? _can;
+    private readonly CancellationToken _ct;
+    private readonly ConcurrentQueue<CanFrameReceivedEventArgs> _rxQueue = new();
+    private readonly SemaphoreSlim _rxSignal = new(0);
+
+    public Charger59V(ICanOpenManager canOpenManager, string canInterface, CancellationToken ct)
+    {
+        _canOpenManager = canOpenManager ?? throw new ArgumentNullException(nameof(canOpenManager));
+        _canInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
+        _ct = ct;
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    public async Task RunAsync()
+    {
+        _can = await _canOpenManager.GetOrCreateCanBusAsync(_canInterface, _ct);
+        _can.FrameReceived += OnFrameReceived;
+
+        // Log($"Max Voltage : {MAX_VOLTAGE_V} V  (raw Q8 = {RAW_MAX_VOLTAGE})");
+        // Log($"Max Current : {MAX_CURRENT_A} A  (raw Q4 = {RAW_MAX_CURRENT})");
+        // Log($"Gửi Heartbeat 0x{COB_HEARTBEAT:X3} mỗi 1000ms...");
+        // Log("Đang chờ pin kết nối...\n");
+
+        try
+        {
+            // Chạy song song 3 vòng lặp
+            await Task.WhenAll(
+                HeartbeatLoopAsync(),   // gửi HB mỗi 1000ms
+                ReceiveLoopAsync(),     // nhận SDO + TPDO từ pin
+                Rpdo1LoopAsync()        // gửi RPDO1 sau khi SDO init xong
+            );
+        }
+        finally
+        {
+            _can.FrameReceived -= OnFrameReceived;
+            while (_rxQueue.TryDequeue(out _)) { }
+            while (_rxSignal.Wait(0)) { }
+        }
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  VÒNG LẶP 1 – Heartbeat (mỗi 1000ms)
+    // ════════════════════════════════════════════════════════════════
+    private async Task HeartbeatLoopAsync()
+    {
+        while (!_ct.IsCancellationRequested)
+        {
+            // NMT Heartbeat: 1 byte [0x05] = Operational state
+            SendFrame(COB_HEARTBEAT, [0x05]);
+            // Dim($"♥ HB → 0x{COB_HEARTBEAT:X3}");
+            await Task.Delay(1000, _ct);
+        }
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  VÒNG LẶP 2 – Nhận frame từ pin
+    // ════════════════════════════════════════════════════════════════
+    private async Task ReceiveLoopAsync()
+    {
+        while (!_ct.IsCancellationRequested)
+        {
+            if (TryReceive(out var frame))
+            {
+                switch (frame.CanId)
+                {
+                    case COB_SDO_RX:  HandleSdo(frame.Data);   break;
+                    case COB_TPDO9:   HandleTpdo9(frame.Data); break;
+                    case COB_TPDO8:   HandleTpdo8(frame.Data); break;
+                }
+            }
+            else
+            {
+                await Task.Delay(1, _ct);   // yield CPU khi không có frame
+            }
+        }
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  VÒNG LẶP 3 – Gửi RPDO1 (mỗi 200ms, sau khi SDO init xong)
+    // ════════════════════════════════════════════════════════════════
+    private async Task Rpdo1LoopAsync()
+    {
+        // Chờ SDO init hoàn tất
+        while (!_sdoInitDone && !_ct.IsCancellationRequested)
+            await Task.Delay(50, _ct);
+
+        if (_ct.IsCancellationRequested) return;
+
+        // Delay 1s trước khi kích hoạt Bit12 (cho pin ổn định)
+        LogOk("SDO init xong! Chờ 1s rồi bật Bit12...");
+        await Task.Delay(1000, _ct);
+
+        // Bật relay và charge mode
+        _relayOpen    = false;
+        _chargeActive = true;
+        LogOk("==> Bit12 SET – Pin đang chuyển sang CHARGE MODE!");
+
+        // Gửi RPDO1 mỗi 200ms
+        while (!_ct.IsCancellationRequested)
+        {
+            SendRpdo1();
+            await Task.Delay(200, _ct);
+        }
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  GỬI RPDO1  (COB-ID 0x1E4)
+    //
+    //  Byte 0-1: Charging Current   [1/256 A, Q8]
+    //  Byte 2-3: Charging Voltage   [1/256 V, Q8]
+    //  Byte 4-5: Max avail Current  [1/16 A,  Q4]
+    //  Byte 6-7: Extended Charger Status
+    //            → Bit12 (0x1000) = kích hoạt charge mode
+    // ════════════════════════════════════════════════════════════════
+    private void SendRpdo1()
+    {
+        ushort extStatus = (_chargeActive && !_relayOpen)
+                           ? (ushort)0x1000   // Bit12 set
+                           : (ushort)0x0000;
+
+        byte[] data =
+        [
+            (byte)(RAW_ACT_CURRENT & 0xFF), (byte)(RAW_ACT_CURRENT >> 8), // Byte 0-1
+            (byte)(RAW_ACT_VOLTAGE & 0xFF), (byte)(RAW_ACT_VOLTAGE >> 8), // Byte 2-3
+            (byte)(RAW_MAX_CURRENT & 0xFF), (byte)(RAW_MAX_CURRENT >> 8), // Byte 4-5
+            (byte)(extStatus       & 0xFF), (byte)(extStatus       >> 8), // Byte 6-7
+        ];
+
+        SendFrame(COB_RPDO1, data);
+        // Dim($"→ RPDO1 0x{COB_RPDO1:X3} [{string.Join(" ", data.Select(b => $"{b:X2}"))}] " +
+            // $"ExtStat=0x{extStatus:X4}");
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  XỬ LÝ SDO REQUEST TỪ PIN  (COB-ID 0x664)
+    // ════════════════════════════════════════════════════════════════
+    private void HandleSdo(byte[] d)
+    {
+        if (d.Length < 8) return;
+        byte   cmd   = d[0];
+        ushort index = (ushort)(d[1] | (d[2] << 8));
+        byte   sub   = d[3];
+
+        switch (cmd)
+        {
+            // Pin GHI vào object của charger
+            case 0x2F: // Write 1 byte
+                OnWrite(index, sub, d[4], 0);
+                break;
+            case 0x2B: // Write 2 bytes
+                OnWrite(index, sub, d[4], (ushort)(d[4] | (d[5] << 8)));
+                break;
+            case 0x23: // Write 4 bytes
+                SdoWriteOk(index, sub); // phản hồi OK, bỏ qua giá trị
+                break;
+
+            // Pin ĐỌC object từ charger
+            case 0x40: // Read request
+                OnRead(index, sub);
+                break;
+        }
+    }
+
+    private void OnWrite(ushort index, byte sub, byte val8, ushort val16)
+    {
+        switch (index)
+        {
+            case 0x6000: // Battery Status
+                _batteryStatus = val8;
+                // Log($"  [SDO] 0x6000 Battery Status ← {val8}  " +
+                //     (val8 == 1 ? "→ Relay CLOSED (power ON)" : "→ Relay OPEN (power OFF)"));
+                _relayOpen = (val8 == 0);
+                break;
+
+            case 0x4200: // Charge Control
+                _chargeControl = val8;
+                // Log($"  [SDO] 0x4200 Charge Control ← {val8}  " +
+                //     (val8 == 1 ? "→ Battery READY" : "→ Battery NOT ready"));
+                // ChargeControl=0 → pin báo full/lỗi → tắt relay
+                if (val8 == 0 && _sdoInitDone)
+                {
+                    LogWarn("ChargeControl=0 → TẮT RELAY (pin đầy hoặc lỗi)");
+                    _relayOpen    = true;
+                    _chargeActive = false;
+                }
+                break;
+
+            case 0x2276: // Voltage Request
+                _voltageReqRaw = val16;
+                // Log($"  [SDO] 0x2276 Voltage Request ← {val16 / 256.0:F3} V");
+                break;
+
+            case 0x6070: // Current Request
+                _currentReqRaw = val16;
+                // Log($"  [SDO] 0x6070 Current Request ← {val16 / 16.0:F3} A");
+                break;
+
+            default:
+                // Log($"  [SDO] Write idx=0x{index:X4}.{sub} val=0x{val16:X4}");
+                break;
+        }
+        SdoWriteOk(index, sub);
+        CheckInitComplete();
+    }
+
+    private void OnRead(ushort index, byte sub)
+    {
+        switch (index)
+        {
+            case 0x4208: // Max Charging Voltage
+                SdoReadOk2(index, sub, RAW_MAX_VOLTAGE);
+                // Log($"  [SDO] 0x4208 Max Voltage → {MAX_VOLTAGE_V} V  (raw=0x{RAW_MAX_VOLTAGE:X4})");
+                break;
+
+            case 0x4212: // Max Charging Current
+                SdoReadOk2(index, sub, RAW_MAX_CURRENT);
+                // Log($"  [SDO] 0x4212 Max Current → {MAX_CURRENT_A} A  (raw=0x{RAW_MAX_CURRENT:X4})");
+                break;
+
+            default:
+                // Abort: object does not exist
+                byte[] abort = [0x80,
+                    (byte)(index & 0xFF), (byte)(index >> 8), sub,
+                    0x00, 0x00, 0x02, 0x06];
+                SendFrame(COB_SDO_TX, abort);
+                break;
+        }
+        CheckInitComplete();
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  XỬ LÝ TPDO9  (COB-ID 0x264)  – Pin gửi mỗi 100ms
+    //  Byte 0: ChargeControl  Byte 1: SoC
+    //  Byte 3-4: Volt Request  Byte 5-6: Curr Request  Byte 7: BattStatus
+    // ════════════════════════════════════════════════════════════════
+    private void HandleTpdo9(byte[] d)
+    {
+        if (d.Length < 7) return;
+        byte   cc   = d[0];
+        byte   soc  = d[1];
+        ushort vReq = (ushort)(d[3] | (d[4] << 8));
+        ushort iReq = (ushort)(d[5] | (d[6] << 8));
+        byte   bs   = d.Length > 7 ? d[7] : (byte)0;
+
+        Console.ForegroundColor = ConsoleColor.Green;
+        // Console.WriteLine(
+        //     $"[{Now}] 📦 PIN   " +
+        //     $"SoC={soc,3}%  " +
+        //     $"VReq={vReq / 256.0,6:F2}V  " +
+        //     $"IReq={iReq / 16.0,6:F2}A  " +
+        //     $"ChargeCtrl={cc}  BattStat={bs}");
+        // Console.ResetColor();
+
+        // Pin gửi ChargeControl=0 → pin đầy hoặc có lỗi → dừng sạc
+        if (cc == 0 && _sdoInitDone && _chargeActive)
+        {
+            LogWarn("ChargeControl=0 → PIN ĐẦY hoặc LỖI → Tắt relay!");
+            _chargeActive = false;
+            _relayOpen    = true;
+        }
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  XỬ LÝ TPDO8  (COB-ID 0x49B) – Battery Charge Control Status
+    // ════════════════════════════════════════════════════════════════
+    private void HandleTpdo8(byte[] d)
+    {
+        if (d.Length < 2) return;
+        ushort s = (ushort)(d[0] | (d[1] << 8));
+        bool chargingNow = s == 0xC011 || s == 0xC033;
+
+        if (chargingNow && !_batteryCharging)
+        {
+            _batteryCharging = true;
+            LogOk($"✅ PIN ĐÃ VÀO TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
+        }
+        else if (!chargingNow && _batteryCharging)
+        {
+            _batteryCharging = false;
+            LogWarn($"PIN THOÁT TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
+        }
+
+        string desc = s switch
+        {
+            0x0033 => "SDO init OK – chờ Bit12",
+            0x4033 => "Standby – chờ Bit12",
+            0xC011 => "⚡ CHARGING ACTIVE",
+            0xC033 => "⚡ Charging (normal)",
+            0xC000 => "Pin đầy – về standby",
+            0xD000 => "Keep-power hết – SHUTDOWN",
+            _      => $"bits={s:X4}"
+        };
+        Console.ForegroundColor = ConsoleColor.Cyan;
+        // Console.WriteLine($"[{Now}] 📊 STATUS 0x{s:X4} → {desc}");
+        Console.ResetColor();
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  Kiểm tra SDO init sequence đã đủ 4 bước chưa
+    // ════════════════════════════════════════════════════════════════
+    private void CheckInitComplete()
+    {
+        if (_sdoInitDone) return;
+        if (_batteryStatus == 1
+         && _chargeControl == 1
+         && _voltageReqRaw >  0
+         && _currentReqRaw >  0)
+        {
+            _sdoInitDone = true;
+            // Console.ForegroundColor = ConsoleColor.Yellow;
+            // Console.WriteLine($"\n[{Now}] ══════════════════════════════════════");
+            // Console.WriteLine($"[{Now}]  ✅ SDO INITIALIZATION HOÀN TẤT!");
+            // Console.WriteLine($"[{Now}]  BatteryStatus={_batteryStatus}  ChargeControl={_chargeControl}");
+            // Console.WriteLine($"[{Now}]  VoltReq={_voltageReqRaw / 256.0:F3}V  CurrReq={_currentReqRaw / 16.0:F3}A");
+            // Console.WriteLine($"[{Now}] ══════════════════════════════════════\n");
+            // Console.ResetColor();
+        }
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  SDO helpers
+    // ════════════════════════════════════════════════════════════════
+    private void SdoWriteOk(ushort index, byte sub)
+    {
+        byte[] d = [0x60, (byte)(index & 0xFF), (byte)(index >> 8), sub, 0, 0, 0, 0];
+        SendFrame(COB_SDO_TX, d);
+        // Dim($"← SDO OK   0x{COB_SDO_TX:X3} idx=0x{index:X4}");
+    }
+
+    private void SdoReadOk2(ushort index, byte sub, ushort value)
+    {
+        byte[] d = [0x4B,
+                    (byte)(index & 0xFF), (byte)(index >> 8), sub,
+                    (byte)(value & 0xFF), (byte)(value >> 8), 0, 0];
+        SendFrame(COB_SDO_TX, d);
+        // Dim($"← SDO RSP  0x{COB_SDO_TX:X3} idx=0x{index:X4} val=0x{value:X4}");
+    }
+
+    private void SendFrame(uint canId, byte[] data)
+    {
+        var bus = _can;
+        if (bus is null || !bus.IsConnected)
+        {
+            return;
+        }
+
+        bus.SendFrameAsync(canId, data, _ct).GetAwaiter().GetResult();
+    }
+
+    private bool TryReceive(out CanFrameReceivedEventArgs frame)
+    {
+        if (_rxQueue.TryDequeue(out frame!))
+        {
+            return true;
+        }
+
+        try
+        {
+            if (!_rxSignal.Wait(10, _ct))
+            {
+                frame = null!;
+                return false;
+            }
+        }
+        catch (OperationCanceledException)
+        {
+            frame = null!;
+            return false;
+        }
+
+        if (_rxQueue.TryDequeue(out frame!))
+        {
+            return true;
+        }
+
+        frame = null!;
+        return false;
+    }
+
+    private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
+    {
+        _rxQueue.Enqueue(e);
+        try
+        {
+            _rxSignal.Release();
+        }
+        catch (SemaphoreFullException)
+        {
+        }
+    }
+
+    // ════════════════════════════════════════════════════════════════
+    //  Logging
+    // ════════════════════════════════════════════════════════════════
+    private static string Now => DateTime.Now.ToString("HH:mm:ss.fff");
+
+    private static void Log(string msg)
+        => Console.WriteLine($"[{Now}] {msg}");
+
+    private static void LogOk(string msg)
+    {
+        Console.ForegroundColor = ConsoleColor.Yellow;
+        Console.WriteLine($"[{Now}] {msg}");
+        Console.ResetColor();
+    }
+
+    private static void LogWarn(string msg)
+    {
+        Console.ForegroundColor = ConsoleColor.Red;
+        Console.WriteLine($"[{Now}] ⚠️  {msg}");
+        Console.ResetColor();
+    }
+
+    private static void Dim(string msg)
+    {
+        Console.ForegroundColor = ConsoleColor.DarkGray;
+        Console.WriteLine($"[{Now}] {msg}");
+        Console.ResetColor();
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/CRCTable.cs

@@ -0,0 +1,159 @@
+namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
+{
+    public class CRCTable
+    {
+        public static readonly byte[] CRC8Table =
+       {
+             0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
+            157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
+            35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
+            190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
+            70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
+            219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
+            101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
+            248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
+            140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
+            17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
+            175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
+            50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
+            202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
+            87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
+            233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
+            116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
+        };
+        public static readonly ushort[] CRC16Table =
+        {
+            0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+            0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+            0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+            0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+            0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+            0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+            0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+            0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+            0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+            0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+            0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+            0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+            0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+            0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+            0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+            0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+            0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+            0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+            0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+            0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+            0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+            0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+            0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+            0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+            0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+            0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+            0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+            0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+            0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+            0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+            0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+            0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+        };
+
+        public static readonly uint[] CRC32Table =
+        {
+             0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
+             0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+             0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+             0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+             0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
+             0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+             0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
+             0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+             0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+             0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+             0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
+             0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+             0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
+             0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+             0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+             0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+             0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
+             0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+             0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
+             0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+             0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+             0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+             0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
+             0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+             0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
+             0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+             0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+             0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+             0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
+             0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+             0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
+             0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+             0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+             0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+             0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
+             0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+             0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
+             0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+             0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+             0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+             0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
+             0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+             0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
+             0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+             0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+             0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+             0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
+             0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+             0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
+             0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+             0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+             0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+             0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
+             0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+             0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
+             0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+             0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+             0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+             0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
+             0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+             0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
+             0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+             0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+             0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+        };
+        public static byte CRC8_Table(byte[] buffer, int counter)
+        {
+            return CRC8_Table(buffer.AsSpan(0, counter));
+        }
+
+        public static byte CRC8_Table(ReadOnlySpan<byte> buffer)
+        {
+            byte crc8 = 0;
+            foreach (byte value in buffer)
+            {
+                byte new_index = (byte)(crc8 ^ value);
+                crc8 = CRC8Table[new_index];
+            }
+            return crc8;
+        }
+
+        // Fixed CRC16_Table method - accepts byte[] and int counter parameter
+        public static ushort CRC16_Table(byte[] buffer, int counter)
+        {
+            return CRC16_Table(buffer.AsSpan(0, counter));
+        }
+
+        public static ushort CRC16_Table(ReadOnlySpan<byte> buffer)
+        {
+            ushort crc16 = 0;
+            foreach (byte value in buffer)
+            {
+                crc16 = (ushort)(CRC16Table[((crc16 >> 8) ^ value) & 0xFF] ^ (crc16 << 8));
+            }
+            return crc16;
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/README.md

@@ -0,0 +1,19 @@
+1.Config USB
+    $sudo chmod 666 /dev/ttyUSB0
+    $sudo nano /etc/udev/rules.d/99-usb-serial.rules
+    ``SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", MODE="0666", GROUP="dialout"``
+2. Kill process robotnet10
+    $ps aux | grep -i robotnet | grep -v grep
+    $lsof /dev/ttyUSB0 2>&1 || echo "Port is free"
+    $kill 2381 4242 8011
+    $kill -9 2381 4242 8011
+    $lsof /dev/ttyUSB0
+    $./run.sh
+3. HTML test 
+    # Option A: Dùng default browser
+    xdg-open /home/robotics/sonvh/odometry_comparison_test.html
+
+    # Option B: Dùng specific browser
+    firefox /home/robotics/sonvh/odometry_comparison_test.html
+    # hoặc
+    google-chrome /home/robotics/sonvh/odometry_comparison_test.html

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecN100IMU.cs

@@ -0,0 +1,863 @@
+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<bool>? _calibrationCompletedTcs;
+        private double _calibrationSumAccX, _calibrationSumAccY, _calibrationSumAccZ;
+        private double _calibrationSumGx, _calibrationSumGy, _calibrationSumGz;
+        private double _calibrationSumRoll, _calibrationSumPitch, _calibrationSumYaw;
+        private int _calibrationCount;
+        private double _accBiasX, _accBiasY, _accBiasZ;
+        private double _gyroBiasX, _gyroBiasY, _gyroBiasZ;
+        private double _orientationBiasRoll, _orientationBiasPitch, _orientationBiasYaw;
+
+        // Yaw integration: tich phan CalibratedGz thay vi dung firmware AHRS Yaw (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<AccelerationChangedEventArgs>? AccelerationChanged;
+        public event EventHandler<AngularVelocityChangedEventArgs>? AngularVelocityChanged;
+        public event EventHandler<MagnetometerChangedEventArgs>? MagnetometerChanged;
+        public event EventHandler<OrientationChangedEventArgs>? OrientationChanged;
+
+        // Event thong nhat cho SensorPipeline
+        public event EventHandler<ImuDataChangedEventArgs>? ImuDataChanged;
+
+        public WheeltecN100IMU(string deviceId, string deviceName, IConfigurationSection connection)
+            : base(deviceId, deviceName, DeviceType.Imu)
+        {
+            _portName = connection.GetValue<string>("Port") ?? throw new Exception("Port is required");
+            _baudRate = connection.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
+            _timeOut = connection.GetValue<int?>("TimeOut") ?? throw new Exception("Timeout is required");
+
+            IMU = new WheeltecReader(_portName, _baudRate, _timeOut);
+
+            // Continuous IMU data printing (optional)
+            _printDataEnabled = connection.GetValue<bool?>("DebugEnabled") ?? false;
+            _printDataInterval = TimeSpan.FromMilliseconds(connection.GetValue<int?>("PrintDataIntervalMs") ?? 200);
+
+            // Doc cau hinh neu co
+            var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
+            var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
+            var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
+
+            if (autoReconnectEnabled.HasValue)
+                AutoReconnectEnabled = autoReconnectEnabled.Value;
+            else
+                AutoReconnectEnabled = true;
+
+            if (reconnectDelayMs.HasValue)
+                ReconnectDelayMs = reconnectDelayMs.Value;
+
+            if (maxReconnectAttempts.HasValue)
+                MaxReconnectAttempts = maxReconnectAttempts.Value;
+
+            _cachedAcceleration = CreateAccelStamped(0, 0, 9.81, DateTime.UtcNow);
+            _cachedAngularVelocity = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
+            _cachedMagnetometer = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
+            _cachedOrientation = CreateVector3Stamped(0, 0, 0, DateTime.UtcNow);
+            _cachedQuaternion = CreateQuaternionStamped(1, 0, 0, 0, DateTime.UtcNow);
+
+            // Khoi tao gia tri properties
+            UpdateProperties();
+        }
+
+        protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
+        {
+            yield return new PropertyDescription("IsCalibrated", "Calibrated", "Trang thai calibrate")
+            {
+                DataType = "boolean",
+                IsReadOnly = true,
+                DisplayOrder = 1,
+                Category = "Trang thai",
+                DefaultValue = "true"
+            };
+
+            yield return new PropertyDescription("SampleRate", "Sample Rate (Hz)", "Tan so lay mau (Hz)")
+            {
+                DataType = "number",
+                IsReadOnly = true,
+                DisplayOrder = 2,
+                Category = "Cau hinh",
+                DefaultValue = "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<bool>();
+
+            // 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<bool>();
+            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<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+        {
+            return Task.FromResult(IMU.IsConnected);
+        }
+
+        /// <summary>
+        /// Reset toan bo trang thai calibration ve gia tri ban dau
+        /// </summary>
+        private void ResetCalibrationState()
+        {
+            _calibrationStartUtc = null;
+            _calibrationDone = false;
+            _isCalibrated = false;
+            _calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
+            _calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
+            _calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
+            _calibrationCount = 0;
+            _accBiasX = _accBiasY = _accBiasZ = 0;
+            _gyroBiasX = _gyroBiasY = _gyroBiasZ = 0;
+            _orientationBiasRoll = _orientationBiasPitch = _orientationBiasYaw = 0;
+            _integratedYaw = 0;
+            _lastIntegrationTime = DateTime.MinValue;
+        }
+
+        /// <summary>
+        /// Event handler cho DataReceived tu 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.
+        /// </summary>
+        private void IMU_DataReceived(object? sender, EventArgs e)
+        {
+            try
+            {
+                var snapshot = IMU.GetSnapshot();
+                // Use high-precision timer instead of DateTime.UtcNow (which has ~10-15ms precision)
+                var timestamp = _timerStartUtc + _highPrecisionTimer.Elapsed;
+
+                // Bat dau cua so calibration khi nhan du lieu lan dau
+                if (_calibrationStartUtc == null)
+                {
+                    _calibrationStartUtc = timestamp;
+                    _calibrationSumAccX = _calibrationSumAccY = _calibrationSumAccZ = 0;
+                    _calibrationSumGx = _calibrationSumGy = _calibrationSumGz = 0;
+                    _calibrationSumRoll = _calibrationSumPitch = _calibrationSumYaw = 0;
+                    _calibrationCount = 0;
+                }
+
+                if (!_calibrationDone)
+                {
+                    var calElapsed = timestamp - _calibrationStartUtc.Value;
+                    if (calElapsed < CalibrationDuration)
+                    {
+                        // Trong 2 giay dau: chi tich luy mau, khong cap nhat cache / fire events / sampleCount
+                        _calibrationSumAccX += snapshot.AccX;
+                        _calibrationSumAccY += snapshot.AccY;
+                        _calibrationSumAccZ += snapshot.AccZ;
+                        _calibrationSumGx += snapshot.Gx;
+                        _calibrationSumGy += snapshot.Gy;
+                        _calibrationSumGz += snapshot.Gz;
+                        _calibrationSumRoll += snapshot.Roll;
+                        _calibrationSumPitch += snapshot.Pitch;
+                        _calibrationSumYaw += snapshot.Yaw;
+                        _calibrationCount++;
+                        return;
+                    }
+
+                    // Het 2 giay: tinh bias va danh dau da calibrate
+                    if (_calibrationCount > 0)
+                    {
+                        double n = _calibrationCount;
+                        double meanAccX = _calibrationSumAccX / n;
+                        double meanAccY = _calibrationSumAccY / n;
+                        double meanAccZ = _calibrationSumAccZ / n;
+
+                        // Validate gravity magnitude — neu lech qua xa 9.81 thi robot bi rung/di chuyen
+                        double gravityMagnitude = Math.Sqrt(meanAccX * meanAccX + meanAccY * meanAccY + meanAccZ * meanAccZ);
+                        if (Math.Abs(gravityMagnitude - GravityMps2) > GravityToleranceMps2)
+                        {
+                            Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [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");
+            }
+        }
+
+        /// <summary>
+        /// Cap nhat properties hien thi
+        /// </summary>
+        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<AccelStamped> ReadAccelerationAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            return CachedAcceleration;
+        }
+
+        public async Task<Vector3Stamped> ReadAngularVelocityAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            return CachedAngularVelocity;
+        }
+
+        public async Task<Vector3Stamped?> ReadMagnetometerAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            return CachedMagnetometer;
+        }
+
+        public async Task<Vector3Stamped> ReadOrientationAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            return CachedOrientation;
+        }
+
+        public async Task<QuaternionStamped?> ReadQuaternionAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            return CachedQuaternion;
+        }
+
+        public async Task<Imu> ReadAllDataAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            lock (_dataLock)
+            {
+                return CreateImuFromCachedData();
+            }
+        }
+
+        private Imu CreateImuFromCachedData()
+        {
+            var timestamp = _lastUpdateTime != default ? _lastUpdateTime : DateTime.UtcNow;
+
+            var orientation = _cachedQuaternion.Quaternion;
+
+            var orientationCovariance = new double[Imu.OrientationCovarianceSize];
+            var angularVelocityCovariance = new double[Imu.AngularVelocityCovarianceSize];
+            var linearAccelerationCovariance = new double[Imu.LinearAccelerationCovarianceSize];
+
+            double gyroVariance = 1e-4;
+            angularVelocityCovariance[0] = gyroVariance;
+            angularVelocityCovariance[4] = gyroVariance;
+            angularVelocityCovariance[8] = gyroVariance;
+
+            double accelVariance = 1e-3;
+            linearAccelerationCovariance[0] = accelVariance;
+            linearAccelerationCovariance[4] = accelVariance;
+            linearAccelerationCovariance[8] = accelVariance;
+
+            orientationCovariance[0] = 0.001;
+            orientationCovariance[4] = 0.001;
+            orientationCovariance[8] = 0.002;
+
+            return new Imu(
+                header: new Header
+                {
+                    Stamp = timestamp,
+                    FrameId = "imu_frame"
+                },
+                orientation: orientation,
+                orientationCovariance: orientationCovariance,
+                angularVelocity: _cachedAngularVelocity.Vector,
+                angularVelocityCovariance: angularVelocityCovariance,
+                linearAcceleration: _cachedAcceleration.Accel.Linear,
+                linearAccelerationCovariance: linearAccelerationCovariance
+            );
+        }
+
+        public async Task<double?> ReadTemperatureAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            return CachedTemperature;
+        }
+
+        public async Task CalibrateAsync(CancellationToken cancellationToken = default)
+        {
+            // Huy event, reset calibration, dang ky lai event va doi calib
+            IMU.DataReceived -= IMU_DataReceived;
+            ResetCalibrationState();
+
+            _timerStartUtc = DateTime.UtcNow;
+            _highPrecisionTimer.Restart();
+
+            _calibrationCompletedTcs = new TaskCompletionSource<bool>();
+            IMU.DataReceived += IMU_DataReceived;
+
+            try
+            {
+                await Task.WhenAny(
+                    _calibrationCompletedTcs.Task,
+                    Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
+            }
+            catch (OperationCanceledException)
+            {
+                // cancellationToken bi huy
+            }
+            _calibrationCompletedTcs = null;
+            UpdateProperties();
+        }
+
+        public async Task CalibrateMagnetometerAsync(CancellationToken cancellationToken = default)
+        {
+            await Task.Delay(2000, cancellationToken);
+        }
+
+        public async Task ResetCalibrationAsync(CancellationToken cancellationToken = default)
+        {
+            // Huy event, reset state, dang ky lai va doi calib moi
+            IMU.DataReceived -= IMU_DataReceived;
+            ResetCalibrationState();
+
+            _timerStartUtc = DateTime.UtcNow;
+            _highPrecisionTimer.Restart();
+
+            _calibrationCompletedTcs = new TaskCompletionSource<bool>();
+            IMU.DataReceived += IMU_DataReceived;
+
+            try
+            {
+                await Task.WhenAny(
+                    _calibrationCompletedTcs.Task,
+                    Task.Delay(CalibrationWaitTimeout, cancellationToken)).ConfigureAwait(false);
+            }
+            catch (OperationCanceledException)
+            {
+                // cancellationToken bi huy
+            }
+            _calibrationCompletedTcs = null;
+            UpdateProperties();
+        }
+
+        public async Task SetSampleRateAsync(double sampleRate, CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+            _sampleRate = Math.Max(1, Math.Min(1000, sampleRate));
+            UpdateProperties();
+        }
+
+        public async Task SetAccelerometerRangeAsync(double range, CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+        }
+
+        public async Task SetGyroscopeRangeAsync(double range, CancellationToken cancellationToken = default)
+        {
+            await Task.CompletedTask;
+        }
+
+        #endregion
+
+        protected override void Dispose(bool disposing)
+        {
+            if (disposing)
+            {
+                IMU.DataReceived -= IMU_DataReceived;
+                IMU.Disconnect();
+                IMU.Dispose();
+                _printDataTimer?.Dispose();
+                _printDataTimer = null;
+            }
+            base.Dispose(disposing);
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/WheeltecIMU/WheeltecReader.cs

@@ -0,0 +1,620 @@
+using System;
+using System.Collections.Generic;
+using System.IO.Ports;
+using System.Threading;
+
+namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
+{
+    public class WheeltecReader : IDisposable
+    {
+        // Buffer nho de tich luy du lieu cho den khi co du mot frame hoan chinh
+        // Frame lon nhat: INSGPS = 8 + 84 = 92 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
+        public event EventHandler? DataReceived;
+
+        // 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 uint Time_stamp { 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; }
+
+        public double Temp { get; private set; }
+
+        public double Rollspeed { get; private set; }
+        public double Pitchspeed { get; private set; }
+        public double Yawspeed { get; private set; }
+
+        const byte FRAME_HEAD = 0xFC;
+        const byte FRAME_END = 0xFD;
+
+        // Loai goi
+        const byte TYPE_IMU = 0x40;
+        const byte TYPE_AHRS = 0x41;
+        const byte TYPE_INSGPS = 0x42;
+        const byte TYPE_GROUND = 0xF0;
+
+        // Chieu dai payload
+        const byte IMU_LEN = 0x38;    // 56
+        const byte AHRS_LEN = 0x30;   // 48
+        const byte INSGPS_LEN = 0x54; // 84
+
+        // Dictionary de map datatype -> expectedLength
+        private static readonly Dictionary<byte, byte> DataTypeLengthMap = new()
+        {
+            { TYPE_IMU, IMU_LEN },
+            { TYPE_AHRS, AHRS_LEN },
+            { TYPE_INSGPS, INSGPS_LEN }
+        };
+
+        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;
+
+        public bool IsConnected => serial != null && serial.IsOpen;
+
+        public WheeltecReader(string portName, int baudRate, int timeOut)
+        {
+            _portName = portName;
+            _baudRate = baudRate;
+            _timeOut = timeOut;
+
+            serial = CreateSerialPort();
+        }
+
+        private SerialPort CreateSerialPort()
+        {
+            return new SerialPort()
+            {
+                PortName = _portName,
+                BaudRate = _baudRate,
+                ReadTimeout = _timeOut,
+                Parity = Parity.None,
+                StopBits = StopBits.One,
+                DataBits = 8,
+            };
+        }
+
+        /// <summary>
+        /// Snapshot structure de lay tat ca du lieu cung luc mot cach thread-safe
+        /// </summary>
+        public struct DataSnapshot
+        {
+            public double AccX, AccY, AccZ;
+            public double Gx, Gy, Gz;
+            public double MagX, MagY, MagZ;
+            public double Roll, Pitch, Yaw;
+            public double Temp;
+        }
+
+        /// <summary>
+        /// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe
+        /// Dam bao tat ca cac gia tri deu tu cung mot thoi diem
+        /// </summary>
+        public DataSnapshot GetSnapshot()
+        {
+            Volatile.Read(ref _memoryBarrier);
+            return new DataSnapshot
+            {
+                AccX = AccX,
+                AccY = AccY,
+                AccZ = AccZ,
+                Gx = Gx,
+                Gy = Gy,
+                Gz = Gz,
+                MagX = MagX,
+                MagY = MagY,
+                MagZ = MagZ,
+                Roll = Roll,
+                Pitch = Pitch,
+                Yaw = Yaw,
+                Temp = Temp
+            };
+        }
+
+        public void Connect()
+        {
+            // Reset frame buffer khi ket noi moi
+            _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();
+        }
+
+        /// <summary>
+        /// Khoi dong reading thread voi priority cao de doc du lieu tu SerialPort
+        /// </summary>
+        private void StartReadingThread()
+        {
+            if (_readingThread != null && _readingThread.IsAlive)
+                return;
+
+            _shouldRead = true;
+
+            // Tao moi CancellationTokenSource cho thread moi
+            _readingCts?.Dispose();
+            _readingCts = new CancellationTokenSource();
+
+            _readingThread = new Thread(() => ReadingThreadLoop(_readingCts.Token))
+            {
+                Name = "WheeltecIMU-Reading",
+                IsBackground = false,
+                Priority = ThreadPriority.Highest
+            };
+
+            _readingThread.Start();
+        }
+
+        /// <summary>
+        /// Dung reading thread
+        /// </summary>
+        private void StopReadingThread()
+        {
+            _shouldRead = false;
+
+            _readingCts?.Cancel();
+
+            if (_readingThread != null)
+            {
+                if (!_readingThread.Join(1000))
+                {
+                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Reading thread did not stop gracefully");
+                }
+                _readingThread = null;
+            }
+
+            // Dispose CancellationTokenSource sau khi thread da dung
+            _readingCts?.Dispose();
+            _readingCts = null;
+        }
+
+        /// <summary>
+        /// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu
+        /// Moi exception tu SerialPort deu duoc bat de tranh crash thread va tu dong
+        /// reconnect sau RECONNECT_BACKOFF_MS
+        /// </summary>
+        private void ReadingThreadLoop(CancellationToken cancellationToken)
+        {
+            Thread.BeginThreadAffinity();
+            byte[] readBuffer = new byte[256];
+
+            while (_shouldRead && !cancellationToken.IsCancellationRequested)
+            {
+                try
+                {
+                    if (serial == null || !serial.IsOpen)
+                    {
+                        SafeCloseSerial();
+                        serial = CreateSerialPort();
+                        serial.Open();
+                        _frameBufferLength = 0;
+                        serial.DiscardInBuffer();
+                        _lastFrameTicks = DateTime.UtcNow.Ticks;
+                        Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Connected to {_portName}");
+                    }
+
+                    InnerReadLoop(readBuffer, cancellationToken);
+                }
+                catch (OperationCanceledException)
+                {
+                    break;
+                }
+                catch (Exception ex)
+                {
+                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] IMU disconnected: {ex.Message}");
+                    SafeCloseSerial();
+                }
+
+                if (_shouldRead && !cancellationToken.IsCancellationRequested)
+                {
+                    cancellationToken.WaitHandle.WaitOne(RECONNECT_BACKOFF_MS);
+                }
+            }
+
+            SafeCloseSerial();
+            Thread.EndThreadAffinity();
+        }
+
+        /// <summary>
+        /// Inner loop doc du lieu tu serial port. Thoat khi port dong hoac co exception
+        /// de outer loop xu ly reconnect
+        /// </summary>
+        private void InnerReadLoop(byte[] readBuffer, CancellationToken cancellationToken)
+        {
+            while (_shouldRead && !cancellationToken.IsCancellationRequested
+                   && serial != null && serial.IsOpen)
+            {
+                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;
+
+                ProcessIncomingData(readBuffer, bytesRead);
+            }
+        }
+
+        /// <summary>
+        /// Dong va dispose SerialPort an toan, set serial=null de lan sau tao moi.
+        /// SerialPort sau IOException thuong khong Open() lai duoc tren Linux nen phai tao moi.
+        /// </summary>
+        private void SafeCloseSerial()
+        {
+            if (serial == null) return;
+
+            try
+            {
+                serial.Close();
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Error closing serial: {ex.Message}");
+            }
+
+            try { serial.Dispose(); }
+            catch { }
+
+            serial = null;
+        }
+
+        /// <summary>
+        /// Xu ly du lieu moi nhan duoc tu serial port
+        /// Them vao frame buffer va tim, parse cac frame hoan chinh
+        /// </summary>
+        private void ProcessIncomingData(byte[] data, int length)
+        {
+            int dataOffset = 0;
+
+            while (dataOffset < length)
+            {
+                ProcessCompleteFramesInBuffer();
+
+                int availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;
+
+                if (availableSpace == 0)
+                {
+                    RemoveIncompleteFrameAtStart();
+                    availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;
+
+                    if (availableSpace == 0)
+                    {
+                        Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Buffer still full after removing incomplete frame, clearing buffer");
+                        _frameBufferLength = 0;
+                        availableSpace = FRAME_BUFFER_SIZE;
+                    }
+                }
+
+                int bytesToAdd = Math.Min(length - dataOffset, availableSpace);
+                Array.Copy(data, dataOffset, _frameBuffer, _frameBufferLength, bytesToAdd);
+                _frameBufferLength += bytesToAdd;
+                dataOffset += bytesToAdd;
+
+                ProcessCompleteFramesInBuffer();
+            }
+        }
+
+        /// <summary>
+        /// Xu ly tat ca cac frame hoan chinh trong buffer hien tai
+        /// </summary>
+        private void ProcessCompleteFramesInBuffer()
+        {
+            while (_frameBufferLength > 0)
+            {
+                ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
+                int headIndex = bufferSpan.IndexOf(FRAME_HEAD);
+
+                if (headIndex < 0)
+                {
+                    _frameBufferLength = 0;
+                    break;
+                }
+
+                if (headIndex > 0)
+                {
+                    int remainingBytes = _frameBufferLength - headIndex;
+                    Array.Copy(_frameBuffer, headIndex, _frameBuffer, 0, remainingBytes);
+                    _frameBufferLength = remainingBytes;
+                    continue;
+                }
+
+                if (_frameBufferLength < 8)
+                {
+                    break;
+                }
+
+                byte datatype = _frameBuffer[1];
+                byte payloadLength = _frameBuffer[2];
+
+                if (!DataTypeLengthMap.TryGetValue(datatype, out byte expectedLength))
+                {
+                    int remainingBytes = _frameBufferLength - 1;
+                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
+                    _frameBufferLength = remainingBytes;
+                    continue;
+                }
+
+                if (payloadLength != expectedLength)
+                {
+                    int remainingBytes = _frameBufferLength - 1;
+                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
+                    _frameBufferLength = remainingBytes;
+                    continue;
+                }
+
+                int totalFrameLength = 8 + payloadLength;
+                if (_frameBufferLength < totalFrameLength)
+                {
+                    break;
+                }
+
+                if (_frameBuffer[7 + payloadLength] != FRAME_END)
+                {
+                    int remainingBytes = _frameBufferLength - 1;
+                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
+                    _frameBufferLength = remainingBytes;
+                    continue;
+                }
+
+                byte[] frame = new byte[totalFrameLength];
+                Array.Copy(_frameBuffer, 0, frame, 0, totalFrameLength);
+
+                int remainingAfterFrame = _frameBufferLength - totalFrameLength;
+                if (remainingAfterFrame > 0)
+                {
+                    Array.Copy(_frameBuffer, totalFrameLength, _frameBuffer, 0, remainingAfterFrame);
+                }
+                _frameBufferLength = remainingAfterFrame;
+
+                try
+                {
+                    if (ParseFrame(frame))
+                    {
+                        _lastFrameTicks = DateTime.UtcNow.Ticks;
+                        try
+                        {
+                            DataReceived?.Invoke(this, EventArgs.Empty);
+                        }
+                        catch (Exception ex)
+                        {
+                            Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] DataReceived subscriber error: {ex.Message}");
+                        }
+                    }
+                }
+                catch (Exception ex)
+                {
+                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Error parsing frame: {ex.Message}");
+                }
+            }
+        }
+
+        /// <summary>
+        /// Loai bo frame thieu o dau buffer va tim frame head tiep theo
+        /// </summary>
+        private void RemoveIncompleteFrameAtStart()
+        {
+            if (_frameBufferLength == 0)
+                return;
+
+            ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
+            int headIndex = bufferSpan.IndexOf(FRAME_HEAD);
+
+            if (headIndex < 0)
+            {
+                _frameBufferLength = 0;
+                return;
+            }
+
+            if (headIndex == 0)
+            {
+                if (_frameBufferLength < 8)
+                    return;
+
+                byte datatype = _frameBuffer[1];
+                if (!DataTypeLengthMap.TryGetValue(datatype, out byte expectedLength))
+                {
+                    int remainingAfterSkip = _frameBufferLength - 1;
+                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingAfterSkip);
+                    _frameBufferLength = remainingAfterSkip;
+                    return;
+                }
+
+                byte payloadLength = _frameBuffer[2];
+                if (payloadLength != expectedLength)
+                {
+                    int remainingAfterSkip = _frameBufferLength - 1;
+                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingAfterSkip);
+                    _frameBufferLength = remainingAfterSkip;
+                    return;
+                }
+
+                int totalFrameLength = 8 + payloadLength;
+                if (_frameBufferLength < totalFrameLength)
+                    return;
+
+                return;
+            }
+
+            int remainingAfterHead = _frameBufferLength - headIndex;
+            Array.Copy(_frameBuffer, headIndex, _frameBuffer, 0, remainingAfterHead);
+            _frameBufferLength = remainingAfterHead;
+        }
+
+        public void Disconnect()
+        {
+            StopReadingThread();
+
+            SafeCloseSerial();
+
+            _frameBufferLength = 0;
+            Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
+        }
+
+        private void DecodeIMU(byte[] payload)
+        {
+            Gx = BitConverter.ToSingle(payload, 0);
+            Gy = BitConverter.ToSingle(payload, 4);
+            Gz = BitConverter.ToSingle(payload, 8);
+            AccX = BitConverter.ToSingle(payload, 12);
+            AccY = BitConverter.ToSingle(payload, 16);
+            AccZ = BitConverter.ToSingle(payload, 20);
+            MagX = BitConverter.ToSingle(payload, 24);
+            MagY = BitConverter.ToSingle(payload, 28);
+            MagZ = BitConverter.ToSingle(payload, 32);
+            Temp = BitConverter.ToSingle(payload, 36);
+            Time_stamp = BitConverter.ToUInt32(payload, 40);
+            Volatile.Write(ref _memoryBarrier, 0);
+        }
+
+        private void DecodeAHRS(byte[] payload)
+        {
+            double rollspeed = BitConverter.ToSingle(payload, 0);
+            double pitchspeed = BitConverter.ToSingle(payload, 4);
+            double yawspeed = BitConverter.ToSingle(payload, 8);
+
+            double roll = BitConverter.ToSingle(payload, 12);
+            double pitch = BitConverter.ToSingle(payload, 16);
+            double yaw = BitConverter.ToSingle(payload, 20);
+
+            Rollspeed = rollspeed;
+            Pitchspeed = pitchspeed;
+            Yawspeed = yawspeed;
+            Roll = roll;
+            Pitch = pitch;
+            Yaw = yaw;
+            Volatile.Write(ref _memoryBarrier, 0);
+        }
+
+        private void DecodeINSGPS(byte[] payload)
+        {
+            double latitude = BitConverter.ToDouble(payload, 0);
+            double longitude = BitConverter.ToDouble(payload, 8);
+            double altitude = BitConverter.ToSingle(payload, 16);
+
+            double vn = BitConverter.ToSingle(payload, 20);
+            double ve = BitConverter.ToSingle(payload, 24);
+            double vd = BitConverter.ToSingle(payload, 28);
+
+            double roll = BitConverter.ToSingle(payload, 32);
+            double pitch = BitConverter.ToSingle(payload, 36);
+            double yaw = BitConverter.ToSingle(payload, 40);
+
+            double qw = BitConverter.ToSingle(payload, 44);
+            double qx = BitConverter.ToSingle(payload, 48);
+            double qy = BitConverter.ToSingle(payload, 52);
+            double qz = BitConverter.ToSingle(payload, 56);
+        }
+
+        /// <summary>
+        /// Parse mot frame hoan chinh tu buffer
+        /// </summary>
+        private bool ParseFrame(byte[] frame)
+        {
+            if (frame.Length < 8)
+                return false;
+
+            byte head = frame[0];
+            if (head != FRAME_HEAD)
+                return false;
+
+            byte datatype = frame[1];
+            byte length = frame[2];
+            byte sn = frame[3];
+            byte crc8 = frame[4];
+            byte crc16_h = frame[5];
+            byte crc16_l = frame[6];
+            ushort head_crc16 = (ushort)(crc16_l + (crc16_h << 8));
+
+            Span<byte> header = [head, datatype, length, sn];
+            byte crc8_calc = CRCTable.CRC8_Table(header);
+            if (crc8_calc != crc8)
+            {
+                throw new Exception($"CRC8 header error: recv={crc8:X2}, calc={crc8_calc:X2}");
+            }
+
+            if (frame[7 + length] != FRAME_END)
+            {
+                throw new Exception($"Frame end error: {BitConverter.ToString(frame)}");
+            }
+
+            ReadOnlySpan<byte> payload = frame.AsSpan(7, length);
+
+            ushort crc16_calc = CRCTable.CRC16_Table(payload);
+            if (crc16_calc != head_crc16)
+            {
+                throw new Exception($"CRC16 payload error: recv={head_crc16:X4}, calc={crc16_calc:X4}");
+            }
+
+            switch (datatype)
+            {
+                case TYPE_AHRS:
+                    DecodeAHRS([..payload]);
+                    break;
+                case TYPE_IMU:
+                    DecodeIMU([..payload]);
+                    break;
+                case TYPE_INSGPS:
+                    DecodeINSGPS([..payload]);
+                    break;
+            }
+
+            return true;
+        }
+
+        public void Dispose()
+        {
+            Dispose(true);
+            GC.SuppressFinalize(this);
+        }
+
+        protected virtual void Dispose(bool disposing)
+        {
+            if (disposing)
+            {
+                Disconnect();
+            }
+        }
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/YNZDH/ModbusRtuClient.cs

@@ -0,0 +1,380 @@
+using System.Diagnostics;
+using System.IO.Ports;
+using Microsoft.Extensions.Logging;
+
+namespace RobotNet10.RobotApp.Drivers.YNZDH;
+
+public class ModbusRtuClient : IDisposable
+{
+    private readonly ILogger? _logger;
+    private SerialPort? _port;
+
+    private readonly string _portName;
+    private readonly int _baud;
+    private readonly Parity _parity;
+    private readonly int _dataBits;
+    private readonly StopBits _stopBits;
+    private readonly int _readTimeoutMs;
+    private readonly int _writeTimeoutMs;
+
+    private readonly object _sync = new();
+    private bool _faulted = false;
+    private DateTime _lastRetry = DateTime.MinValue;
+    private int _retryDelayMs = 1000; // backoff min = 1s
+    private DateTime _lastDataTime = DateTime.MinValue;
+    private readonly int _dataTimeoutMs = 10_000; // 10 giây
+    private int _consecutiveFails = 0;
+    private readonly int _maxFails = 3; // sau 3 lần fail liên tiếp thì coi như lost
+
+    public bool IsFaulted => _faulted;
+
+    public ModbusRtuClient(string portName,
+        int baud = 9600,
+        Parity parity = Parity.None,
+        int dataBits = 8,
+        StopBits stopBits = StopBits.One,
+        int readTimeoutMs = 50,
+        int writeTimeoutMs = 50,
+        ILogger? logger = null)
+    {
+        _logger = logger;
+        _portName = portName;
+        _baud = baud;
+        _parity = parity;
+        _dataBits = dataBits;
+        _stopBits = stopBits;
+        _readTimeoutMs = readTimeoutMs;
+        _writeTimeoutMs = writeTimeoutMs;
+        _lastDataTime = DateTime.Now;
+        _consecutiveFails = 0;
+
+        EnsureConnected();
+    }
+
+    // -------------------------
+    // AUTO RECONNECT (giống TadaRs485Client)
+    // -------------------------
+    // NOTE: This method uses lock (_sync) which may cause contention if called from multiple threads.
+    // If called from a non-realtime thread, it may delay realtime polling thread.
+    private void EnsureConnected()
+    {
+        var ensureStartTicks = Stopwatch.GetTimestamp();
+        double lockAcquisitionMs = 0;
+        double checkTimeMs = 0;
+        double disposeTimeMs = 0;
+        double createTimeMs = 0;
+        double openTimeMs = 0;
+        int currentThreadId = Thread.CurrentThread.ManagedThreadId;
+        string? currentThreadName = Thread.CurrentThread.Name;
+        
+        var lockStartTicks = Stopwatch.GetTimestamp();
+        lock (_sync)
+        {
+            var lockEndTicks = Stopwatch.GetTimestamp();
+            lockAcquisitionMs = ((lockEndTicks - lockStartTicks) * 1000.0) / Stopwatch.Frequency;
+            
+            var checkStartTicks = Stopwatch.GetTimestamp();
+            if (_port != null && _port.IsOpen && !_faulted)
+            {
+                var checkEndTicks = Stopwatch.GetTimestamp();
+                checkTimeMs = ((checkEndTicks - checkStartTicks) * 1000.0) / Stopwatch.Frequency;
+                return;
+            }
+
+            if ((DateTime.Now - _lastRetry).TotalMilliseconds < _retryDelayMs)
+            {
+                var checkEndTicks = Stopwatch.GetTimestamp();
+                checkTimeMs = ((checkEndTicks - checkStartTicks) * 1000.0) / Stopwatch.Frequency;
+                return;
+            }
+            var checkEndTicks2 = Stopwatch.GetTimestamp();
+            checkTimeMs = ((checkEndTicks2 - checkStartTicks) * 1000.0) / Stopwatch.Frequency;
+
+            _lastRetry = DateTime.Now;
+
+            try
+            {
+                var disposeStartTicks = Stopwatch.GetTimestamp();
+                _port?.Dispose();
+                var disposeEndTicks = Stopwatch.GetTimestamp();
+                disposeTimeMs = ((disposeEndTicks - disposeStartTicks) * 1000.0) / Stopwatch.Frequency;
+                
+                var createStartTicks = Stopwatch.GetTimestamp();
+                _port = new SerialPort(_portName, _baud, _parity, _dataBits, _stopBits)
+                {
+                    ReadTimeout = _readTimeoutMs,
+                    WriteTimeout = _writeTimeoutMs,
+                    // Set buffer sizes to minimize kernel delays
+                    ReadBufferSize = 4096,
+                    WriteBufferSize = 4096
+                };
+                var createEndTicks = Stopwatch.GetTimestamp();
+                createTimeMs = ((createEndTicks - createStartTicks) * 1000.0) / Stopwatch.Frequency;
+                
+                var openStartTicks = Stopwatch.GetTimestamp();
+                _port.Open();
+                var openEndTicks = Stopwatch.GetTimestamp();
+                openTimeMs = ((openEndTicks - openStartTicks) * 1000.0) / Stopwatch.Frequency;
+
+                _faulted = false;
+                _retryDelayMs = 1000; // reset backoff
+            }
+            catch (Exception ex)
+            {
+                _logger?.LogError("[ModbusRtuClient] Connect failed: {ex.Message}", ex.Message);
+                _faulted = true;
+
+                // exponential backoff giống Tada
+                _retryDelayMs = Math.Min(_retryDelayMs * 2, 30_000);
+            }
+        }
+        
+        var ensureEndTicks = Stopwatch.GetTimestamp();
+        var ensureTotalMs = ((ensureEndTicks - ensureStartTicks) * 1000.0) / Stopwatch.Frequency;
+        
+        // Log if EnsureConnected took longer than 10ms (should be very fast if already connected)
+        if (ensureTotalMs > 10.0 && _logger != null)
+        {
+            _logger.LogWarning(
+                "[ModbusRtuClient] Slow EnsureConnected: Total={TotalMs:F1}ms, ThreadId={ThreadId}, ThreadName={ThreadName}, " +
+                "LockAcquisition={LockAcquisitionMs:F1}ms, Check={CheckTimeMs:F1}ms, " +
+                "Dispose={DisposeTimeMs:F1}ms, Create={CreateTimeMs:F1}ms, Open={OpenTimeMs:F1}ms. " +
+                "NOTE: High LockAcquisition time indicates lock contention from other threads.",
+                ensureTotalMs, currentThreadId, currentThreadName ?? "Unknown",
+                lockAcquisitionMs, checkTimeMs, disposeTimeMs, createTimeMs, openTimeMs);
+        }
+    }
+
+    private void CheckDataTimeout()
+    {
+        if (_lastDataTime != DateTime.MinValue &&
+            (DateTime.Now - _lastDataTime).TotalMilliseconds > _dataTimeoutMs)
+        {
+            _logger?.LogError("[ModbusRtuClient] Communication lost (data timeout).");
+            _faulted = true;
+            _lastDataTime = DateTime.MinValue; // reset để tránh spam log
+        }
+    }
+
+    public void ForceReconnect()
+    {
+        lock (_sync)
+        {
+            try
+            {
+                if (_port != null)
+                {
+                    try { if (_port.IsOpen) _port.Close(); } catch { }
+                    _port.Dispose();
+                    _port = null;
+                }
+            }
+            catch { }
+
+            _faulted = false;
+            _lastRetry = DateTime.MinValue;
+            _retryDelayMs = 1000;
+            _consecutiveFails = 0;
+            _lastDataTime = DateTime.Now;
+
+            EnsureConnected();
+        }
+    }
+
+    // -------------------------
+    // CRC
+    // -------------------------
+    private static ushort Crc16(byte[] data, int len)
+    {
+        ushort crc = 0xFFFF;
+        for (int i = 0; i < len; i++)
+        {
+            crc ^= data[i];
+            for (int j = 0; j < 8; j++)
+            {
+                bool lsb = (crc & 0x0001) != 0;
+                crc >>= 1;
+                if (lsb) crc ^= 0xA001;
+            }
+        }
+        return crc;
+    }
+
+    // -------------------------
+    // TX/RX WITH RECONNECT
+    // -------------------------
+    private byte[] TxRx(byte[] req, int respLen)
+    {
+        EnsureConnected();
+        if (_port == null || !_port.IsOpen || _faulted)
+            throw new Exception("Modbus port not available");
+
+        try
+        {
+            // Build frame
+            ushort crc = Crc16(req, req.Length);
+            byte[] frame = new byte[req.Length + 2];
+            Array.Copy(req, frame, req.Length);
+            frame[^2] = (byte)(crc & 0xFF);       // CRC Lo
+            frame[^1] = (byte)(crc >> 8 & 0xFF);  // CRC Hi
+
+            // Discard buffer and write
+            _port.DiscardInBuffer();
+            _port.DiscardOutBuffer();
+            _port.Write(frame, 0, frame.Length);
+
+            // Read response
+            byte[] buf = new byte[respLen];
+            int got = 0;
+            while (got < respLen)
+            {
+                int bytesToRead = respLen - got;
+                int bytesRead = _port.Read(buf, got, bytesToRead);  // may throw TimeoutException
+                got += bytesRead;
+            }
+
+            // Verify CRC
+            if (got < 3)
+            {
+                _consecutiveFails++;
+                if (_consecutiveFails >= _maxFails)
+                {
+                    _logger?.LogError("[ModbusRtuClient] Communication lost (response too short).");
+                    _faulted = true;
+                }
+                CheckDataTimeout();
+                throw new Exception("Response too short");
+            }
+
+            ushort rxCrc = (ushort)(buf[got - 2] | buf[got - 1] << 8);
+            ushort calc = Crc16(buf, got - 2);
+            if (rxCrc != calc)
+            {
+                _consecutiveFails++;
+                if (_consecutiveFails >= _maxFails)
+                {
+                    _logger?.LogError("[ModbusRtuClient] Communication lost (CRC mismatch).");
+                    _faulted = true;
+                }
+                CheckDataTimeout();
+                throw new Exception("CRC mismatch");
+            }
+
+            // Success - reset fail counter and update last data time
+            _lastDataTime = DateTime.Now;
+            _consecutiveFails = 0;
+            return buf;
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError("[ModbusRtuClient] IO error: {ExMessage}", ex.Message);
+            _consecutiveFails++;
+            if (_consecutiveFails >= _maxFails)
+            {
+                _logger?.LogError("[ModbusRtuClient] Communication lost (too many failed reads).");
+                _faulted = true;
+            }
+            CheckDataTimeout();
+            EnsureConnected(); // thử reconnect
+            throw;
+        }
+    }
+
+    /// <summary>
+    /// Read Holding Registers (FC 0x03)
+    /// </summary>
+    public ushort[] ReadHoldingRegisters(byte slave, ushort startAddr, ushort quantity)
+    {
+        byte[] pdu =
+        [
+            slave, 0x03,
+                (byte)(startAddr >> 8), (byte)(startAddr & 0xFF),
+                (byte)(quantity >> 8), (byte)(quantity & 0xFF),
+        ];
+
+        // Expected response: [slave][0x03][byteCount][data...][CRClo][CRChi]
+        int byteCount = quantity * 2;
+        int respLen = 3 + byteCount + 2;
+        var resp = TxRx(pdu, respLen);
+
+        if (resp[0] != slave || resp[1] != 0x03)
+        {
+            _consecutiveFails++;
+            if (_consecutiveFails >= _maxFails)
+            {
+                _logger?.LogError("[ModbusRtuClient] Communication lost (invalid response function).");
+                _faulted = true;
+            }
+            CheckDataTimeout();
+            throw new Exception("Invalid response function");
+        }
+
+        if (resp[2] != byteCount)
+        {
+            _consecutiveFails++;
+            if (_consecutiveFails >= _maxFails)
+            {
+                _logger?.LogError("[ModbusRtuClient] Communication lost (unexpected byte count).");
+                _faulted = true;
+            }
+            CheckDataTimeout();
+            throw new Exception("Unexpected byte count");
+        }
+
+        // Success - reset fail counter and update last data time (TxRx already did this, but ensure it's updated)
+        // Note: _consecutiveFails and _lastDataTime are already reset in TxRx() on success
+        _lastDataTime = DateTime.Now;
+
+        ushort[] regs = new ushort[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            int idx = 3 + i * 2;
+            regs[i] = (ushort)(resp[idx] << 8 | resp[idx + 1]); // Big-endian to ushort
+        }
+        return regs;
+    }
+
+    public void WriteMultipleRegisters(byte slave, ushort startAddr, ushort[] values)
+    {
+        int byteCount = values.Length * 2;
+        byte[] pdu = new byte[7 + byteCount];
+
+        pdu[0] = slave;
+        pdu[1] = 0x10;
+        pdu[2] = (byte)(startAddr >> 8);
+        pdu[3] = (byte)startAddr;
+        pdu[4] = (byte)(values.Length >> 8);
+        pdu[5] = (byte)values.Length;
+        pdu[6] = (byte)byteCount;
+
+        for (int i = 0; i < values.Length; i++)
+        {
+            pdu[7 + i * 2] = (byte)(values[i] >> 8);
+            pdu[7 + i * 2 + 1] = (byte)values[i];
+        }
+
+        int respLen = 8;
+        TxRx(pdu, respLen);
+    }
+
+    // -------------------------
+    // Dispose
+    // -------------------------
+    public void Dispose()
+    {
+        lock (_sync)
+        {
+            try
+            {
+                if (_port != null)
+                {
+                    if (_port.IsOpen) _port.Close();
+                    _port.Dispose();
+                }
+            }
+            catch { }
+            _port = null;
+        }
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/YNZDH/YNZDH_RfHandle.cs

@@ -0,0 +1,432 @@
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.Shared;
+using RobotNet10.Shared.Sensor;
+using System.Diagnostics;
+
+namespace RobotNet10.RobotApp.Drivers.YNZDH;
+
+[Device(DeviceType.RfHandle, "YNZDH", "YNZDH_RfHandle", "1.0.0",
+    Description = "YNZDH RF Handle (minimal but full simulation properties)")]
+public class YNZDH_RfHandle : DeviceBase, IRfHandle
+{
+    private readonly string _port;
+    private readonly int _baud;
+    private readonly ILogger<YNZDH_RfHandle> _logger;
+
+    private ModbusRtuClient? _modbus;
+    
+    // High-priority polling thread for real-time data acquisition
+    private Thread? _pollingThread;
+    private CancellationTokenSource? _pollingCts;
+    private volatile bool _shouldPoll = false;
+
+    private readonly Lock _lock = new();
+
+    public event Action? Updated;
+
+    public YNZDH_RfHandle(string deviceId, string deviceName, IConfigurationSection cfg, IServiceProvider serviceProvider)
+        : base(deviceId, deviceName, DeviceType.RfHandle)
+    {
+        _port = cfg.GetValue<string>("Port") ?? throw new Exception("Port is required");
+        _baud = cfg.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
+
+        var loggerFactory = serviceProvider.GetRequiredService<ILoggerFactory>();
+        _logger = loggerFactory.CreateLogger<YNZDH_RfHandle>();
+
+        AutoReconnectEnabled = true;
+        ReconnectDelayMs = 2000;
+        MaxReconnectAttempts = 0;
+
+        // Khởi tạo PropertyDescriptions → DeviceBase validation pass
+        UpdateProperties();
+    }
+
+    // ====================== STATES ===========================
+
+    public DateTime LastUpdateTime { get; private set; }
+
+    public int Heartbeat { get; private set; }
+    public bool RemoteReady { get; private set; }
+    public bool EStop { get; private set; }
+
+    public bool LiftUp { get; private set; }
+    public bool LiftDown { get; private set; }
+    public bool RotateLeft { get; private set; }
+    public bool RotateRight { get; private set; }
+
+    public bool ModeSelect { get; private set; }
+    public bool Enable { get; private set; }
+
+    public int Speed { get; private set; }          // 0–100
+    public double Linear { get; private set; }       // -1 → +1
+    public double Angular { get; private set; }      // -1 → +1
+
+    public RFMode Mode { get; private set; } = RFMode.None;
+
+    private Joy? _cachedJoyState;
+
+    // IRfHandle Implementation
+    public Joy? CurrentJoyState
+    {
+        get { lock (_lock) { return _cachedJoyState; } }
+    }
+
+    // ====================== SIM PROPERTIES ====================
+
+    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
+    {
+        return
+        [
+            new("Heartbeat", "Heartbeat"),
+            new("RemoteReady", "Remote Ready"),
+            new("EStop", "Emergency Stop"),
+
+            new("LiftUp", "Lift Up"),
+            new("LiftDown", "Lift Down"),
+            new("RotateLeft", "Rotate Left"),
+            new("RotateRight", "Rotate Right"),
+
+            new("ModeSelect", "Mode Select"),
+            new("Enable", "Enable"),
+
+            new("Speed", "Speed"),
+            new("Mode", "Mode"),
+
+            new("LastUpdate", "Last Update Time")
+        ];
+    }
+
+    private void UpdateProperties()
+    {
+        SetProperty("Heartbeat", Heartbeat.ToString());
+        SetProperty("RemoteReady", RemoteReady.ToString());
+        SetProperty("EStop", EStop.ToString());
+
+        SetProperty("LiftUp", LiftUp.ToString());
+        SetProperty("LiftDown", LiftDown.ToString());
+        SetProperty("RotateLeft", RotateLeft.ToString());
+        SetProperty("RotateRight", RotateRight.ToString());
+
+        SetProperty("ModeSelect", ModeSelect.ToString());
+        SetProperty("Enable", Enable.ToString());
+
+        SetProperty("Speed", Speed.ToString());
+        SetProperty("Mode", Mode.ToString());
+
+        SetProperty("LastUpdate", LastUpdateTime == default
+            ? "Never"
+            : LastUpdateTime.ToString("yyyy-MM-dd HH:mm:ss"));
+    }
+
+    // ====================== DEVICEBASE ========================
+
+    protected override Task OnInitializeAsync(CancellationToken cancellationToken)
+    {
+        try
+        {
+            //_modbus = new ModbusRtuClient(_port, _baud, logger: _logger);
+            _modbus = new ModbusRtuClient(_port, _baud);
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "Modbus init failed");
+            LastError = ex;
+            OnErrorOccurred(ex, "Modbus init failed");
+        }
+
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnConnectAsync(CancellationToken cancellationToken)
+    {
+        StartPolling();
+        return Task.CompletedTask;
+    }
+
+    protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
+    {
+        StopPolling();
+        return Task.CompletedTask;
+    }
+
+    protected override void Dispose(bool disposing)
+    {
+        if (disposing)
+        {
+            // Stop polling thread before disposing base class
+            StopPolling();
+        }
+        base.Dispose(disposing);
+    }
+
+    protected override Task OnResetAsync(CancellationToken cancellationToken)
+    {
+        StopPolling();
+        StartPolling();
+        return Task.CompletedTask;
+    }
+
+    protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
+        => Task.FromResult(_modbus != null);
+
+    // ===================== POLLING LOOP ======================
+
+    /// <summary>
+    /// Start high-priority polling thread for real-time data acquisition at 10Hz
+    /// </summary>
+    private void StartPolling()
+    {
+        StopPolling(); // Đảm bảo không có thread nào đang chạy
+
+        _shouldPoll = true;
+        
+        // Tạo mới CancellationTokenSource cho thread mới
+        _pollingCts?.Dispose();
+        _pollingCts = new CancellationTokenSource();
+        
+        _pollingThread = new Thread(() => PollingThreadLoop(_pollingCts.Token))
+        {
+            Name = $"YNZDH-RfHandle-Polling-{DeviceId}",
+            IsBackground = false, // Không phải background thread để đảm bảo chạy liên tục
+            Priority = ThreadPriority.Highest // Priority cao để đảm bảo real-time polling
+        };
+        
+        _pollingThread.Start();
+        _logger.LogDebug("YNZDH_RfHandle: Started high-priority polling thread at 10Hz for device {DeviceId}", DeviceId);
+    }
+
+    /// <summary>
+    /// Stop polling thread gracefully
+    /// </summary>
+    private void StopPolling()
+    {
+        _shouldPoll = false;
+        
+        _pollingCts?.Cancel();
+        
+        if (_pollingThread != null)
+        {
+            if (!_pollingThread.Join(1000)) // Đợi tối đa 1 giây
+            {
+                _logger.LogWarning("YNZDH_RfHandle: Polling thread did not stop gracefully for device {DeviceId}", DeviceId);
+            }
+            _pollingThread = null;
+        }
+        
+        // Dispose CancellationTokenSource sau khi thread đã dừng
+        _pollingCts?.Dispose();
+        _pollingCts = null;
+    }
+
+    /// <summary>
+    /// High-priority polling thread loop - runs at 10Hz (100ms interval)
+    /// Uses Stopwatch for high-precision timing to ensure accurate 10Hz polling rate
+    /// </summary>
+    private void PollingThreadLoop(CancellationToken cancellationToken)
+    {
+        var modbusClient = _modbus;
+        if (modbusClient == null)
+        {
+            _logger.LogError("YNZDH_RfHandle: Modbus client is null");
+            return;
+        }
+
+        Thread.BeginThreadAffinity();
+        try
+        {
+            const int pollingIntervalMs = 100; // 10Hz = 100ms
+            var intervalTicks = pollingIntervalMs * TimeSpan.TicksPerMillisecond;
+            var stopwatch = Stopwatch.StartNew();
+            var nextPollTime = stopwatch.ElapsedTicks + intervalTicks;
+            var spinWait = new SpinWait();
+            long currentTicks = 0;
+
+            while (_shouldPoll && !cancellationToken.IsCancellationRequested)
+            {
+                currentTicks = stopwatch.ElapsedTicks;
+
+                // Check if it's time to poll
+                if (currentTicks >= nextPollTime)
+                {
+                    try
+                    {
+                        ushort[] regs = modbusClient.ReadHoldingRegisters(1, 1, 4);
+                        DecodeRegisters(regs);
+                    }
+                    catch (Exception ex)
+                    {
+                        LastError = ex;
+                        OnErrorOccurred(ex, "Polling error");
+                        ResetToDefaultValues();
+                    }
+
+                    // Calculate next poll time
+                    nextPollTime = currentTicks + intervalTicks;
+                }
+
+                // SpinWait for precise timing (10 spins then reset)
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.SpinOnce();
+                spinWait.Reset();
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.LogError(ex, "YNZDH_RfHandle: Error in polling thread loop for device {DeviceId}", DeviceId);
+            LastError = ex;
+            OnErrorOccurred(ex, "Polling thread error");
+        }
+        finally
+        {
+            Thread.EndThreadAffinity();
+        }
+    }
+
+    // ===================== DECODE ============================
+
+    private void ResetToDefaultValues()
+    {
+        lock (_lock)
+        {
+            Heartbeat = 0;
+            RemoteReady = false;
+            EStop = false;
+
+            LiftUp = false;
+            LiftDown = false;
+            RotateLeft = false;
+            RotateRight = false;
+
+            ModeSelect = false;
+            Enable = false;
+
+            Speed = 0;
+            Linear = 0.0;
+            Angular = 0.0;
+            Mode = RFMode.None;
+
+            _cachedJoyState = null;
+
+            UpdateProperties();
+        }
+
+        Updated?.Invoke();
+    }
+
+    private void DecodeRegisters(ushort[] regs)
+    {
+        byte d0 = (byte)(regs[0] >> 8); // Word0H
+        byte d1 = (byte)(regs[0]);      // Word0L
+        byte d2 = (byte)(regs[1] >> 8); // Word1H
+        byte d3 = (byte)(regs[1]);      // Word1L
+        byte d6 = (byte)(regs[3] >> 8); // Word3H (JOY FB)
+        byte d7 = (byte)(regs[3]);      // Word3L (JOY LR)
+        lock (_lock)
+        {
+            // ===== System =====
+            Heartbeat = (d0 >> 4) & 0x0F;
+            RemoteReady = (d0 & 0x04) != 0;
+            RemoteReady = !RemoteReady;
+            EStop = (d0 & 0x01) != 0;
+
+            // ===== Buttons =====
+            Enable = (d1 & 0x80) != 0;
+            ModeSelect = (d1 & 0x40) != 0;
+
+            LiftUp = (d1 & 0x01) != 0;
+            LiftDown = (d1 & 0x02) != 0;
+            RotateLeft = (d1 & 0x04) != 0;
+            RotateRight = (d1 & 0x08) != 0;
+
+            // ===== Speed =====
+            Speed = Math.Clamp((int)d3, 0, 100);
+            // ===== Mode =====
+            Mode = DecodeMode(d2);
+
+            // ===== Safety =====
+            if (!RemoteReady || !Enable || EStop)
+            {
+                Linear = 0;
+                Angular = 0;
+            }
+            else
+            {
+                // ===== Joystick ANALOG =====
+                Linear = (d6 - 127f) / 127f;
+                Angular = (127f - d7) / 127f;
+            }
+
+            LastUpdateTime = DateTime.UtcNow;
+
+            // Update cached JoyState
+            _cachedJoyState = CreateJoyStateFromCache();
+            
+            UpdateProperties();
+        }
+
+        Updated?.Invoke();
+    }
+
+    private static RFMode DecodeMode(byte d2) =>
+        (d2 & 0x0F) switch
+        {
+            0x00 => RFMode.Default,
+            0x01 => RFMode.Maintenance,
+            0x02 => RFMode.Override,
+            _ => RFMode.None
+        };
+
+    // ===================== IRfHandle Implementation ===========
+
+    public Task<Joy> ReadJoyStateAsync(CancellationToken cancellationToken = default)
+    {
+        lock (_lock)
+        {
+            if (_cachedJoyState.HasValue)
+            {
+                return Task.FromResult(_cachedJoyState.Value);
+            }
+            
+            return Task.FromResult(CreateJoyStateFromCache());
+        }
+    }
+
+    private Joy CreateJoyStateFromCache()
+    {
+        return new Joy
+        {
+            Header = new Header
+            {
+                Stamp = LastUpdateTime == default ? DateTime.UtcNow : LastUpdateTime,
+                FrameId = "rfhandle_frame"
+            },
+            Axes =
+            [
+                Linear,                 // Axis 0: Forward / Backward
+                Angular,                // Axis 1: Left / Right
+                Speed / 100f            // Axis 2: Speed
+            ],
+            Buttons =
+            [
+                LiftUp ? 1 : 0,
+                LiftDown ? 1 : 0,
+                RotateLeft ? 1 : 0,
+                RotateRight ? 1 : 0,
+                ModeSelect ? 1 : 0,
+                Enable ? 1 : 0,
+                EStop ? 1 : 0
+            ]
+        };
+    }
+
+}