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

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

556
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/CanOpenManager.cs Normal file
View File

@@ -0,0 +1,556 @@
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen.Interfaces;
using RobotNet10.CANOpen.Services;
using System.Collections.Concurrent;
namespace RobotNet10.CANOpen;
/// <summary>
/// Implementation của ICanOpenManager và IHostedService
/// Quản lý ICanBus và CanOpenDevice instances với thread-safe operations
/// Tự động tạo SocketCanBus từ interface name
/// </summary>
public class CanOpenManager : ICanOpenManager, IHostedService
{
private readonly ConcurrentDictionary<string, ICanBus> _canBuses = [];
private readonly ConcurrentDictionary<string, Dictionary<byte, CanOpenDevice>> _devices = []; // Key: interfaceName -> (nodeId -> device)
private readonly SemaphoreSlim _semaphore = new(1, 1); // Binary semaphore for exclusive access
private readonly object _stoppingLock = new(); // Lock object for _stopping flag
private readonly ILoggerFactory? _loggerFactory;
private readonly ILogger<CanOpenManager>? _logger;
private readonly TimeSpan _lockTimeout = TimeSpan.FromSeconds(30); // Timeout cho lock operations
private readonly int _maxRetryCount; // Số lần retry khi acquire lock thất bại
private readonly TimeSpan _retryDelay; // Delay giữa các lần retry
private bool _disposed;
private bool _stopping;
/// <summary>
/// Constructor với các tham số cấu hình
/// </summary>
/// <param name="loggerFactory">Logger factory để tạo logger</param>
/// <param name="maxRetryCount">Số lần retry khi acquire lock thất bại (mặc định: 3)</param>
/// <param name="lockTimeout">Timeout cho mỗi lần acquire lock (mặc định: 30 giây)</param>
/// <param name="retryDelay">Delay giữa các lần retry (mặc định: 100ms)</param>
public CanOpenManager(
ILoggerFactory? loggerFactory = null,
int maxRetryCount = 3,
TimeSpan? lockTimeout = null,
TimeSpan? retryDelay = null)
{
_loggerFactory = loggerFactory;
_logger = loggerFactory?.CreateLogger<CanOpenManager>();
_maxRetryCount = maxRetryCount >= 0 ? maxRetryCount : throw new ArgumentException("MaxRetryCount must be >= 0", nameof(maxRetryCount));
_lockTimeout = lockTimeout ?? TimeSpan.FromSeconds(30);
_retryDelay = retryDelay ?? TimeSpan.FromMilliseconds(100);
}
/// <summary>
/// Acquire lock với timeout và retry để tránh deadlock
/// </summary>
private async Task<IDisposable> AcquireLockAsync(CancellationToken cancellationToken = default)
{
Exception? lastException = null;
for (int attempt = 0; attempt <= _maxRetryCount; attempt++)
{
// Nếu không phải lần thử đầu tiên, đợi một chút trước khi retry
if (attempt > 0)
{
try
{
await Task.Delay(_retryDelay, cancellationToken);
}
catch (OperationCanceledException)
{
throw;
}
}
using (var cts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken))
{
cts.CancelAfter(_lockTimeout);
try
{
await _semaphore.WaitAsync(cts.Token);
return new SemaphoreRelease(_semaphore);
}
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
{
throw;
}
catch (OperationCanceledException ex)
{
lastException = new TimeoutException(
$"Failed to acquire lock within {_lockTimeout.TotalSeconds} seconds (attempt {attempt + 1}/{_maxRetryCount + 1})",
ex);
// Nếu đã hết số lần retry, throw exception
if (attempt >= _maxRetryCount)
{
throw lastException;
}
}
}
}
// Không bao giờ đến đây, nhưng compiler cần return statement
throw lastException ?? new TimeoutException($"Failed to acquire lock after {_maxRetryCount + 1} attempts");
}
/// <summary>
/// Acquire lock với timeout và retry (synchronous version)
/// </summary>
private IDisposable AcquireLock()
{
Exception? lastException = null;
for (int attempt = 0; attempt <= _maxRetryCount; attempt++)
{
// Nếu không phải lần thử đầu tiên, đợi một chút trước khi retry
if (attempt > 0)
{
Thread.Sleep(_retryDelay);
}
if (_semaphore.Wait(_lockTimeout))
{
return new SemaphoreRelease(_semaphore);
}
lastException = new TimeoutException(
$"Failed to acquire lock within {_lockTimeout.TotalSeconds} seconds (attempt {attempt + 1}/{_maxRetryCount + 1})");
// Nếu đã hết số lần retry, throw exception
if (attempt >= _maxRetryCount)
{
throw lastException;
}
}
// Không bao giờ đến đây, nhưng compiler cần return statement
throw lastException ?? new TimeoutException($"Failed to acquire lock after {_maxRetryCount + 1} attempts");
}
/// <summary>
/// Helper class để release semaphore khi dispose
/// </summary>
private sealed class SemaphoreRelease : IDisposable
{
private readonly SemaphoreSlim _semaphore;
private bool _disposed;
public SemaphoreRelease(SemaphoreSlim semaphore)
{
_semaphore = semaphore;
}
public void Dispose()
{
if (!_disposed)
{
_semaphore.Release();
_disposed = true;
}
}
}
public async Task<ICanBus> GetOrCreateCanBusAsync(string interfaceName, CancellationToken cancellationToken = default)
{
if (IsStoppingOrDisposed())
throw new ObjectDisposedException(nameof(CanOpenManager), "CanOpenManager is disposed or stopping");
if (string.IsNullOrWhiteSpace(interfaceName))
throw new ArgumentException("Interface name cannot be null or empty", nameof(interfaceName));
// Chưa có trong dictionary, cần tạo mới
// Dùng lock để đảm bảo chỉ một thread tạo và connect
using (await AcquireLockAsync(cancellationToken))
{
// Double-check: có thể đã được tạo bởi thread khác trong lúc chờ lock
if (_canBuses.TryGetValue(interfaceName, out var existingBus))
{
return existingBus;
}
// Tạo mới SocketCanBus instance
var canBusLogger = _loggerFactory?.CreateLogger<SocketCanBus>();
var canBus = new SocketCanBus(interfaceName, canBusLogger);
// TODO: Shutdown và enable lại interfaceName
try
{
// Connect đến CAN bus
await canBus.ConnectAsync(cancellationToken);
// Add vào dictionary
if (_canBuses.TryAdd(interfaceName, canBus))
{
return canBus;
}
else
{
// Should not happen vì đã check và lock, nhưng handle để an toàn
// Nếu vẫn add thất bại, có thể thread khác đã add trong lúc này
if (_canBuses.TryGetValue(interfaceName, out existingBus))
{
_logger?.LogWarning("Another thread added CAN bus for interface {InterfaceName} during creation", interfaceName);
// Dispose instance vừa tạo vì không thể add vào dictionary
try
{
await canBus.DisconnectAsync(cancellationToken);
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Error disconnecting CAN bus during cleanup");
}
canBus.Dispose();
return existingBus;
}
_logger?.LogError("Failed to add CAN bus to dictionary for interface {InterfaceName}", interfaceName);
throw new InvalidOperationException($"Failed to add CAN bus for interface {interfaceName}");
}
}
catch (Exception ex)
{
// Nếu connect fail hoặc có lỗi khác, dispose instance đã tạo
_logger?.LogError(ex, "Error creating CAN bus for interface {InterfaceName}", interfaceName);
try
{
if (canBus.IsConnected)
{
await canBus.DisconnectAsync(cancellationToken);
}
}
catch (Exception disconnectEx)
{
_logger?.LogWarning(disconnectEx, "Error disconnecting CAN bus during error cleanup");
}
canBus.Dispose();
throw;
}
}
}
public ICanBus? GetCanBus(string interfaceName)
{
if (IsStoppingOrDisposed())
throw new ObjectDisposedException(nameof(CanOpenManager), "CanOpenManager is disposed or stopping");
// ConcurrentDictionary.TryGetValue is thread-safe, no lock needed
return _canBuses.TryGetValue(interfaceName, out var canBus) ? canBus : null;
}
public async Task<CanOpenDevice> GetOrCreateDeviceAsync(string interfaceName, byte nodeId, CancellationToken cancellationToken = default)
{
if (IsStoppingOrDisposed())
throw new ObjectDisposedException(nameof(CanOpenManager), "CanOpenManager is disposed or stopping");
if (nodeId == 0 || nodeId > 127)
throw new ArgumentException("NodeId must be between 1 and 127", nameof(nodeId));
// Đảm bảo ICanBus đã được tạo và connect
var canBus = await GetOrCreateCanBusAsync(interfaceName, cancellationToken);
// Với ConcurrentDictionary, cần lock cho inner Dictionary<byte, CanOpenDevice>
// Vì inner Dictionary không phải thread-safe
using (await AcquireLockAsync(cancellationToken))
{
// Kiểm tra xem device đã tồn tại chưa
if (_devices.TryGetValue(interfaceName, out var devicesForInterface))
{
if (devicesForInterface.TryGetValue(nodeId, out var existingDevice))
{
return existingDevice;
}
}
else
{
// Tạo mới inner dictionary nếu chưa có
devicesForInterface = [];
_devices[interfaceName] = devicesForInterface;
}
// Tạo mới CanOpenDevice với logger
var deviceLogger = _loggerFactory?.CreateLogger<CanOpenDevice>();
var device = new CanOpenDevice(canBus, nodeId, deviceLogger, _loggerFactory);
devicesForInterface[nodeId] = device;
return device;
}
}
public CanOpenDevice? GetDevice(string interfaceName, byte nodeId)
{
if (IsStoppingOrDisposed())
throw new ObjectDisposedException(nameof(CanOpenManager), "CanOpenManager is disposed or stopping");
using (AcquireLock())
{
if (_devices.TryGetValue(interfaceName, out var devicesForInterface))
{
return devicesForInterface.TryGetValue(nodeId, out var device) ? device : null;
}
return null;
}
}
public bool RemoveDevice(string interfaceName, byte nodeId)
{
if (IsStoppingOrDisposed())
return false;
using (AcquireLock())
{
if (!_devices.TryGetValue(interfaceName, out var devicesForInterface))
return false;
if (!devicesForInterface.TryGetValue(nodeId, out var device))
return false;
// Dispose device
device.Dispose();
devicesForInterface.Remove(nodeId);
// Nếu không còn device nào cho interface này, xóa dictionary
if (devicesForInterface.Count == 0)
{
_devices.Remove(interfaceName, out var _);
}
return true;
}
}
public async Task<bool> RemoveCanBusAsync(string interfaceName, CancellationToken cancellationToken = default)
{
if (IsStoppingOrDisposed())
return false;
List<CanOpenDevice> devicesToDispose = [];
ICanBus? canBus = null;
using (await AcquireLockAsync(cancellationToken))
{
// Check và collect trong cùng một lock để tránh race condition
if (!_canBuses.TryGetValue(interfaceName, out canBus))
return false;
// Collect tất cả devices đang sử dụng canBus này
if (_devices.TryGetValue(interfaceName, out var devicesForInterface))
{
devicesToDispose.AddRange(devicesForInterface.Values);
_devices.Remove(interfaceName, out var _);
}
_canBuses.Remove(interfaceName, out var _);
}
// Dispose devices trước (ngoài lock)
foreach (var device in devicesToDispose)
{
device.Dispose();
}
// Disconnect và dispose canBus (ngoài lock)
await canBus.DisconnectAsync(cancellationToken);
canBus.Dispose();
return true;
}
public IReadOnlyList<string> GetManagedInterfaces()
{
if (IsStoppingOrDisposed())
return Array.Empty<string>().AsReadOnly();
// ConcurrentDictionary.Keys is thread-safe for enumeration
return _canBuses.Keys.ToList().AsReadOnly();
}
public IReadOnlyList<byte> GetManagedNodeIds(string interfaceName)
{
if (IsStoppingOrDisposed())
return Array.Empty<byte>().AsReadOnly();
// Need lock because inner Dictionary<byte, CanOpenDevice> is not thread-safe
using (AcquireLock())
{
if (!_devices.TryGetValue(interfaceName, out var devicesForInterface))
return Array.Empty<byte>().AsReadOnly();
return devicesForInterface.Keys.ToList().AsReadOnly();
}
}
public bool IsInterfaceManaged(string interfaceName)
{
if (IsStoppingOrDisposed())
return false;
// ConcurrentDictionary.ContainsKey is thread-safe
return _canBuses.ContainsKey(interfaceName);
}
public bool IsDeviceManaged(string interfaceName, byte nodeId)
{
if (IsStoppingOrDisposed())
return false;
// Need lock because inner Dictionary<byte, CanOpenDevice> is not thread-safe
using (AcquireLock())
{
if (!_devices.TryGetValue(interfaceName, out var devicesForInterface))
return false;
return devicesForInterface.ContainsKey(nodeId);
}
}
#region IHostedService
public Task StartAsync(CancellationToken cancellationToken)
{
return Task.CompletedTask;
}
public async Task StopAsync(CancellationToken cancellationToken)
{
// Mark as stopping to prevent new operations
lock (_stoppingLock)
{
_stopping = true;
}
await CleanupResourcesAsync(cancellationToken);
}
#endregion
#region IDisposable
public void Dispose()
{
if (_disposed)
return;
// Mark as stopping
lock (_stoppingLock)
{
_stopping = true;
}
// Cleanup resources synchronously (for Dispose, we use blocking approach)
try
{
var cleanupTask = CleanupResourcesAsync(CancellationToken.None);
if (!cleanupTask.Wait(TimeSpan.FromSeconds(30)))
{
_logger?.LogWarning("Timeout waiting for cleanup to complete during Dispose");
}
}
catch (Exception ex)
{
_logger?.LogError(ex, "Error during cleanup in Dispose");
}
_semaphore.Dispose();
_disposed = true;
GC.SuppressFinalize(this);
}
#endregion
#region Private Helper Methods
/// <summary>
/// Kiểm tra xem manager có đang stopping hoặc disposed không (thread-safe)
/// </summary>
private bool IsStoppingOrDisposed()
{
lock (_stoppingLock)
{
return _disposed || _stopping;
}
}
#endregion
#region Private Cleanup Methods
/// <summary>
/// Cleanup tất cả resources (devices và canBuses) một cách async
/// </summary>
private async Task CleanupResourcesAsync(CancellationToken cancellationToken)
{
List<CanOpenDevice> devicesToDispose = [];
List<ICanBus> canBusesToDispose = [];
// Collect tất cả resources cần dispose (trong lock ngắn)
using (await AcquireLockAsync(cancellationToken))
{
// Collect devices
foreach (var devicesForInterface in _devices.Values)
{
devicesToDispose.AddRange(devicesForInterface.Values);
}
_devices.Clear();
// Collect canBuses
canBusesToDispose.AddRange(_canBuses.Values);
_canBuses.Clear();
}
// Dispose devices (ngoài lock để tránh blocking)
foreach (var device in devicesToDispose)
{
cancellationToken.ThrowIfCancellationRequested();
try
{
device.Dispose();
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Error disposing CanOpenDevice");
}
}
// Disconnect và dispose canBuses (ngoài lock để tránh blocking async calls)
foreach (var canBus in canBusesToDispose)
{
cancellationToken.ThrowIfCancellationRequested();
try
{
// Disconnect với timeout
using (var cts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken))
{
cts.CancelAfter(TimeSpan.FromSeconds(5));
await canBus.DisconnectAsync(cts.Token);
}
}
catch (OperationCanceledException)
{
_logger?.LogWarning("Timeout or cancellation while disconnecting CAN bus");
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Error disconnecting CAN bus");
}
try
{
canBus.Dispose();
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Error disposing CAN bus");
}
}
}
#endregion
}

