Initial commit

srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs

@@ -0,0 +1,756 @@
+using System.Collections.Concurrent;
+using System.Text.RegularExpressions;
+using Microsoft.Extensions.Logging;
+using RobotNet10.CANOpen.Enums;
+using RobotNet10.CANOpen.Exceptions;
+using RobotNet10.CANOpen.Interfaces;
+using RobotNet10.CANOpen.Models;
+
+namespace RobotNet10.CANOpen.Services;
+
+/// <summary>
+/// PDO Manager để xử lý Transmit và Receive PDOs
+/// </summary>
+public class PdoManager : IDisposable
+{
+    private readonly ICanBus _canBus;
+    private readonly byte _nodeId;
+    private readonly ILogger<PdoManager>? _logger;
+    private readonly ConcurrentDictionary<byte, PdoConfiguration> _tpdoConfigs;
+    private readonly ConcurrentDictionary<byte, PdoConfiguration> _rpdoConfigs;
+    private readonly HashSet<byte> _configuredTpdoNumbers = new(); // Track TPDO đã configure thành công
+    private readonly HashSet<byte> _configuredRpdoNumbers = new(); // Track RPDO đã configure thành công
+    private readonly ConcurrentDictionary<byte, long> _lastUnconfiguredTpdoWarningTicks = new(); // Throttle repeated warnings
+    private const int UnconfiguredTpdoWarningIntervalMs = 60000; // Log at most once per minute per TPDO
+    private bool _disposed;
+    
+    public event EventHandler<PdoReceivedEventArgs>? PdoReceived;
+    
+    public PdoManager(ICanBus canBus, byte nodeId, ILogger<PdoManager>? logger = null)
+    {
+        _canBus = canBus ?? throw new ArgumentNullException(nameof(canBus));
+        _nodeId = nodeId;
+        _logger = logger;
+        _tpdoConfigs = new ConcurrentDictionary<byte, PdoConfiguration>();
+        _rpdoConfigs = new ConcurrentDictionary<byte, PdoConfiguration>();
+        
+        _canBus.FrameReceived += OnFrameReceived;
+    }
+    
+    /// <summary>
+    /// Check xem TPDO đã được configure thành công trên device chưa
+    /// </summary>
+    public bool IsTpdoConfigured(byte pdoNumber) => _configuredTpdoNumbers.Contains(pdoNumber);
+    
+    /// <summary>
+    /// Check xem RPDO đã được configure thành công trên device chưa
+    /// </summary>
+    public bool IsRpdoConfigured(byte pdoNumber) => _configuredRpdoNumbers.Contains(pdoNumber);
+    
+    /// <summary>
+    /// Configure TPDO (Transmit PDO - từ device)
+    /// </summary>
+    public void ConfigureTPDO(PdoConfiguration config)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(PdoManager));
+        
+        _tpdoConfigs[config.PdoNumber] = config;
+    }
+    
+    /// <summary>
+    /// Configure RPDO (Receive PDO - đến device)
+    /// </summary>
+    public void ConfigureRPDO(PdoConfiguration config)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(PdoManager));
+        
+        _rpdoConfigs[config.PdoNumber] = config;
+    }
+    
+    /// <summary>
+    /// Gửi RPDO (Receive PDO) đến device
+    /// Tự động fallback về SDO nếu PDO chưa được configure trên device
+    /// </summary>
+    public async Task SendRPDOAsync(byte pdoNumber, byte[] data, CancellationToken ct = default)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(PdoManager));
+        
+        if (!_rpdoConfigs.TryGetValue(pdoNumber, out var config))
+            throw new InvalidOperationException($"RPDO {pdoNumber} not configured in master-side");
+        
+        if (!config.IsValid)
+            throw new InvalidOperationException($"RPDO {pdoNumber} is not valid");
+        
+        if (data.Length > 8)
+            throw new ArgumentException("PDO data cannot exceed 8 bytes");
+        
+        // Check xem PDO đã được configure trên device chưa
+        if (!_configuredRpdoNumbers.Contains(pdoNumber))
+        {
+            _logger?.LogWarning("RPDO{PDONumber} is not configured on device Node {NodeId}. Cannot send via PDO.", 
+                pdoNumber, _nodeId);
+            throw new InvalidOperationException(
+                $"RPDO {pdoNumber} is not configured on device Node {_nodeId}. " +
+                $"Please ensure PDO configuration is completed before using PDO communication.");
+        }
+        
+        uint cobId = config.CobId & 0x1FFFFFFF; // Mask out valid/RTR bits
+        await _canBus.SendFrameAsync(cobId, data, ct);
+    }
+    
+    /// <summary>
+    /// Request TPDO từ device (RTR - Remote Transmission Request)
+    /// </summary>
+    public async Task RequestTPDOAsync(byte pdoNumber, CancellationToken ct = default)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(PdoManager));
+        
+        if (!_tpdoConfigs.TryGetValue(pdoNumber, out var config))
+            throw new InvalidOperationException($"TPDO {pdoNumber} not configured");
+
+        if (!config.RtrAllowed)
+            throw new InvalidOperationException($"TPDO {pdoNumber} does not allow RTR");
+
+        uint cobId = config.CobId & 0x1FFFFFFF;
+        // Send RTR frame (empty data with RTR flag)
+        await _canBus.SendFrameAsync(cobId | 0x40000000, [], ct);
+    }
+    
+    /// <summary>
+    /// Ghi PDO configuration xuống device qua SDO
+    /// </summary>
+    /// <param name="device">ICanOpenDevice instance để ghi config</param>
+    /// <param name="isTpdo">True nếu là TPDO, False nếu là RPDO</param>
+    /// <param name="pdoNumber">PDO number (1-4)</param>
+    /// <param name="ct">Cancellation token</param>
+    public async Task WritePdoConfigurationToDeviceAsync(ICanOpenDevice device, bool isTpdo, byte pdoNumber, CancellationToken ct = default)
+    {
+        if (pdoNumber < 1 || pdoNumber > 4)
+            throw new ArgumentException("PDO number must be between 1 and 4", nameof(pdoNumber));
+
+        PdoConfiguration? config;
+        if (isTpdo)
+        {
+            if (!_tpdoConfigs.TryGetValue(pdoNumber, out config))
+                throw new InvalidOperationException($"TPDO {pdoNumber} not configured");
+        }
+        else
+        {
+            if (!_rpdoConfigs.TryGetValue(pdoNumber, out config))
+                throw new InvalidOperationException($"RPDO {pdoNumber} not configured");
+        }
+        
+        if (config == null)
+            return;
+        
+        // Validate configuration
+        var validation = config.ValidateConfiguration();
+        if (!validation.IsValid)
+        {
+            throw new InvalidOperationException($"PDO {pdoNumber} configuration is invalid: {string.Join(", ", validation.Errors)}");
+        }
+        
+        // Calculate indices theo CANOpen DS301
+        // Communication Parameter Index: chứa COB-ID, Transmission Type, Inhibit Time, Event Timer
+        // Mapping Parameter Index: chứa mapping count và các mappings
+        ushort commParamIndex = isTpdo 
+            ? (ushort)(0x1800 + (pdoNumber - 1))  // TPDO1: 0x1800, TPDO2: 0x1801, TPDO3: 0x1802, TPDO4: 0x1803
+            : (ushort)(0x1400 + (pdoNumber - 1)); // RPDO1: 0x1400, RPDO2: 0x1401, RPDO3: 0x1402, RPDO4: 0x1403
+        
+        ushort mappingParamIndex = isTpdo 
+            ? (ushort)(0x1A00 + (pdoNumber - 1))  // TPDO1: 0x1A00, TPDO2: 0x1A01, TPDO3: 0x1A02, TPDO4: 0x1A03 (theo CANOpen standard và EDS file)
+            : (ushort)(0x1600 + (pdoNumber - 1));         // RPDO1: 0x1600, RPDO2: 0x1601, RPDO3: 0x1602, RPDO4: 0x1603
+        
+        // Device phải ở PreOperational state để configure PDOs
+        // Note: User cần đảm bảo device ở PreOperational trước khi gọi method này
+        
+        // 1. Disable PDO trước (set bit 31 của COB-ID trong Communication Parameter)
+        uint disabledCobId = config.CobId | 0x80000000;
+        await device.WriteUInt32Async(commParamIndex, 0x01, disabledCobId, ct);
+        await Task.Delay(10, ct); // Small delay để device process
+        
+        // 2. Clear existing mappings (write 0 to mapping count trong Mapping Parameter)
+        await device.WriteUInt8Async(mappingParamIndex, 0x00, 0, ct);
+        await Task.Delay(10, ct);
+        
+        // 3. Write mappings vào Mapping Parameter (sub-index 1, 2, 3, ...)
+        byte mappingSubIndex = 1;
+        foreach (var mapping in config.Mappings)
+        {
+            uint mappingValue = mapping.ToMappingValue();
+            await device.WriteUInt32Async(mappingParamIndex, mappingSubIndex, mappingValue, ct);
+            mappingSubIndex++;
+        }
+        
+        // 4. Write mapping count vào Mapping Parameter (sub-index 0)
+        await device.WriteUInt8Async(mappingParamIndex, 0x00, (byte)config.Mappings.Count, ct);
+        
+        // 5. Write Transmission Type vào Communication Parameter (sub-index 2)
+        await device.WriteUInt8Async(commParamIndex, 0x02, (byte)config.TransmissionType, ct);
+        
+        // 6. Write Inhibit Time (chỉ cho TPDO) vào Communication Parameter (sub-index 3)
+        // Lưu ý: InhibitTime ngăn TPDO gửi quá thường xuyên (minimum time between transmissions)
+        // Nếu InhibitTime = 0, device có thể dùng default value từ EDS file
+        // Để đảm bảo TPDO gửi khi có thay đổi, nên set InhibitTime = 0 hoặc giá trị nhỏ
+        if (isTpdo)
+        {
+            // Nếu config không có InhibitTime, set = 0 để disable inhibit time
+            // Điều này cho phép TPDO gửi ngay khi có thay đổi (nếu không có EventTimer)
+            ushort inhibitTime = config.InhibitTime;
+            await device.WriteUInt16Async(commParamIndex, 0x03, inhibitTime, ct);
+        }
+        
+        // 7. Write Event Timer vào Communication Parameter
+        // TPDO: sub-index 0x05 (theo DS301 và EDS file)
+        // RPDO: thường không có Event Timer, nhưng nếu có thì ở sub-index 0x03 (theo DS301)
+        // Tuy nhiên, nhiều device RPDO không hỗ trợ Event Timer, nên chỉ ghi cho TPDO
+        // QUAN TRỌNG: Luôn write EventTimer (kể cả = 0) để clear giá trị cũ trên device
+        // Nếu không write, device có thể giữ EventTimer cũ từ lần configure trước
+        if (isTpdo)
+        {
+            try
+            {
+                await device.WriteUInt16Async(commParamIndex, 0x05, config.EventTimer, ct);
+            }
+            catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
+                                         ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
+            {
+                // Sub-index 0x05 không tồn tại (device không hỗ trợ EventTimer)
+                // Đây không phải lỗi nghiêm trọng, chỉ log warning
+                _logger?.LogWarning("TPDO{PDONumber} EventTimer sub-index 0x05 does not exist on device. EventTimer will not be set.", pdoNumber);
+            }
+        }
+        
+        // 8. Enable PDO (write COB-ID với bit 31 = 0 vào Communication Parameter, sub-index 0x01)
+        // QUAN TRỌNG: Phải enable PDO sau khi đã configure tất cả parameters
+        uint enabledCobId = config.CobId & 0x7FFFFFFF; // Clear bit 31
+        await device.WriteUInt32Async(commParamIndex, 0x01, enabledCobId, ct);
+        await Task.Delay(10, ct); // Small delay để device process
+    }
+    
+    /// <summary>
+    /// Verify object tồn tại trên device bằng cách đọc thử
+    /// Theo CANOpen standard, nên đọc sub-index 0x00 (highest sub-index supported) để verify object tồn tại
+    /// Sub-index 0x00 luôn tồn tại và read-only nếu object tồn tại
+    /// </summary>
+    private async Task<bool> VerifyObjectExistsAsync(ICanOpenDevice device, ushort index, byte subIndex, CancellationToken ct)
+    {
+        try
+        {
+            // Đọc thử object với timeout ngắn
+            // Nếu đọc được → object tồn tại
+            await device.ReadUInt8Async(index, subIndex, ct);
+            return true;
+        }
+        catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
+                                       ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
+        {
+            // Object hoặc sub-index không tồn tại
+            return false;
+        }
+        catch
+        {
+            // Các lỗi khác (timeout, etc.) - giả định object không tồn tại hoặc không accessible
+            // Lưu ý: Có thể device đang ở trạng thái không cho phép truy cập object dictionary
+            return false;
+        }
+    }
+    
+    /// <summary>
+    /// Clear PDO mapping trên device (nếu object tồn tại)
+    /// Dùng để clear mapping cũ trước khi verify và configure mapping mới
+    /// </summary>
+    private async Task<bool> TryClearPdoMappingAsync(ICanOpenDevice device, ushort mappingParamIndex, CancellationToken ct)
+    {
+        try
+        {
+            // Thử clear mapping bằng cách write 0 vào mapping count (sub-index 0)
+            // Nếu object không tồn tại, sẽ throw exception
+            await device.WriteUInt8Async(mappingParamIndex, 0x00, 0, ct);
+            await Task.Delay(10, ct); // Small delay để device process
+            return true;
+        }
+        catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
+                                       ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
+        {
+            // Object không tồn tại
+            return false;
+        }
+        catch
+        {
+            // Các lỗi khác - giả định object không tồn tại hoặc không accessible
+            return false;
+        }
+    }
+    
+    /// <summary>
+    /// Parse EDS file để lấy danh sách các PDO objects được hỗ trợ
+    /// Chỉ match main index entries (không có sub-index)
+    /// </summary>
+    /// <param name="edsFilePath">Đường dẫn đến EDS file (có thể null)</param>
+    /// <returns>HashSet chứa các PDO object indices được hỗ trợ (0x1A00, 0x1A01, 0x1A02, 0x1A03 cho TPDO; 0x1600, 0x1601, 0x1602, 0x1603 cho RPDO)</returns>
+    private static HashSet<ushort> ParseSupportedPdoIndicesFromEds(string? edsFilePath)
+    {
+        var supportedIndices = new HashSet<ushort>();
+        
+        if (string.IsNullOrWhiteSpace(edsFilePath) || !File.Exists(edsFilePath))
+        {
+            return supportedIndices; // Empty set
+        }
+        
+        try
+        {
+            var content = File.ReadAllText(edsFilePath);
+            
+            // Pattern để tìm các main index entries (không có sub-index):
+            // Match [Index] nhưng không match [Index]subX
+            // Pattern: [Index] không theo sau bởi "sub"
+            var indexPattern = @"\[([0-9A-Fa-f]+)\](?!sub)";
+            var matches = Regex.Matches(content, indexPattern, RegexOptions.IgnoreCase);
+            
+            foreach (Match match in matches)
+            {
+                if (match.Success && match.Groups.Count > 1)
+                {
+                    var indexStr = match.Groups[1].Value;
+                    if (ushort.TryParse(indexStr, System.Globalization.NumberStyles.HexNumber, null, out ushort index))
+                    {
+                        // Check if this is a PDO-related index
+                        // TPDO Mapping Parameters: 0x1A00, 0x1A01, 0x1A02, 0x1A03 (theo CANOpen standard và EDS file)
+                        if (index >= 0x1A00 && index <= 0x1A03)
+                        {
+                            supportedIndices.Add(index);
+                        }
+                        // RPDO Mapping Parameters: 0x1600, 0x1601, 0x1602, 0x1603
+                        else if (index >= 0x1600 && index <= 0x1603)
+                        {
+                            supportedIndices.Add(index);
+                        }
+                        // TPDO Communication Parameters: 0x1800, 0x1801, 0x1802, 0x1803
+                        else if (index >= 0x1800 && index <= 0x1803)
+                        {
+                            supportedIndices.Add(index);
+                        }
+                        // RPDO Communication Parameters: 0x1400, 0x1401, 0x1402, 0x1403
+                        else if (index >= 0x1400 && index <= 0x1403)
+                        {
+                            supportedIndices.Add(index);
+                        }
+                    }
+                }
+            }
+        }
+        catch
+        {
+            // Nếu parse EDS fail, return empty set
+        }
+        
+        return supportedIndices;
+    }
+    
+    /// <summary>
+    /// Check xem PDO có được hỗ trợ trong EDS không
+    /// </summary>
+    private static bool IsPdoSupportedInEds(ushort mappingParamIndex, HashSet<ushort> supportedIndices)
+    {
+        return supportedIndices.Contains(mappingParamIndex);
+    }
+    
+    /// <summary>
+    /// Stop và clear tất cả TPDOs trên device
+    /// </summary>
+    private async Task StopAndClearAllTpdosAsync(ICanOpenDevice device, CancellationToken ct)
+    {
+        // Loop qua tất cả TPDOs có thể có (1-4)
+        for (byte pdoNumber = 1; pdoNumber <= 4; pdoNumber++)
+        {
+            ushort commParamIndex = (ushort)(0x1800 + (pdoNumber - 1));
+            // Theo CANOpen standard và EDS file: TPDO1=0x1A00, TPDO2=0x1A01, TPDO3=0x1A02, TPDO4=0x1A03
+            ushort mappingParamIndex = (ushort)(0x1A00 + (pdoNumber - 1));
+            
+            try
+            {
+                // 1. Stop PDO: Disable bằng cách set bit 31 của COB-ID
+                // Đọc COB-ID hiện tại trước
+                try
+                {
+                    uint currentCobId = await device.ReadUInt32Async(commParamIndex, 0x01, ct);
+                    uint disabledCobId = currentCobId | 0x80000000; // Set bit 31 để disable
+                    await device.WriteUInt32Async(commParamIndex, 0x01, disabledCobId, ct);
+                    await Task.Delay(10, ct);
+                }
+                catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
+                                               ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
+                {
+                    // Object không tồn tại, skip
+                    continue;
+                }
+                
+                // 2. Clear mapping: Write 0 vào mapping count
+                try
+                {
+                    await device.WriteUInt8Async(mappingParamIndex, 0x00, 0, ct);
+                    await Task.Delay(10, ct);
+                }
+                catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
+                                               ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
+                {
+                    // Object không tồn tại, skip
+                    continue;
+                }
+            }
+            catch (Exception ex)
+            {
+                // Log warning nhưng tiếp tục với các PDO khác
+                _logger?.LogWarning(ex, "Failed to stop/clear TPDO{PDONumber} on device Node {NodeId}. Continuing...", 
+                    pdoNumber, device.NodeId);
+            }
+        }
+    }
+    
+    /// <summary>
+    /// Ghi tất cả TPDO configurations xuống device
+    /// Verify object tồn tại trước khi configure
+    /// Nếu EDS có nhưng device không hỗ trợ → throw exception
+    /// </summary>
+    /// <param name="device">CANOpen device</param>
+    /// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support)</param>
+    /// <param name="ct">Cancellation token</param>
+    public async Task WriteAllTpdoConfigurationsToDeviceAsync(ICanOpenDevice device, string? edsFilePath = null, CancellationToken ct = default)
+    {
+        // 1. Reset CANOpen communication để đảm bảo device ở trạng thái sạch
+        try
+        {
+            await device.ResetCommunicationAsync(ct);
+            await Task.Delay(100, ct); // Wait for device to reset
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogWarning(ex, "Failed to reset communication on device Node {NodeId}. Continuing...", device.NodeId);
+        }
+        
+        // 2. Đảm bảo device ở PreOperational state (cần thiết để configure PDOs)
+        try
+        {
+            var currentState = device.State;
+            if (currentState != NmtState.PreOperational && currentState != NmtState.Stopped)
+            {
+                await device.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
+                await Task.Delay(100, ct); // Wait for state change
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogWarning(ex, "Failed to set device Node {NodeId} to PreOperational state. Continuing...", device.NodeId);
+        }
+        
+        // 3. Stop và clear tất cả TPDOs trước khi configure mới
+        await StopAndClearAllTpdosAsync(device, ct);
+        
+        // Parse EDS để lấy danh sách PDO được hỗ trợ
+        var supportedIndices = ParseSupportedPdoIndicesFromEds(edsFilePath);
+        var useEdsFilter = supportedIndices.Count > 0 && !string.IsNullOrWhiteSpace(edsFilePath);
+        
+        _configuredTpdoNumbers.Clear(); // Reset tracking
+        _lastUnconfiguredTpdoWarningTicks.Clear(); // Reset throttle so we log again after reconfig
+        
+        foreach (var kvp in _tpdoConfigs)
+        {
+            // Tính mapping parameter index cho TPDO
+            // Theo CANOpen standard và EDS file: TPDO1=0x1A00, TPDO2=0x1A01, TPDO3=0x1A02, TPDO4=0x1A03
+            ushort mappingParamIndex = (ushort)(0x1A00 + (kvp.Key - 1));
+            ushort commParamIndex = (ushort)(0x1800 + (kvp.Key - 1));
+            
+            // Nếu có EDS và PDO không được hỗ trợ trong EDS, bỏ qua
+            if (useEdsFilter && !IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+            {
+                _logger?.LogWarning("TPDO{PDONumber} (0x{MappingIndex:X4}) is not found in EDS file. Skipping configuration.", 
+                    kvp.Key, mappingParamIndex);
+                continue;
+            }
+            
+            // Verify object tồn tại trên device trước khi configure
+            // Quan trọng: Check communication parameter trước, vì nếu comm parameter không tồn tại
+            // thì mapping parameter cũng không tồn tại (device không hỗ trợ PDO này)
+            // Đọc sub-index 0x00 (highest sub-index supported) để verify object tồn tại - đây là cách chuẩn theo CANOpen
+            bool commExists = await VerifyObjectExistsAsync(device, commParamIndex, 0x00, ct);
+            
+            if (!commExists)
+            {
+                // Communication parameter không tồn tại → device không hỗ trợ PDO này
+                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
+                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+                {
+                    _logger?.LogError("EDS file mismatch: TPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
+                        "Communication parameter 0x{CommIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
+                        kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
+                    throw new ConfigurationException($"TPDO{kvp.Key}", 
+                        $"EDS file declares TPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
+                        $"Communication parameter 0x{commParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
+                }
+                
+                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
+                _logger?.LogWarning("TPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Communication parameter 0x{CommIndex:X4} does not exist. Skipping configuration.", 
+                    kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
+                continue;
+            }
+            
+            // Communication parameter tồn tại → tiếp tục verify mapping parameter
+            // Clear mapping cũ trước (nếu có) để đảm bảo device ở trạng thái sạch
+            // Điều này cũng giúp verify object tồn tại: nếu clear thành công thì object tồn tại
+            bool mappingCleared = await TryClearPdoMappingAsync(device, mappingParamIndex, ct);
+            bool mappingExists = mappingCleared || await VerifyObjectExistsAsync(device, mappingParamIndex, 0x00, ct);
+            
+            if (!mappingExists)
+            {
+                // Mapping parameter không tồn tại (nhưng comm parameter tồn tại - trường hợp hiếm)
+                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
+                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+                {
+                    _logger?.LogError("EDS file mismatch: TPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
+                        "Mapping parameter 0x{MappingIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
+                        kvp.Key, mappingParamIndex, device.NodeId, mappingParamIndex);
+                    throw new ConfigurationException($"TPDO{kvp.Key}", 
+                        $"EDS file declares TPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
+                        $"Mapping parameter 0x{mappingParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
+                }
+                
+                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
+                _logger?.LogWarning("TPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Mapping parameter does not exist. Skipping configuration.", 
+                    kvp.Key, mappingParamIndex, device.NodeId);
+                continue;
+            }
+            
+            // Object tồn tại → configure
+            try
+            {
+                // Log COB-ID trước khi configure
+                var config = _tpdoConfigs[kvp.Key];
+                uint cobId = config.CobId & 0x1FFFFFFF;
+                
+                await WritePdoConfigurationToDeviceAsync(device, true, kvp.Key, ct);
+                _configuredTpdoNumbers.Add(kvp.Key);
+            }
+            catch (Exception ex)
+            {
+                // Nếu có EDS và EDS nói device hỗ trợ nhưng configure fail → ERROR
+                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+                {
+                    _logger?.LogError(ex, "Failed to configure TPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId} even though it is declared in EDS file.", 
+                        kvp.Key, mappingParamIndex, device.NodeId);
+                    throw new ConfigurationException($"TPDO{kvp.Key}", 
+                        $"Failed to configure TPDO{kvp.Key} (0x{mappingParamIndex:X4}) on device Node {device.NodeId}: {ex.Message}");
+                }
+                
+                // Nếu không có EDS hoặc EDS không có → chỉ log warning
+                _logger?.LogWarning(ex, "Failed to configure TPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId}. Skipping.", 
+                    kvp.Key, mappingParamIndex, device.NodeId);
+            }
+        }
+    }
+    
+    /// <summary>
+    /// Ghi tất cả RPDO configurations xuống device
+    /// Verify object tồn tại trước khi configure
+    /// Nếu EDS có nhưng device không hỗ trợ → throw exception
+    /// </summary>
+    /// <param name="device">CANOpen device</param>
+    /// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support)</param>
+    /// <param name="ct">Cancellation token</param>
+    public async Task WriteAllRpdoConfigurationsToDeviceAsync(ICanOpenDevice device, string? edsFilePath = null, CancellationToken ct = default)
+    {
+        // Parse EDS để lấy danh sách PDO được hỗ trợ
+        var supportedIndices = ParseSupportedPdoIndicesFromEds(edsFilePath);
+        var useEdsFilter = supportedIndices.Count > 0 && !string.IsNullOrWhiteSpace(edsFilePath);
+        
+        _configuredRpdoNumbers.Clear(); // Reset tracking
+        
+        foreach (var kvp in _rpdoConfigs)
+        {
+            // Tính mapping parameter index cho RPDO
+            ushort mappingParamIndex = (ushort)(0x1600 + (kvp.Key - 1));
+            ushort commParamIndex = (ushort)(0x1400 + (kvp.Key - 1));
+            
+            // Nếu có EDS và PDO không được hỗ trợ trong EDS, bỏ qua
+            if (useEdsFilter && !IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+            {
+                _logger?.LogWarning("RPDO{PDONumber} (0x{MappingIndex:X4}) is not found in EDS file. Skipping configuration.", 
+                    kvp.Key, mappingParamIndex);
+                continue;
+            }
+            
+            // Verify object tồn tại trên device trước khi configure
+            // Quan trọng: Check communication parameter trước, vì nếu comm parameter không tồn tại
+            // thì mapping parameter cũng không tồn tại (device không hỗ trợ PDO này)
+            // Đọc sub-index 0x00 (highest sub-index supported) để verify object tồn tại - đây là cách chuẩn theo CANOpen
+            bool commExists = await VerifyObjectExistsAsync(device, commParamIndex, 0x00, ct);
+            
+            if (!commExists)
+            {
+                // Communication parameter không tồn tại → device không hỗ trợ PDO này
+                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
+                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+                {
+                    _logger?.LogError("EDS file mismatch: RPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
+                        "Communication parameter 0x{CommIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
+                        kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
+                    throw new ConfigurationException($"RPDO{kvp.Key}", 
+                        $"EDS file declares RPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
+                        $"Communication parameter 0x{commParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
+                }
+                
+                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
+                _logger?.LogWarning("RPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Communication parameter 0x{CommIndex:X4} does not exist. Skipping configuration.", 
+                    kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
+                continue;
+            }
+            
+            // Communication parameter tồn tại → tiếp tục verify mapping parameter
+            // Clear mapping cũ trước (nếu có) để đảm bảo device ở trạng thái sạch
+            // Điều này cũng giúp verify object tồn tại: nếu clear thành công thì object tồn tại
+            bool mappingCleared = await TryClearPdoMappingAsync(device, mappingParamIndex, ct);
+            bool mappingExists = mappingCleared || await VerifyObjectExistsAsync(device, mappingParamIndex, 0x00, ct);
+            
+            if (!mappingExists)
+            {
+                // Mapping parameter không tồn tại (nhưng comm parameter tồn tại - trường hợp hiếm)
+                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
+                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+                {
+                    _logger?.LogError("EDS file mismatch: RPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
+                        "Mapping parameter 0x{MappingIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
+                        kvp.Key, mappingParamIndex, device.NodeId, mappingParamIndex);
+                    throw new ConfigurationException($"RPDO{kvp.Key}", 
+                        $"EDS file declares RPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
+                        $"Mapping parameter 0x{mappingParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
+                }
+                
+                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
+                _logger?.LogWarning("RPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Mapping parameter does not exist. Skipping configuration.", 
+                    kvp.Key, mappingParamIndex, device.NodeId);
+                continue;
+            }
+            
+            // Object tồn tại → configure
+            try
+            {
+                await WritePdoConfigurationToDeviceAsync(device, false, kvp.Key, ct);
+                _configuredRpdoNumbers.Add(kvp.Key);
+            }
+            catch (Exception ex)
+            {
+                // Nếu có EDS và EDS nói device hỗ trợ nhưng configure fail → ERROR
+                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
+                {
+                    _logger?.LogError(ex, "Failed to configure RPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId} even though it is declared in EDS file.", 
+                        kvp.Key, mappingParamIndex, device.NodeId);
+                    throw new ConfigurationException($"RPDO{kvp.Key}", 
+                        $"Failed to configure RPDO{kvp.Key} (0x{mappingParamIndex:X4}) on device Node {device.NodeId}: {ex.Message}");
+                }
+                
+                // Nếu không có EDS hoặc EDS không có → chỉ log warning
+                _logger?.LogWarning(ex, "Failed to configure RPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId}. Skipping.", 
+                    kvp.Key, mappingParamIndex, device.NodeId);
+            }
+        }
+    }
+    
+    /// <summary>
+    /// Ghi tất cả PDO configurations (TPDO + RPDO) xuống device
+    /// Chỉ configure các PDO được hỗ trợ trong EDS file (nếu có)
+    /// </summary>
+    /// <param name="device">CANOpen device</param>
+    /// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support)</param>
+    /// <param name="ct">Cancellation token</param>
+    public async Task WriteAllPdoConfigurationsToDeviceAsync(ICanOpenDevice device, string? edsFilePath = null, CancellationToken ct = default)
+    {
+        await WriteAllTpdoConfigurationsToDeviceAsync(device, edsFilePath, ct);
+        await WriteAllRpdoConfigurationsToDeviceAsync(device, edsFilePath, ct);
+    }
+    
+    private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
+    {
+        // Create snapshot of configs to avoid issues during iteration
+        // ConcurrentDictionary.Values is thread-safe for iteration, but snapshot is safer
+        var configs = _tpdoConfigs.Values.ToArray();
+        
+        // Check if this is a TPDO for this node
+        foreach (var config in configs)
+        {
+            uint expectedCobId = config.CobId & 0x1FFFFFFF;
+            
+            // Debug logging để kiểm tra matching
+            if (e.CanId == expectedCobId)
+            {
+                if (!config.IsValid)
+                {
+                    _logger?.LogWarning("TPDO{PDONumber} frame received (COB-ID=0x{CobId:X3}) but config is invalid (disabled). Expected COB-ID=0x{ExpectedCobId:X3}", 
+                        config.PdoNumber, e.CanId, expectedCobId);
+                    continue;
+                }
+                
+                // Check if TPDO was configured on device
+                if (!_configuredTpdoNumbers.Contains(config.PdoNumber))
+                {
+                    // Throttle repeated warnings to avoid log spam
+                    var now = Environment.TickCount64;
+                    var lastLog = _lastUnconfiguredTpdoWarningTicks.GetOrAdd(config.PdoNumber, 0L);
+                    if (now - lastLog >= UnconfiguredTpdoWarningIntervalMs)
+                    {
+                        _lastUnconfiguredTpdoWarningTicks[config.PdoNumber] = now;
+                        _logger?.LogWarning("TPDO{PDONumber} frame received (COB-ID=0x{CobId:X3}) but TPDO{PDONumber} was not successfully configured on device Node {NodeId}. Skipping. (This warning is throttled to once per minute.)", 
+                            config.PdoNumber, e.CanId, config.PdoNumber, _nodeId);
+                    }
+                    continue;
+                }
+                
+                // Match found - invoke event
+                var pdoData = new PdoData(config.PdoNumber, e.CanId, e.Data, e.Timestamp);
+                PdoReceived?.Invoke(this, new PdoReceivedEventArgs(pdoData, PdoType.Transmit));
+                return;
+            }
+        }
+        
+        // Log unmatched frames that might be TPDOs (check if COB-ID is in TPDO range)
+        // TPDO COB-ID range: 0x180-0x1FF, 0x280-0x2FF, 0x380-0x3FF, 0x480-0x4FF (TPDO1-4 for nodes 1-127)
+        if ((e.CanId >= 0x180 && e.CanId <= 0x1FF) || 
+            (e.CanId >= 0x280 && e.CanId <= 0x2FF) || 
+            (e.CanId >= 0x380 && e.CanId <= 0x3FF) || 
+            (e.CanId >= 0x480 && e.CanId <= 0x4FF))
+        {
+            _logger?.LogTrace("Received CAN frame with COB-ID=0x{CobId:X3} (possible TPDO) but no matching TPDO config found. Configured TPDOs: {TpdoList}, Configured on device: {ConfiguredList}", 
+                e.CanId, 
+                string.Join(", ", configs.Select(c => $"TPDO{c.PdoNumber}(0x{c.CobId & 0x1FFFFFFF:X3})")),
+                string.Join(", ", _configuredTpdoNumbers.Select(n => $"TPDO{n}")));
+        }
+    }
+    
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+        
+        _canBus.FrameReceived -= OnFrameReceived;
+        _tpdoConfigs.Clear();
+        _rpdoConfigs.Clear();
+        _configuredTpdoNumbers.Clear();
+        _configuredRpdoNumbers.Clear();
+        _lastUnconfiguredTpdoWarningTicks.Clear();
+        
+        _disposed = true;
+        GC.SuppressFinalize(this);
+    }
+}
+
+/// <summary>
+/// Event args cho PDO received
+/// </summary>
+public class PdoReceivedEventArgs(PdoData data, PdoType type) : EventArgs
+{
+    public PdoData Data { get; } = data;
+    public PdoType Type { get; } = type;
+}