144
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/EmergencyMonitor.cs Normal file
View File

@@ -0,0 +1,144 @@
using System.Collections.Concurrent;
using RobotNet10.CANOpen.Enums;
using RobotNet10.CANOpen.Interfaces;
using RobotNet10.CANOpen.Models;
namespace RobotNet10.CANOpen.Services;
/// <summary>
/// Emergency Monitor - theo dõi emergency messages từ các nodes
/// </summary>
public class EmergencyMonitor : IDisposable
{
private readonly ICanBus _canBus;
private readonly ConcurrentDictionary<byte, EmergencyMessage> _lastEmergencies;
private bool _disposed;
public event EventHandler<EmergencyReceivedEventArgs>? EmergencyReceived;
public EmergencyMonitor(ICanBus canBus)
{
_canBus = canBus ?? throw new ArgumentNullException(nameof(canBus));
_lastEmergencies = new ConcurrentDictionary<byte, EmergencyMessage>();
_canBus.FrameReceived += OnFrameReceived;
}
/// <summary>
/// Lấy emergency message cuối cùng từ một node
/// </summary>
public EmergencyMessage? GetLastEmergency(byte nodeId)
{
return _lastEmergencies.TryGetValue(nodeId, out var emergency) ? emergency : null;
}
/// <summary>
/// Xóa emergency history của một node
/// </summary>
public void ClearEmergency(byte nodeId)
{
_lastEmergencies.TryRemove(nodeId, out _);
}
/// <summary>
/// Xóa tất cả emergency history
/// </summary>
public void ClearAll()
{
_lastEmergencies.Clear();
}
private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
{
// EMERGENCY COB-ID: 0x80 + NodeID
uint baseEmergencyCobId = (uint)CanMessageType.Emergency;
if (e.CanId >= baseEmergencyCobId && e.CanId < baseEmergencyCobId + 0x7F)
{
var emergency = EmergencyMessage.FromCanFrame(e.CanId, e.Data, e.Timestamp);
// Thread-safe update using ConcurrentDictionary
_lastEmergencies[emergency.NodeId] = emergency;
EmergencyReceived?.Invoke(this, new EmergencyReceivedEventArgs(emergency));
}
}
public void Dispose()
{
if (_disposed)
return;
_canBus.FrameReceived -= OnFrameReceived;
_lastEmergencies.Clear();
_disposed = true;
GC.SuppressFinalize(this);
}
}
/// <summary>
/// Event args cho Emergency received
/// </summary>
public class EmergencyReceivedEventArgs(EmergencyMessage Emergency) : EventArgs
{
/// <summary>
/// Helper để format emergency message cho logging
/// </summary>
public string GetDescription()
{
var errorClass = ((ushort)Emergency.ErrorCode) >> 8;
var errorType = GetErrorType(errorClass);
// Error Register bits (DS301):
// Bit 0: Generic Error
// Bit 1: Current
// Bit 2: Voltage
// Bit 3: Temperature
// Bit 4: Communication Error
// Bit 5: Device Profile Specific
// Bit 6: Reserved
// Bit 7: Manufacturer Specific
bool isGeneric = (Emergency.ErrorRegister & 0x01) != 0;
bool isCurrent = (Emergency.ErrorRegister & 0x02) != 0;
bool isVoltage = (Emergency.ErrorRegister & 0x04) != 0;
bool isTemp = (Emergency.ErrorRegister & 0x08) != 0;
return $"Node {Emergency.NodeId}: {errorType} - Error 0x{(ushort)Emergency.ErrorCode:X4}" +
$" (Generic: {isGeneric}, Current: {isCurrent}, " +
$"Voltage: {isVoltage}, Temp: {isTemp})";
}
private static string GetErrorType(int errorClass)
{
return errorClass switch
{
0x00 => "Error Reset or No Error",
0x10 => "Generic Error",
0x20 => "Current Error",
0x21 => "Current Device Input Side",
0x22 => "Current Inside Device",
0x23 => "Current Device Output Side",
0x30 => "Voltage Error",
0x31 => "Mains Voltage",
0x32 => "Voltage Inside Device",
0x33 => "Output Voltage",
0x40 => "Temperature Error",
0x41 => "Ambient Temperature",
0x42 => "Device Temperature",
0x50 => "Device Hardware Error",
0x60 => "Device Software Error",
0x61 => "Internal Software Error",
0x62 => "User Software Error",
0x63 => "Data Set Error",
0x70 => "Additional Modules Error",
0x80 => "Monitoring Error",
0x81 => "Communication Error",
0x82 => "Protocol Error",
0x90 => "External Error",
0xF0 => "Additional Functions Error",
0xFF => "Device Specific Error",
_ => "Unknown Error"
};
}
}

178
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/HeartbeatConsumer.cs Normal file
View File

@@ -0,0 +1,178 @@
using System.Collections.Concurrent;
using RobotNet10.CANOpen.Enums;
using RobotNet10.CANOpen.Interfaces;
using RobotNet10.CANOpen.Models;
namespace RobotNet10.CANOpen.Services;
/// <summary>
/// Heartbeat Consumer - theo dõi heartbeat messages từ các nodes và phát hiện node timeout
/// </summary>
public class HeartbeatConsumer : IDisposable
{
private readonly ICanBus _canBus;
private readonly ConcurrentDictionary<byte, NodeHeartbeatInfo> _nodes;
private readonly Timer _checkTimer;
private bool _disposed;
public event EventHandler<HeartbeatReceivedEventArgs>? HeartbeatReceived;
public event EventHandler<HeartbeatTimeoutEventArgs>? HeartbeatTimeout;
public HeartbeatConsumer(ICanBus canBus, int checkIntervalMs = 100)
{
_canBus = canBus ?? throw new ArgumentNullException(nameof(canBus));
_nodes = new ConcurrentDictionary<byte, NodeHeartbeatInfo>();
_canBus.FrameReceived += OnFrameReceived;
_checkTimer = new Timer(CheckHeartbeats, null, checkIntervalMs, checkIntervalMs);
}
/// <summary>
/// Bắt đầu monitor heartbeat của một node
/// </summary>
/// <param name="nodeId">Node ID</param>
/// <param name="timeoutMs">Timeout in milliseconds (thường 1000-3000ms)</param>
public void MonitorNode(byte nodeId, int timeoutMs)
{
var info = new NodeHeartbeatInfo
{
NodeId = nodeId,
TimeoutMs = timeoutMs,
LastState = NmtState.Unknown,
LastReceived = DateTime.MinValue,
IsAlive = false
};
_nodes[nodeId] = info;
}
/// <summary>
/// Dừng monitor heartbeat của một node
/// </summary>
public void StopMonitoring(byte nodeId)
{
_nodes.TryRemove(nodeId, out _);
}
/// <summary>
/// Lấy thông tin heartbeat của một node
/// </summary>
public NodeHeartbeatInfo? GetNodeInfo(byte nodeId)
{
return _nodes.TryGetValue(nodeId, out var info) ? info : null;
}
/// <summary>
/// Lấy tất cả nodes đang được monitor
/// </summary>
public IReadOnlyDictionary<byte, NodeHeartbeatInfo> GetAllNodes()
{
return _nodes;
}
private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
{
// Heartbeat COB-ID: 0x700 + NodeID
uint baseHeartbeatCobId = (uint)CanMessageType.Heartbeat;
if (e.CanId >= baseHeartbeatCobId && e.CanId < baseHeartbeatCobId + 0x7F)
{
byte nodeId = (byte)(e.CanId - baseHeartbeatCobId);
if (_nodes.TryGetValue(nodeId, out var info) && e.Data.Length >= 1)
{
var heartbeat = HeartbeatMessage.FromBytes(nodeId, e.Data);
info.LastState = heartbeat.State;
info.LastReceived = DateTime.UtcNow;
info.IsAlive = true;
HeartbeatReceived?.Invoke(this, new HeartbeatReceivedEventArgs(heartbeat));
}
}
}
private void CheckHeartbeats(object? state)
{
var now = DateTime.UtcNow;
foreach (var kvp in _nodes)
{
var info = kvp.Value;
if (info.IsAlive && info.LastReceived != DateTime.MinValue)
{
var elapsed = (now - info.LastReceived).TotalMilliseconds;
if (elapsed > info.TimeoutMs)
{
info.IsAlive = false;
HeartbeatTimeout?.Invoke(this, new HeartbeatTimeoutEventArgs(
info.NodeId,
info.LastState,
(int)elapsed));
}
}
}
}
public void Dispose()
{
if (_disposed)
return;
_canBus.FrameReceived -= OnFrameReceived;
_checkTimer?.Dispose();
_nodes.Clear();
_disposed = true;
GC.SuppressFinalize(this);
}
}
/// <summary>
/// Thông tin heartbeat của một node
/// </summary>
public class NodeHeartbeatInfo
{
public byte NodeId { get; set; }
public int TimeoutMs { get; set; }
public NmtState LastState { get; set; }
public DateTime LastReceived { get; set; }
public bool IsAlive { get; set; }
public int TimeSinceLastHeartbeat =>
LastReceived == DateTime.MinValue
? -1
: (int)(DateTime.UtcNow - LastReceived).TotalMilliseconds;
}
/// <summary>
/// Event args cho Heartbeat received
/// </summary>
public class HeartbeatReceivedEventArgs : EventArgs
{
public HeartbeatMessage Heartbeat { get; }
public HeartbeatReceivedEventArgs(HeartbeatMessage heartbeat)
{
Heartbeat = heartbeat;
}
}
/// <summary>
/// Event args cho Heartbeat timeout
/// </summary>
public class HeartbeatTimeoutEventArgs : EventArgs
{
public byte NodeId { get; }
public NmtState LastKnownState { get; }
public int ElapsedMs { get; }
public HeartbeatTimeoutEventArgs(byte nodeId, NmtState lastKnownState, int elapsedMs)
{
NodeId = nodeId;
LastKnownState = lastKnownState;
ElapsedMs = elapsedMs;
}
}

51
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/NmtMaster.cs Normal file
View File

@@ -0,0 +1,51 @@
using RobotNet10.CANOpen.Enums;
using RobotNet10.CANOpen.Interfaces;
using RobotNet10.CANOpen.Models;
namespace RobotNet10.CANOpen.Services;
public class NmtMaster
{
private readonly ICanBus _canBus;
public NmtMaster(ICanBus canBus)
{
_canBus = canBus;
}
public async Task SendCommandAsync(NmtCommand command, byte nodeId, CancellationToken cancellationToken = default)
{
var message = new NmtMessage(command, nodeId);
await _canBus.SendFrameAsync((uint)CanMessageType.Nmt, message.ToBytes(), cancellationToken);
}
public async Task BroadcastCommandAsync(NmtCommand command, CancellationToken cancellationToken = default)
{
await SendCommandAsync(command, 0, cancellationToken);
}
public async Task StartNodeAsync(byte nodeId, CancellationToken cancellationToken = default)
{
await SendCommandAsync(NmtCommand.Start, nodeId, cancellationToken);
}
public async Task StopNodeAsync(byte nodeId, CancellationToken cancellationToken = default)
{
await SendCommandAsync(NmtCommand.Stop, nodeId, cancellationToken);
}
public async Task SetPreOperationalAsync(byte nodeId, CancellationToken cancellationToken = default)
{
await SendCommandAsync(NmtCommand.PreOperational, nodeId, cancellationToken);
}
public async Task ResetNodeAsync(byte nodeId, CancellationToken cancellationToken = default)
{
await SendCommandAsync(NmtCommand.ResetNode, nodeId, cancellationToken);
}
public async Task ResetCommunicationAsync(byte nodeId, CancellationToken cancellationToken = default)
{
await SendCommandAsync(NmtCommand.ResetCommunication, nodeId, cancellationToken);
}
}

756
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs Normal file
View File

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

334
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SdoClient.cs Normal file
View File

@@ -0,0 +1,334 @@
using System.Collections.Concurrent;
using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen.Enums;
using RobotNet10.CANOpen.Exceptions;
using RobotNet10.CANOpen.Interfaces;
using RobotNet10.CANOpen.Models;
namespace RobotNet10.CANOpen.Services;
public class SdoClient : IDisposable
{
private readonly ICanBus _canBus;
private readonly byte _nodeId;
private readonly ConcurrentDictionary<string, TaskCompletionSource<SdoResponse>> _pendingRequests;
private readonly TimeSpan _timeout;
private readonly ILogger<SdoClient>? _logger;
private bool _disposed;
public SdoClient(ICanBus canBus, byte nodeId, TimeSpan? timeout = null, ILogger<SdoClient>? logger = null)
{
_canBus = canBus ?? throw new ArgumentNullException(nameof(canBus));
_nodeId = nodeId;
_timeout = timeout ?? TimeSpan.FromSeconds(1);
_pendingRequests = new ConcurrentDictionary<string, TaskCompletionSource<SdoResponse>>();
_logger = logger;
_canBus.FrameReceived += OnFrameReceived;
}
public async Task<byte[]> UploadAsync(ushort index, byte subIndex, CancellationToken cancellationToken = default)
{
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
var request = SdoRequest.CreateUpload(index, subIndex);
var response = await SendRequestAsync(request, cancellationToken);
if (response.IsAbort)
{
var abortCode = (SdoAbortCode)(uint)response.AbortCode;
var message = $"SDO Upload failed: {abortCode.GetDescription()}";
throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message);
}
// Check if expedited or segmented
if (response.IsExpedited)
{
return response.GetDataBytes();
}
else
{
// Segmented transfer
return await UploadSegmentedAsync(index, subIndex, response, cancellationToken);
}
}
/// <summary>
/// Upload data using segmented transfer
/// </summary>
private async Task<byte[]> UploadSegmentedAsync(ushort index, byte subIndex, SdoResponse initiateResponse, CancellationToken cancellationToken)
{
int totalSize = initiateResponse.GetDataSize();
var result = new List<byte>(totalSize);
byte toggle = 0;
while (result.Count < totalSize)
{
// Request next segment
var segmentRequest = SdoRequest.CreateUploadSegment(toggle);
string segmentKey = $"SEG:{index:X4}:{subIndex:X2}:{result.Count}";
var segmentTcs = new TaskCompletionSource<SdoResponse>();
if (!_pendingRequests.TryAdd(segmentKey, segmentTcs))
throw new InvalidOperationException($"Segment request already pending for {segmentKey}");
try
{
uint cobId = (uint)(CanMessageType.Rsdo) + _nodeId;
await _canBus.SendFrameAsync(cobId, segmentRequest.ToBytes(), cancellationToken);
using var cts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
cts.CancelAfter(_timeout);
try
{
var segmentResponse = await segmentTcs.Task.WaitAsync(cts.Token);
if (segmentResponse.IsAbort)
{
var abortCode = (SdoAbortCode)(uint)segmentResponse.AbortCode;
var message = $"SDO Upload Segment failed: {abortCode.GetDescription()}";
throw new SdoException(_nodeId, index, subIndex, (uint)segmentResponse.AbortCode, message);
}
// Verify toggle bit
byte responseToggle = segmentResponse.GetToggle();
if (responseToggle != toggle)
throw new InvalidOperationException($"Toggle bit mismatch: expected {toggle}, got {responseToggle}");
// Get segment data
var segmentData = segmentResponse.GetSegmentData();
result.AddRange(segmentData);
toggle = (byte)(1 - toggle); // Toggle for next segment
// Check if last segment
if (segmentResponse.IsLastSegment)
break;
}
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
{
throw new CanOpenTimeoutException($"SDO Upload Segment", _timeout, $"Segment at offset {result.Count}");
}
}
finally
{
_pendingRequests.TryRemove(segmentKey, out _);
}
}
return result.ToArray();
}
public async Task DownloadAsync(ushort index, byte subIndex, byte[] data, CancellationToken cancellationToken = default)
{
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
if (data.Length <= 4)
{
// Expedited transfer (≤4 bytes)
var request = SdoRequest.CreateDownload(index, subIndex, data);
var response = await SendRequestAsync(request, cancellationToken);
if (response.IsAbort)
{
var abortCode = (SdoAbortCode)(uint)response.AbortCode;
var message = $"SDO Download failed: {abortCode.GetDescription()}";
throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message);
}
}
else
{
// Segmented transfer (>4 bytes)
await DownloadSegmentedAsync(index, subIndex, data, cancellationToken);
}
}
/// <summary>
/// Download data using segmented transfer (for data > 4 bytes)
/// </summary>
private async Task DownloadSegmentedAsync(ushort index, byte subIndex, byte[] data, CancellationToken cancellationToken)
{
// Step 1: Send Download Initiate
var initiateRequest = SdoRequest.CreateDownloadInitiate(index, subIndex, data.Length);
var initiateResponse = await SendRequestAsync(initiateRequest, cancellationToken);
if (initiateResponse.IsAbort)
{
var abortCode = (SdoAbortCode)(uint)initiateResponse.AbortCode;
var message = $"SDO Download Initiate failed: {abortCode.GetDescription()}";
throw new SdoException(_nodeId, index, subIndex, (uint)initiateResponse.AbortCode, message);
}
// Step 2: Send segments (7 bytes per segment)
byte toggle = 0;
int offset = 0;
while (offset < data.Length)
{
int remaining = data.Length - offset;
int segmentSize = Math.Min(7, remaining);
bool isLastSegment = (offset + segmentSize) >= data.Length;
var segmentData = new byte[segmentSize];
Array.Copy(data, offset, segmentData, 0, segmentSize);
var segmentRequest = SdoRequest.CreateDownloadSegment(toggle, isLastSegment, segmentData);
// Use a unique key for segment requests
string segmentKey = $"SEG:{index:X4}:{subIndex:X2}:{offset}";
var segmentTcs = new TaskCompletionSource<SdoResponse>();
if (!_pendingRequests.TryAdd(segmentKey, segmentTcs))
throw new InvalidOperationException($"Segment request already pending for {segmentKey}");
try
{
uint cobId = (uint)(CanMessageType.Rsdo) + _nodeId;
await _canBus.SendFrameAsync(cobId, segmentRequest.ToBytes(), cancellationToken);
using var cts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
cts.CancelAfter(_timeout);
try
{
var segmentResponse = await segmentTcs.Task.WaitAsync(cts.Token);
if (segmentResponse.IsAbort)
{
var abortCode = (SdoAbortCode)(uint)segmentResponse.AbortCode;
var message = $"SDO Download Segment failed: {abortCode.GetDescription()}";
throw new SdoException(_nodeId, index, subIndex, (uint)segmentResponse.AbortCode, message);
}
// Verify toggle bit
byte responseToggle = segmentResponse.GetToggle();
if (responseToggle != toggle)
throw new InvalidOperationException($"Toggle bit mismatch: expected {toggle}, got {responseToggle}");
toggle = (byte)(1 - toggle); // Toggle for next segment
offset += segmentSize;
}
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
{
throw new CanOpenTimeoutException($"SDO Download Segment", _timeout, $"Segment at offset {offset}");
}
}
finally
{
_pendingRequests.TryRemove(segmentKey, out _);
}
}
}
private async Task<SdoResponse> SendRequestAsync(SdoRequest request, CancellationToken cancellationToken)
{
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
string requestKey = $"{request.Index:X4}:{request.SubIndex:X2}";
var tcs = new TaskCompletionSource<SdoResponse>();
if (!_pendingRequests.TryAdd(requestKey, tcs))
throw new InvalidOperationException($"A request for {requestKey} is already pending");
try
{
uint cobId = (uint)(CanMessageType.Rsdo) + _nodeId;
await _canBus.SendFrameAsync(cobId, request.ToBytes(), cancellationToken);
using var cts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
cts.CancelAfter(_timeout);
try
{
return await tcs.Task.WaitAsync(cts.Token);
}
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
{
var message = $"SDO request for object {request.Index:X4}h.{request.SubIndex:X2}h";
throw new CanOpenTimeoutException($"SDO {(request.CommandSpecifier == (byte)SdoCommand.UploadInitiate ? "Upload" : "Download")}", _timeout, message);
}
}
finally
{
_pendingRequests.TryRemove(requestKey, out _);
}
}
private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
{
uint expectedCobId = (uint)(CanMessageType.Tsdo) + _nodeId;
if (e.CanId != expectedCobId || e.Data.Length < 8)
return;
try
{
var response = SdoResponse.FromBytes(e.Data);
// Check if this is a segment response (Index = 0, SubIndex = 0, and command is segment)
bool isSegmentResponse = response.Index == 0 && response.SubIndex == 0 &&
((response.CommandSpecifier & 0xE0) == (byte)SdoCommand.DownloadSegment ||
(response.CommandSpecifier & 0xE0) == (byte)SdoCommand.UploadSegment);
if (isSegmentResponse)
{
// Match with the first pending segment request (FIFO order)
// Note: In practice, there should only be one pending segment at a time per transfer
foreach (var kvp in _pendingRequests)
{
if (kvp.Key.StartsWith("SEG:"))
{
kvp.Value.TrySetResult(response);
return;
}
}
}
else
{
// Regular response
string requestKey = $"{response.Index:X4}:{response.SubIndex:X2}";
if (_pendingRequests.TryGetValue(requestKey, out var tcs))
{
tcs.TrySetResult(response);
}
}
}
catch (Exception ex)
{
// Log error but don't throw - this is called from event handler
_logger?.LogError(ex, "SDO response parsing error for Node {NodeId}", _nodeId);
}
}
public void Dispose()
{
if (_disposed)
return;
_canBus.FrameReceived -= OnFrameReceived;
// Cancel all pending requests
foreach (var kvp in _pendingRequests)
{
try
{
kvp.Value.TrySetCanceled();
}
catch
{
// Ignore errors when canceling
}
}
_pendingRequests.Clear();
_disposed = true;
GC.SuppressFinalize(this);
}
}

315
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SocketCanBus.cs Normal file
View File

@@ -0,0 +1,315 @@
using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen.Interfaces;
using SocketCANSharp;
using System.Net.Sockets;
using System.Runtime.InteropServices;
namespace RobotNet10.CANOpen.Services;
public class SocketCanBus : ICanBus
{
private readonly string _interfaceName;
private readonly ILogger<SocketCanBus>? _logger;
private SafeFileDescriptorHandle? _socketHandle;
private bool _isConnected;
private Task? _receiveTask;
private CancellationTokenSource? _receiveCts;
public string InterfaceName => _interfaceName;
public bool IsConnected => _isConnected;
public event EventHandler<CanFrameReceivedEventArgs>? FrameReceived;
public SocketCanBus(string interfaceName, ILogger<SocketCanBus>? logger = null)
{
_interfaceName = interfaceName ?? throw new ArgumentNullException(nameof(interfaceName));
_logger = logger;
}
public async Task ConnectAsync(CancellationToken cancellationToken = default)
{
if (_isConnected)
return;
await Task.Run(() =>
{
_socketHandle = LibcNativeMethods.Socket(
SocketCanConstants.PF_CAN,
SocketType.Raw,
SocketCanProtocolType.CAN_RAW);
if (_socketHandle.IsInvalid)
throw new InvalidOperationException("Failed to create CAN socket");
var ifr = new Ifreq(_interfaceName);
int ioctlResult = LibcNativeMethods.Ioctl(_socketHandle, SocketCanConstants.SIOCGIFINDEX, ifr);
if (ioctlResult == -1)
throw new InvalidOperationException($"Failed to find interface {_interfaceName}");
var addr = new SockAddrCan(ifr.IfIndex);
int bindResult = LibcNativeMethods.Bind(_socketHandle, addr, Marshal.SizeOf<SockAddrCan>());
if (bindResult == -1)
throw new InvalidOperationException("Failed to bind to CAN interface");
_isConnected = true;
}, cancellationToken);
_receiveCts = new CancellationTokenSource();
_receiveTask = Task.Run(() => ReceiveLoop(_receiveCts.Token), _receiveCts.Token);
}
public async Task DisconnectAsync(CancellationToken cancellationToken = default)
{
if (!_isConnected)
return;
_isConnected = false;
// Dispose the handle wrapper (should be safe even if already closed)
if (_socketHandle != null)
{
try
{
// Close socket to release resources
int closeResult = LibcNativeMethods.Close(_socketHandle.DangerousGetHandle());
if (closeResult != 0)
{
int errorCode = Marshal.GetLastPInvokeError();
_logger?.LogWarning("Close socket returned error code {ErrorCode} for {InterfaceName}", errorCode, _interfaceName);
}
_socketHandle.Dispose();
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Error disposing socket handle wrapper");
}
_socketHandle = null;
}
// Cancel receive task after closing socket
if (_receiveCts != null)
{
_receiveCts.Cancel();
// Wait for receive task to finish (should exit quickly now that socket is closed)
// Use timeout to avoid hanging if receive task is stuck in blocking Read()
if (_receiveTask != null)
{
try
{
using (var timeoutCts = new CancellationTokenSource(TimeSpan.FromSeconds(2)))
{
await _receiveTask.WaitAsync(timeoutCts.Token);
}
}
catch (OperationCanceledException)
{
_logger?.LogWarning("Timeout waiting for receive task to finish for SocketCanBus {InterfaceName}", _interfaceName);
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Error waiting for receive task to finish");
}
}
_receiveCts.Dispose();
_receiveCts = null;
}
}
public Task SendFrameAsync(uint canId, byte[] data, CancellationToken cancellationToken = default)
{
try
{
if (!_isConnected || _socketHandle == null)
throw new InvalidOperationException("Not connected to CAN bus");
if (data.Length > 8)
throw new ArgumentException("CAN frame data cannot exceed 8 bytes");
return Task.Run(() =>
{
var frame = new CanFrame
{
CanId = canId,
Length = (byte)data.Length,
Data = new byte[8]
};
for (int i = 0; i < data.Length; i++)
frame.Data[i] = data[i];
int frameSize = Marshal.SizeOf<CanFrame>();
int bytesWritten = LibcNativeMethods.Write(_socketHandle, ref frame, frameSize);
if (bytesWritten != frameSize)
throw new InvalidOperationException($"Failed to send CAN frame with ID 0x{canId:X3} on interface {_interfaceName}. Bytes written: {bytesWritten}, Frame size: {frameSize}");
}, cancellationToken);
}
catch (Exception ex)
{
_logger?.LogError(ex, "Failed to send CAN frame with ID 0x{CanId:X3} on interface {InterfaceName}", canId, _interfaceName);
throw;
}
}
private void ReceiveLoop(CancellationToken cancellationToken)
{
if (_socketHandle == null)
return;
int frameSize = Marshal.SizeOf<CanFrame>();
try
{
while (!cancellationToken.IsCancellationRequested && _isConnected)
{
// Check if socket handle is still valid before attempting to read
if (_socketHandle == null || _socketHandle.IsInvalid)
{
break;
}
try
{
var readFrame = new CanFrame();
int nReadBytes = LibcNativeMethods.Read(_socketHandle, ref readFrame, frameSize);
// Read returns 0 if socket is closed, negative on error
if (nReadBytes <= 0)
{
// Socket closed or error occurred
break;
}
if (nReadBytes > 0)
{
// Check again after blocking Read() returns
if (cancellationToken.IsCancellationRequested || !_isConnected)
{
break;
}
// Check if socket handle is still valid before ioctl
if (_socketHandle == null || _socketHandle.IsInvalid)
{
break;
}
var timeval = new Timeval();
int result = LibcNativeMethods.Ioctl(_socketHandle, SocketCanConstants.SIOCGSTAMP, timeval);
DateTime timestamp = result != -1
? DateTimeOffset.FromUnixTimeSeconds(timeval.Seconds)
.AddMicroseconds(timeval.Microseconds).DateTime
: DateTime.UtcNow;
byte[] data = new byte[readFrame.Length];
for (int i = 0; i < readFrame.Length; i++)
data[i] = readFrame.Data[i];
FrameReceived?.Invoke(this, new CanFrameReceivedEventArgs(
readFrame.CanId,
data,
timestamp));
}
}
catch (Exception ex)
{
// If cancellation is requested, exit gracefully
if (cancellationToken.IsCancellationRequested)
{
break;
}
// If socket is closed/disposed, exit gracefully
if (_socketHandle == null || _socketHandle.IsInvalid || !_isConnected)
{
break;
}
// Other exceptions should be logged and re-thrown
_logger?.LogError(ex, "Error in receive loop for SocketCanBus {InterfaceName}", _interfaceName);
throw;
}
}
}
catch (Exception ex)
{
_logger?.LogError(ex, "Error in receive loop for SocketCanBus {InterfaceName}", _interfaceName);
}
}
public void Dispose()
{
// Cancel receive task first
if (_receiveCts != null)
{
try
{
_receiveCts.Cancel();
}
catch (ObjectDisposedException)
{
// Already disposed, ignore
}
}
// Wait for receive task to finish (with timeout to avoid hanging)
if (_receiveTask != null)
{
try
{
if (!_receiveTask.Wait(TimeSpan.FromSeconds(2)))
{
_logger?.LogWarning("Timeout waiting for receive task to finish during disposal");
}
}
catch (Exception ex)
{
_logger?.LogError(ex, "Error waiting for receive task during disposal");
}
}
// Dispose receive CTS
try
{
_receiveCts?.Dispose();
}
catch (ObjectDisposedException)
{
// Already disposed, ignore
}
_receiveCts = null;
_receiveTask = null;
// Disconnect and dispose socket handle
_isConnected = false;
// Close socket directly if still valid
if (_socketHandle != null)
{
try
{
// Close socket to release resources
int closeResult = LibcNativeMethods.Close(_socketHandle.DangerousGetHandle());
if (closeResult != 0)
{
int errorCode = Marshal.GetLastPInvokeError();
_logger?.LogWarning("Close socket returned error code {ErrorCode} for {InterfaceName}", errorCode, _interfaceName);
}
_socketHandle.Dispose();
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Error disposing socket handle wrapper");
}
_socketHandle = null;
}
GC.SuppressFinalize(this);
}
}

98
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SyncProducer.cs Normal file
View File

@@ -0,0 +1,98 @@
using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen.Enums;
using RobotNet10.CANOpen.Interfaces;
namespace RobotNet10.CANOpen.Services;
/// <summary>
/// SYNC Producer - phát tin nhắn SYNC định kỳ cho synchronous PDOs
/// </summary>
public class SyncProducer : IDisposable
{
private readonly ICanBus _canBus;
private readonly uint _cobId;
private readonly ILogger<SyncProducer>? _logger;
private Timer? _timer;
private byte _counter;
private bool _useCounter;
private bool _isRunning;
public bool IsRunning => _isRunning;
public int IntervalMs { get; private set; }
public SyncProducer(ICanBus canBus, uint cobId = (uint)CanMessageType.Sync, ILogger<SyncProducer>? logger = null)
{
_canBus = canBus ?? throw new ArgumentNullException(nameof(canBus));
_cobId = cobId;
_logger = logger;
_counter = 0;
_useCounter = false;
_isRunning = false;
}
/// <summary>
/// Start SYNC producer với interval tính bằng milliseconds
/// </summary>
/// <param name="intervalMs">SYNC interval in milliseconds (thường 1-100ms)</param>
/// <param name="useCounter">Nếu true, SYNC message sẽ có counter byte (1-240)</param>
public void Start(int intervalMs, bool useCounter = false)
{
if (_isRunning)
Stop();
IntervalMs = intervalMs;
_useCounter = useCounter;
_counter = 0;
_isRunning = true;
_timer = new Timer(SendSyncCallback, null, 0, intervalMs);
}
/// <summary>
/// Stop SYNC producer
/// </summary>
public void Stop()
{
_timer?.Dispose();
_timer = null;
_isRunning = false;
_counter = 0;
}
/// <summary>
/// Gửi một SYNC message thủ công (không dùng timer)
/// </summary>
public async Task SendSyncAsync(CancellationToken ct = default)
{
byte[] data = Array.Empty<byte>();
if (_useCounter)
{
_counter++;
if (_counter > 240)
_counter = 1;
data = new byte[] { _counter };
}
await _canBus.SendFrameAsync(_cobId, data, ct);
}
private async void SendSyncCallback(object? state)
{
try
{
await SendSyncAsync(CancellationToken.None);
}
catch (Exception ex)
{
// Log error nhưng không dừng timer
_logger?.LogError(ex, "Failed to send SYNC message with COB-ID 0x{CobId:X3}", _cobId);
}
}
public void Dispose()
{
Stop();
}
}