Initial commit
This commit is contained in:
@@ -0,0 +1,388 @@
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
using RobotNet10.CANOpen.Interfaces;
|
||||
using RobotNet10.CANOpen.Models;
|
||||
using RobotNet10.CANOpen.Services;
|
||||
|
||||
namespace RobotNet10.CANOpen;
|
||||
|
||||
/// <summary>
|
||||
/// CANOpen Device với đầy đủ các protocols: SDO, NMT, PDO, SYNC, Emergency, Heartbeat
|
||||
/// </summary>
|
||||
public class CanOpenDevice : ICanOpenDevice, IDisposable
|
||||
{
|
||||
private readonly ICanBus _canBus;
|
||||
private readonly SdoClient _sdoClient;
|
||||
private readonly NmtMaster _nmtMaster;
|
||||
private readonly PdoManager _pdoManager;
|
||||
private readonly EmergencyMonitor _emergencyMonitor;
|
||||
private readonly HeartbeatConsumer _heartbeatConsumer;
|
||||
private readonly ILogger<CanOpenDevice>? _logger;
|
||||
private readonly ILoggerFactory? _loggerFactory;
|
||||
private SyncProducer? _syncProducer;
|
||||
|
||||
public byte NodeId { get; }
|
||||
public NmtState State { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Enable automatic state verification via Heartbeat messages
|
||||
/// When enabled, State will be updated automatically when heartbeat is received
|
||||
/// </summary>
|
||||
public bool AutoVerifyStateViaHeartbeat { get; set; } = false;
|
||||
|
||||
// Expose các services để user có thể truy cập
|
||||
public PdoManager Pdo => _pdoManager;
|
||||
public EmergencyMonitor Emergency => _emergencyMonitor;
|
||||
public HeartbeatConsumer Heartbeat => _heartbeatConsumer;
|
||||
public SyncProducer? Sync => _syncProducer;
|
||||
|
||||
// Events từ các protocols
|
||||
public event EventHandler<PdoReceivedEventArgs>? PdoReceived
|
||||
{
|
||||
add => _pdoManager.PdoReceived += value;
|
||||
remove => _pdoManager.PdoReceived -= value;
|
||||
}
|
||||
|
||||
public event EventHandler<EmergencyReceivedEventArgs>? EmergencyReceived
|
||||
{
|
||||
add => _emergencyMonitor.EmergencyReceived += value;
|
||||
remove => _emergencyMonitor.EmergencyReceived -= value;
|
||||
}
|
||||
|
||||
public event EventHandler<HeartbeatReceivedEventArgs>? HeartbeatReceived
|
||||
{
|
||||
add => _heartbeatConsumer.HeartbeatReceived += value;
|
||||
remove => _heartbeatConsumer.HeartbeatReceived -= value;
|
||||
}
|
||||
|
||||
public event EventHandler<HeartbeatTimeoutEventArgs>? HeartbeatTimeout
|
||||
{
|
||||
add => _heartbeatConsumer.HeartbeatTimeout += value;
|
||||
remove => _heartbeatConsumer.HeartbeatTimeout -= value;
|
||||
}
|
||||
|
||||
public CanOpenDevice(ICanBus canBus, byte nodeId, ILogger<CanOpenDevice>? logger = null, ILoggerFactory? loggerFactory = null)
|
||||
{
|
||||
if (nodeId == 0 || nodeId > 127)
|
||||
throw new ArgumentException("NodeId must be between 1 and 127", nameof(nodeId));
|
||||
|
||||
_canBus = canBus ?? throw new ArgumentNullException(nameof(canBus));
|
||||
NodeId = nodeId;
|
||||
State = NmtState.PreOperational;
|
||||
_logger = logger;
|
||||
_loggerFactory = loggerFactory;
|
||||
|
||||
// Create SdoClient với logger
|
||||
var sdoLogger = loggerFactory?.CreateLogger<SdoClient>() ?? null;
|
||||
_sdoClient = new SdoClient(canBus, nodeId, timeout: null, sdoLogger);
|
||||
_nmtMaster = new NmtMaster(canBus);
|
||||
|
||||
// Create PdoManager với logger
|
||||
var pdoLogger = loggerFactory?.CreateLogger<PdoManager>() ?? null;
|
||||
_pdoManager = new PdoManager(canBus, nodeId, pdoLogger);
|
||||
|
||||
_emergencyMonitor = new EmergencyMonitor(canBus);
|
||||
_heartbeatConsumer = new HeartbeatConsumer(canBus);
|
||||
|
||||
// Subscribe to heartbeat events to verify state
|
||||
_heartbeatConsumer.HeartbeatReceived += OnHeartbeatReceived;
|
||||
}
|
||||
|
||||
#region SDO Operations
|
||||
|
||||
public async Task<byte[]> ReadObjectAsync(ushort index, byte subIndex, CancellationToken cancellationToken = default)
|
||||
{
|
||||
return await _sdoClient.UploadAsync(index, subIndex, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task WriteObjectAsync(ushort index, byte subIndex, byte[] data, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await _sdoClient.DownloadAsync(index, subIndex, data, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task<byte> ReadUInt8Async(ushort index, byte subIndex, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var data = await ReadObjectAsync(index, subIndex, cancellationToken);
|
||||
return data.Length > 0 ? data[0] : (byte)0;
|
||||
}
|
||||
|
||||
public async Task<ushort> ReadUInt16Async(ushort index, byte subIndex, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var data = await ReadObjectAsync(index, subIndex, cancellationToken);
|
||||
return data.Length >= 2 ? BitConverter.ToUInt16(data, 0) : (ushort)0;
|
||||
}
|
||||
|
||||
public async Task<uint> ReadUInt32Async(ushort index, byte subIndex, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var data = await ReadObjectAsync(index, subIndex, cancellationToken);
|
||||
return data.Length >= 4 ? BitConverter.ToUInt32(data, 0) : 0u;
|
||||
}
|
||||
|
||||
public async Task<short> ReadInt16Async(ushort index, byte subIndex, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var data = await ReadObjectAsync(index, subIndex, cancellationToken);
|
||||
return data.Length >= 2 ? BitConverter.ToInt16(data, 0) : (short)0;
|
||||
}
|
||||
|
||||
public async Task<int> ReadInt32Async(ushort index, byte subIndex, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var data = await ReadObjectAsync(index, subIndex, cancellationToken);
|
||||
return data.Length >= 4 ? BitConverter.ToInt32(data, 0) : 0;
|
||||
}
|
||||
|
||||
public async Task WriteUInt8Async(ushort index, byte subIndex, byte value, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await WriteObjectAsync(index, subIndex, [value], cancellationToken);
|
||||
}
|
||||
|
||||
public async Task WriteUInt16Async(ushort index, byte subIndex, ushort value, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await WriteObjectAsync(index, subIndex, BitConverter.GetBytes(value), cancellationToken);
|
||||
}
|
||||
|
||||
public async Task WriteUInt32Async(ushort index, byte subIndex, uint value, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await WriteObjectAsync(index, subIndex, BitConverter.GetBytes(value), cancellationToken);
|
||||
}
|
||||
|
||||
public async Task WriteInt16Async(ushort index, byte subIndex, short value, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await WriteObjectAsync(index, subIndex, BitConverter.GetBytes(value), cancellationToken);
|
||||
}
|
||||
|
||||
public async Task WriteInt32Async(ushort index, byte subIndex, int value, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await WriteObjectAsync(index, subIndex, BitConverter.GetBytes(value), cancellationToken);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region NMT Operations
|
||||
|
||||
public async Task SendNmtCommandAsync(NmtCommand command, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await _nmtMaster.SendCommandAsync(command, NodeId, cancellationToken);
|
||||
|
||||
// Update expected state based on command
|
||||
var expectedState = command switch
|
||||
{
|
||||
NmtCommand.Start => NmtState.Operational,
|
||||
NmtCommand.Stop => NmtState.Stopped,
|
||||
NmtCommand.PreOperational => NmtState.PreOperational,
|
||||
_ => State
|
||||
};
|
||||
|
||||
State = expectedState;
|
||||
|
||||
// If auto-verify is enabled, wait for heartbeat to confirm state
|
||||
if (AutoVerifyStateViaHeartbeat && command != NmtCommand.ResetNode && command != NmtCommand.ResetCommunication)
|
||||
{
|
||||
// Wait for heartbeat to verify state (with timeout)
|
||||
var timeout = TimeSpan.FromSeconds(2);
|
||||
var startTime = DateTime.UtcNow;
|
||||
|
||||
while ((DateTime.UtcNow - startTime) < timeout)
|
||||
{
|
||||
var nodeInfo = _heartbeatConsumer.GetNodeInfo(NodeId);
|
||||
if (nodeInfo != null && nodeInfo.LastState == expectedState)
|
||||
{
|
||||
// State verified via heartbeat
|
||||
State = expectedState;
|
||||
return;
|
||||
}
|
||||
|
||||
await Task.Delay(50, cancellationToken);
|
||||
}
|
||||
|
||||
// If timeout, state might not match - log warning but keep expected state
|
||||
// Note: In production, you might want to throw exception or log warning
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Verify current state via Heartbeat
|
||||
/// </summary>
|
||||
/// <param name="timeoutMs">Timeout in milliseconds</param>
|
||||
/// <param name="cancellationToken">Cancellation token</param>
|
||||
/// <returns>True if state matches, false if timeout or mismatch</returns>
|
||||
public async Task<bool> VerifyStateViaHeartbeatAsync(int timeoutMs = 2000, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var nodeInfo = _heartbeatConsumer.GetNodeInfo(NodeId);
|
||||
if (nodeInfo == null)
|
||||
return false;
|
||||
|
||||
var timeout = TimeSpan.FromMilliseconds(timeoutMs);
|
||||
var startTime = DateTime.UtcNow;
|
||||
|
||||
while ((DateTime.UtcNow - startTime) < timeout)
|
||||
{
|
||||
nodeInfo = _heartbeatConsumer.GetNodeInfo(NodeId);
|
||||
if (nodeInfo != null && nodeInfo.LastState == State)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
await Task.Delay(50, cancellationToken);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Handler for heartbeat received events - updates state if auto-verify is enabled
|
||||
/// </summary>
|
||||
private void OnHeartbeatReceived(object? sender, HeartbeatReceivedEventArgs e)
|
||||
{
|
||||
if (AutoVerifyStateViaHeartbeat && e.Heartbeat.NodeId == NodeId)
|
||||
{
|
||||
// Update state from heartbeat
|
||||
State = e.Heartbeat.State;
|
||||
}
|
||||
}
|
||||
|
||||
public async Task StartNodeAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
await SendNmtCommandAsync(NmtCommand.Start, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task StopNodeAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
await SendNmtCommandAsync(NmtCommand.Stop, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task ResetNodeAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
await SendNmtCommandAsync(NmtCommand.ResetNode, cancellationToken);
|
||||
}
|
||||
|
||||
public async Task ResetCommunicationAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
await SendNmtCommandAsync(NmtCommand.ResetCommunication, cancellationToken);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region SYNC Operations
|
||||
|
||||
/// <summary>
|
||||
/// Khởi tạo SYNC producer (chỉ dùng cho master node)
|
||||
/// </summary>
|
||||
public void EnableSyncProducer(int intervalMs, bool useCounter = false)
|
||||
{
|
||||
if (_syncProducer == null)
|
||||
{
|
||||
// Create logger for SyncProducer if loggerFactory is available
|
||||
var syncLogger = _loggerFactory?.CreateLogger<SyncProducer>();
|
||||
_syncProducer = new SyncProducer(_canBus, cobId: (uint)CanMessageType.Sync, syncLogger);
|
||||
}
|
||||
_syncProducer.Start(intervalMs, useCounter);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dừng SYNC producer
|
||||
/// </summary>
|
||||
public void DisableSyncProducer()
|
||||
{
|
||||
_syncProducer?.Stop();
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region PDO Configuration Helpers
|
||||
|
||||
/// <summary>
|
||||
/// Configure standard TPDO1-4 theo CANOpen DS301
|
||||
/// </summary>
|
||||
public void ConfigureStandardTPDOs()
|
||||
{
|
||||
// TPDO1: COB-ID 0x180 + NodeId
|
||||
_pdoManager.ConfigureTPDO(new PdoConfiguration(1, (uint)(0x180 + NodeId)));
|
||||
|
||||
// TPDO2: COB-ID 0x280 + NodeId
|
||||
_pdoManager.ConfigureTPDO(new PdoConfiguration(2, (uint)(0x280 + NodeId)));
|
||||
|
||||
// TPDO3: COB-ID 0x380 + NodeId
|
||||
_pdoManager.ConfigureTPDO(new PdoConfiguration(3, (uint)(0x380 + NodeId)));
|
||||
|
||||
// TPDO4: COB-ID 0x480 + NodeId
|
||||
_pdoManager.ConfigureTPDO(new PdoConfiguration(4, (uint)(0x480 + NodeId)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Configure standard RPDO1-4 theo CANOpen DS301
|
||||
/// </summary>
|
||||
public void ConfigureStandardRPDOs()
|
||||
{
|
||||
// RPDO1: COB-ID 0x200 + NodeId
|
||||
_pdoManager.ConfigureRPDO(new PdoConfiguration(1, (uint)(0x200 + NodeId)));
|
||||
|
||||
// RPDO2: COB-ID 0x300 + NodeId
|
||||
_pdoManager.ConfigureRPDO(new PdoConfiguration(2, (uint)(0x300 + NodeId)));
|
||||
|
||||
// RPDO3: COB-ID 0x400 + NodeId
|
||||
_pdoManager.ConfigureRPDO(new PdoConfiguration(3, (uint)(0x400 + NodeId)));
|
||||
|
||||
// RPDO4: COB-ID 0x500 + NodeId
|
||||
_pdoManager.ConfigureRPDO(new PdoConfiguration(4, (uint)(0x500 + NodeId)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ghi PDO configuration xuống device
|
||||
/// Device phải ở PreOperational state trước khi gọi method này
|
||||
/// </summary>
|
||||
public async Task WritePdoConfigurationToDeviceAsync(bool isTpdo, byte pdoNumber, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await _pdoManager.WritePdoConfigurationToDeviceAsync(this, isTpdo, pdoNumber, cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ghi tất cả PDO configurations xuống device
|
||||
/// Device phải ở PreOperational state trước khi gọi method này
|
||||
/// </summary>
|
||||
/// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support trước khi configure)</param>
|
||||
/// <param name="cancellationToken">Cancellation token</param>
|
||||
public async Task WriteAllPdoConfigurationsToDeviceAsync(string? edsFilePath = null, CancellationToken cancellationToken = default)
|
||||
{
|
||||
await _pdoManager.WriteAllPdoConfigurationsToDeviceAsync(this, edsFilePath, cancellationToken);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Heartbeat Operations
|
||||
|
||||
/// <summary>
|
||||
/// Bắt đầu monitor heartbeat của node này
|
||||
/// </summary>
|
||||
public void EnableHeartbeatMonitoring(int timeoutMs = 2000)
|
||||
{
|
||||
_heartbeatConsumer.MonitorNode(NodeId, timeoutMs);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dừng monitor heartbeat
|
||||
/// </summary>
|
||||
public void DisableHeartbeatMonitoring()
|
||||
{
|
||||
_heartbeatConsumer.StopMonitoring(NodeId);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
// Unsubscribe from events
|
||||
if (_heartbeatConsumer != null)
|
||||
{
|
||||
_heartbeatConsumer.HeartbeatReceived -= OnHeartbeatReceived;
|
||||
}
|
||||
|
||||
// Dispose services
|
||||
_syncProducer?.Dispose();
|
||||
_heartbeatConsumer?.Dispose();
|
||||
_sdoClient?.Dispose();
|
||||
_emergencyMonitor?.Dispose();
|
||||
_pdoManager?.Dispose();
|
||||
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
using Microsoft.Extensions.DependencyInjection;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace RobotNet10.CANOpen;
|
||||
|
||||
/// <summary>
|
||||
/// Extension methods để register CANOpen services vào DI container
|
||||
/// </summary>
|
||||
public static class CanOpenExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Thêm CanOpenManager vào DI container như Singleton service và IHostedService
|
||||
/// CanOpenManager sẽ tự động tạo SocketCanBus từ interface name
|
||||
/// </summary>
|
||||
/// <param name="services">Service collection</param>
|
||||
/// <returns>Service collection để chain calls</returns>
|
||||
public static IServiceCollection AddCanOpenManager(this IServiceCollection services)
|
||||
{
|
||||
services.AddSingleton<CanOpenManager>();
|
||||
services.AddSingleton<ICanOpenManager>(serviceProvider => serviceProvider.GetRequiredService<CanOpenManager>());
|
||||
services.AddHostedService(serviceProvider => serviceProvider.GetRequiredService<CanOpenManager>());
|
||||
|
||||
return services;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
namespace RobotNet10.CANOpen.Enums;
|
||||
|
||||
/// <summary>
|
||||
/// Loại tin nhắn CANOpen dựa trên COB-ID
|
||||
/// </summary>
|
||||
public enum CanMessageType : uint
|
||||
{
|
||||
Nmt = 0x000,
|
||||
Sync = 0x080,
|
||||
Emergency = 0x080,
|
||||
Time = 0x100,
|
||||
Tpdo1 = 0x180,
|
||||
Rpdo1 = 0x200,
|
||||
Tpdo2 = 0x280,
|
||||
Rpdo2 = 0x300,
|
||||
Tpdo3 = 0x380,
|
||||
Rpdo3 = 0x400,
|
||||
Tpdo4 = 0x480,
|
||||
Rpdo4 = 0x500,
|
||||
Tsdo = 0x580,
|
||||
Rsdo = 0x600,
|
||||
Heartbeat = 0x700
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
namespace RobotNet10.CANOpen.Enums;
|
||||
|
||||
/// <summary>
|
||||
/// NMT (Network Management) commands
|
||||
/// </summary>
|
||||
public enum NmtCommand : byte
|
||||
{
|
||||
Start = 0x01,
|
||||
Stop = 0x02,
|
||||
PreOperational = 0x80,
|
||||
ResetNode = 0x81,
|
||||
ResetCommunication = 0x82
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// NMT States
|
||||
/// </summary>
|
||||
public enum NmtState : byte
|
||||
{
|
||||
Unknown = 0xFF,
|
||||
Initializing = 0x00,
|
||||
Stopped = 0x04,
|
||||
Operational = 0x05,
|
||||
PreOperational = 0x7F,
|
||||
BootUp = 0x00
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
namespace RobotNet10.CANOpen.Enums;
|
||||
|
||||
/// <summary>
|
||||
/// Loại PDO (Process Data Object)
|
||||
/// </summary>
|
||||
public enum PdoType
|
||||
{
|
||||
/// <summary>
|
||||
/// Transmit PDO - Dữ liệu gửi từ device
|
||||
/// </summary>
|
||||
Transmit,
|
||||
|
||||
/// <summary>
|
||||
/// Receive PDO - Dữ liệu nhận vào device
|
||||
/// </summary>
|
||||
Receive
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO Transmission Type
|
||||
/// </summary>
|
||||
public enum PdoTransmissionType : byte
|
||||
{
|
||||
/// <summary>
|
||||
/// Acyclic synchronous - Gửi theo lệnh
|
||||
/// </summary>
|
||||
AcyclicSynchronous = 0x00,
|
||||
|
||||
/// <summary>
|
||||
/// Cyclic synchronous - Gửi sau mỗi N SYNC (N = 1-240)
|
||||
/// </summary>
|
||||
CyclicSynchronousMin = 0x01,
|
||||
CyclicSynchronousMax = 0xF0,
|
||||
|
||||
/// <summary>
|
||||
/// Synchronous RTR-only
|
||||
/// </summary>
|
||||
SynchronousRTR = 0xFC,
|
||||
|
||||
/// <summary>
|
||||
/// Asynchronous RTR-only
|
||||
/// </summary>
|
||||
AsynchronousRTR = 0xFD,
|
||||
|
||||
/// <summary>
|
||||
/// Asynchronous - Gửi khi có thay đổi
|
||||
/// </summary>
|
||||
Asynchronous = 0xFE,
|
||||
|
||||
/// <summary>
|
||||
/// Device profile specific
|
||||
/// </summary>
|
||||
DeviceProfile = 0xFF
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO Trigger type
|
||||
/// </summary>
|
||||
public enum PdoTrigger
|
||||
{
|
||||
/// <summary>
|
||||
/// Sync triggered
|
||||
/// </summary>
|
||||
Sync,
|
||||
|
||||
/// <summary>
|
||||
/// Event triggered (change of state)
|
||||
/// </summary>
|
||||
Event,
|
||||
|
||||
/// <summary>
|
||||
/// RTR (Remote Transmission Request)
|
||||
/// </summary>
|
||||
RTR,
|
||||
|
||||
/// <summary>
|
||||
/// Time triggered
|
||||
/// </summary>
|
||||
Timer
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
namespace RobotNet10.CANOpen.Enums;
|
||||
|
||||
/// <summary>
|
||||
/// SDO Abort Codes theo CANOpen DS301
|
||||
/// </summary>
|
||||
public enum SdoAbortCode : uint
|
||||
{
|
||||
// Protocol Errors
|
||||
ToggleBitNotAlternated = 0x05030000,
|
||||
SdoProtocolTimedOut = 0x05040000,
|
||||
CommandSpecifierNotValidOrUnknown = 0x05040001,
|
||||
InvalidBlockSize = 0x05040002,
|
||||
InvalidSequenceNumber = 0x05040003,
|
||||
CrcError = 0x05040004,
|
||||
OutOfMemory = 0x05040005,
|
||||
UnsupportedAccessToAnObject = 0x06010000,
|
||||
AttemptToReadAWriteOnlyObject = 0x06010001,
|
||||
AttemptToWriteAReadOnlyObject = 0x06010002,
|
||||
|
||||
// Object Dictionary Errors
|
||||
ObjectDoesNotExist = 0x06020000,
|
||||
ObjectCannotBeMappedToThePdo = 0x06040041,
|
||||
NumberAndLengthOfObjectsToBeExceed = 0x06040042,
|
||||
GeneralParameterIncompatibility = 0x06040043,
|
||||
GeneralInternalIncompatibility = 0x06040047,
|
||||
ObjectAccessFailedDueToHardwareError = 0x06060000,
|
||||
DataTypeMismatchLengthOfService = 0x06070010,
|
||||
DataTypeMismatchLengthOfServiceTooHigh = 0x06070012,
|
||||
DataTypeMismatchLengthOfServiceTooLow = 0x06070013,
|
||||
SubIndexDoesNotExist = 0x06090011,
|
||||
InvalidValueForParameter = 0x06090030,
|
||||
ValueOfParameterWrittenTooHigh = 0x06090031,
|
||||
ValueOfParameterWrittenTooLow = 0x06090032,
|
||||
MaximumLessMinimum = 0x06090036,
|
||||
ResourceNotAvailable = 0x060A0023,
|
||||
|
||||
// General Errors
|
||||
GeneralError = 0x08000000,
|
||||
DataCannotBeTransferredOrStoredToApplication = 0x08000020,
|
||||
DataCannotBeTransferredLocalControl = 0x08000021,
|
||||
DataCannotBeTransferredDeviceState = 0x08000022,
|
||||
ObjectDictionaryDynamicGenerationFails = 0x08000023,
|
||||
NoDataAvailable = 0x08000024,
|
||||
|
||||
// Manufacturer Specific (0x0000 0000 - 0x0000 FFFF không được sử dụng)
|
||||
// User Defined (0x2000 0000 - 0xFFFF FFFF)
|
||||
|
||||
// Custom/Unknown
|
||||
Unknown = 0xFFFFFFFF
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Extension methods cho SdoAbortCode
|
||||
/// </summary>
|
||||
public static class SdoAbortCodeExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Get human readable description của abort code
|
||||
/// </summary>
|
||||
public static string GetDescription(this SdoAbortCode code)
|
||||
{
|
||||
return code switch
|
||||
{
|
||||
SdoAbortCode.ToggleBitNotAlternated => "Toggle bit not alternated",
|
||||
SdoAbortCode.SdoProtocolTimedOut => "SDO protocol timed out",
|
||||
SdoAbortCode.CommandSpecifierNotValidOrUnknown => "Command specifier not valid or unknown",
|
||||
SdoAbortCode.InvalidBlockSize => "Invalid block size",
|
||||
SdoAbortCode.InvalidSequenceNumber => "Invalid sequence number",
|
||||
SdoAbortCode.CrcError => "CRC error",
|
||||
SdoAbortCode.OutOfMemory => "Out of memory",
|
||||
SdoAbortCode.UnsupportedAccessToAnObject => "Unsupported access to an object",
|
||||
SdoAbortCode.AttemptToReadAWriteOnlyObject => "Attempt to read a write-only object",
|
||||
SdoAbortCode.AttemptToWriteAReadOnlyObject => "Attempt to write a read-only object",
|
||||
SdoAbortCode.ObjectDoesNotExist => "Object does not exist in the object dictionary",
|
||||
SdoAbortCode.ObjectCannotBeMappedToThePdo => "Object cannot be mapped to the PDO",
|
||||
SdoAbortCode.NumberAndLengthOfObjectsToBeExceed => "Number and length of the objects to be mapped would exceed PDO length",
|
||||
SdoAbortCode.GeneralParameterIncompatibility => "General parameter incompatibility reason",
|
||||
SdoAbortCode.GeneralInternalIncompatibility => "General internal incompatibility in the device",
|
||||
SdoAbortCode.ObjectAccessFailedDueToHardwareError => "Access failed due to hardware error",
|
||||
SdoAbortCode.DataTypeMismatchLengthOfService => "Data type does not match, length of service parameter does not match",
|
||||
SdoAbortCode.DataTypeMismatchLengthOfServiceTooHigh => "Data type does not match, length of service parameter too high",
|
||||
SdoAbortCode.DataTypeMismatchLengthOfServiceTooLow => "Data type does not match, length of service parameter too low",
|
||||
SdoAbortCode.SubIndexDoesNotExist => "Sub-index does not exist",
|
||||
SdoAbortCode.InvalidValueForParameter => "Invalid value for parameter (download only)",
|
||||
SdoAbortCode.ValueOfParameterWrittenTooHigh => "Value of parameter written too high (download only)",
|
||||
SdoAbortCode.ValueOfParameterWrittenTooLow => "Value of parameter written too low (download only)",
|
||||
SdoAbortCode.MaximumLessMinimum => "Maximum value is less than minimum value",
|
||||
SdoAbortCode.ResourceNotAvailable => "Resource not available: SDO connection",
|
||||
SdoAbortCode.GeneralError => "General error",
|
||||
SdoAbortCode.DataCannotBeTransferredOrStoredToApplication => "Data cannot be transferred or stored to the application",
|
||||
SdoAbortCode.DataCannotBeTransferredLocalControl => "Data cannot be transferred or stored to the application because of local control",
|
||||
SdoAbortCode.DataCannotBeTransferredDeviceState => "Data cannot be transferred or stored to the application because of the present device state",
|
||||
SdoAbortCode.ObjectDictionaryDynamicGenerationFails => "Object dictionary dynamic generation fails or no object dictionary is present",
|
||||
SdoAbortCode.NoDataAvailable => "No data available",
|
||||
SdoAbortCode.Unknown => "Unknown error",
|
||||
_ => $"SDO Abort Code: 0x{(uint)code:X8}"
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if abort code is manufacturer specific
|
||||
/// </summary>
|
||||
public static bool IsManufacturerSpecific(this SdoAbortCode code)
|
||||
{
|
||||
uint value = (uint)code;
|
||||
return value >= 0x20000000 && value <= 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if abort code is protocol error
|
||||
/// </summary>
|
||||
public static bool IsProtocolError(this SdoAbortCode code)
|
||||
{
|
||||
uint value = (uint)code;
|
||||
return value >= 0x05030000 && value <= 0x05040005;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if abort code is object dictionary error
|
||||
/// </summary>
|
||||
public static bool IsObjectDictionaryError(this SdoAbortCode code)
|
||||
{
|
||||
uint value = (uint)code;
|
||||
return (value >= 0x06010000 && value <= 0x06010002) ||
|
||||
(value >= 0x06020000 && value <= 0x060A0023);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace RobotNet10.CANOpen.Enums;
|
||||
|
||||
public enum SdoCommand : byte
|
||||
{
|
||||
DownloadInitiate = 0x20,
|
||||
DownloadSegment = 0x00,
|
||||
UploadInitiate = 0x40,
|
||||
UploadSegment = 0x60,
|
||||
Abort = 0x80,
|
||||
BlockDownload = 0xC0,
|
||||
BlockUpload = 0xA0
|
||||
}
|
||||
@@ -0,0 +1,187 @@
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
|
||||
namespace RobotNet10.CANOpen.Exceptions;
|
||||
|
||||
/// <summary>
|
||||
/// Base exception cho tất cả CANOpen errors
|
||||
/// </summary>
|
||||
public class CanOpenException : Exception
|
||||
{
|
||||
public ushort ErrorCode { get; }
|
||||
public byte NodeId { get; }
|
||||
|
||||
public CanOpenException(string message) : base(message)
|
||||
{
|
||||
}
|
||||
|
||||
public CanOpenException(string message, Exception innerException) : base(message, innerException)
|
||||
{
|
||||
}
|
||||
|
||||
public CanOpenException(ushort errorCode, string message) : base(message)
|
||||
{
|
||||
ErrorCode = errorCode;
|
||||
}
|
||||
|
||||
public CanOpenException(byte nodeId, ushort errorCode, string message) : base(message)
|
||||
{
|
||||
NodeId = nodeId;
|
||||
ErrorCode = errorCode;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// SDO communication errors
|
||||
/// </summary>
|
||||
public class SdoException : CanOpenException
|
||||
{
|
||||
public uint AbortCode { get; }
|
||||
public ushort Index { get; }
|
||||
public byte SubIndex { get; }
|
||||
|
||||
public SdoException(uint abortCode, string message) : base(message)
|
||||
{
|
||||
AbortCode = abortCode;
|
||||
}
|
||||
|
||||
public SdoException(byte nodeId, ushort index, byte subIndex, uint abortCode, string message)
|
||||
: base(nodeId, 0, message)
|
||||
{
|
||||
Index = index;
|
||||
SubIndex = subIndex;
|
||||
AbortCode = abortCode;
|
||||
}
|
||||
|
||||
public override string Message =>
|
||||
$"SDO Error on Node {NodeId}, Object {Index:X4}h.{SubIndex:X2}h: {base.Message} (Abort Code: 0x{AbortCode:X8})";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO communication errors
|
||||
/// </summary>
|
||||
public class PdoException : CanOpenException
|
||||
{
|
||||
public byte PdoNumber { get; }
|
||||
public uint CobId { get; }
|
||||
|
||||
public PdoException(string message) : base(message)
|
||||
{
|
||||
}
|
||||
|
||||
public PdoException(byte nodeId, byte pdoNumber, uint cobId, string message) : base(nodeId, 0, message)
|
||||
{
|
||||
PdoNumber = pdoNumber;
|
||||
CobId = cobId;
|
||||
}
|
||||
|
||||
public override string Message =>
|
||||
$"PDO{PdoNumber} Error on Node {NodeId} (COB-ID: 0x{CobId:X3}): {base.Message}";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// NMT communication errors
|
||||
/// </summary>
|
||||
public class NmtException : CanOpenException
|
||||
{
|
||||
public NmtCommand? Command { get; }
|
||||
|
||||
public NmtException(string message) : base(message)
|
||||
{
|
||||
}
|
||||
|
||||
public NmtException(byte nodeId, NmtCommand command, string message) : base(nodeId, 0, message)
|
||||
{
|
||||
Command = command;
|
||||
}
|
||||
|
||||
public override string Message =>
|
||||
Command.HasValue
|
||||
? $"NMT Error on Node {NodeId}, Command {Command}: {base.Message}"
|
||||
: base.Message;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Emergency message errors
|
||||
/// </summary>
|
||||
public class EmergencyException : CanOpenException
|
||||
{
|
||||
public ushort EmergencyErrorCode { get; }
|
||||
public byte ErrorRegister { get; }
|
||||
public byte[] ManufacturerSpecificData { get; }
|
||||
|
||||
public EmergencyException(byte nodeId, ushort emergencyErrorCode, byte errorRegister,
|
||||
byte[] manufacturerData, string message) : base(nodeId, emergencyErrorCode, message)
|
||||
{
|
||||
EmergencyErrorCode = emergencyErrorCode;
|
||||
ErrorRegister = errorRegister;
|
||||
ManufacturerSpecificData = manufacturerData;
|
||||
}
|
||||
|
||||
public override string Message =>
|
||||
$"Emergency on Node {NodeId}: {base.Message} (Error Code: 0x{EmergencyErrorCode:X4}, Error Register: 0x{ErrorRegister:X2})";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Configuration validation errors
|
||||
/// </summary>
|
||||
public class ConfigurationException : CanOpenException
|
||||
{
|
||||
public string Parameter { get; }
|
||||
public object? InvalidValue { get; }
|
||||
|
||||
public ConfigurationException(string parameter, string message) : base(message)
|
||||
{
|
||||
Parameter = parameter;
|
||||
}
|
||||
|
||||
public ConfigurationException(string parameter, object? invalidValue, string message) : base(message)
|
||||
{
|
||||
Parameter = parameter;
|
||||
InvalidValue = invalidValue;
|
||||
}
|
||||
|
||||
public override string Message =>
|
||||
InvalidValue != null
|
||||
? $"Configuration Error for '{Parameter}' = {InvalidValue}: {base.Message}"
|
||||
: $"Configuration Error for '{Parameter}': {base.Message}";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Timeout errors
|
||||
/// </summary>
|
||||
public class CanOpenTimeoutException : CanOpenException
|
||||
{
|
||||
public TimeSpan Timeout { get; }
|
||||
public string Operation { get; }
|
||||
|
||||
public CanOpenTimeoutException(string operation, TimeSpan timeout, string message) : base(message)
|
||||
{
|
||||
Operation = operation;
|
||||
Timeout = timeout;
|
||||
}
|
||||
|
||||
public override string Message =>
|
||||
$"Timeout after {Timeout.TotalMilliseconds}ms during {Operation}: {base.Message}";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// CAN bus communication errors
|
||||
/// </summary>
|
||||
public class CanBusException : CanOpenException
|
||||
{
|
||||
public string InterfaceName { get; }
|
||||
|
||||
public CanBusException(string interfaceName, string message) : base(message)
|
||||
{
|
||||
InterfaceName = interfaceName;
|
||||
}
|
||||
|
||||
public CanBusException(string interfaceName, string message, Exception innerException)
|
||||
: base(message, innerException)
|
||||
{
|
||||
InterfaceName = interfaceName;
|
||||
}
|
||||
|
||||
public override string Message =>
|
||||
$"CAN Bus Error on interface '{InterfaceName}': {base.Message}";
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
using RobotNet10.CANOpen.Models;
|
||||
using RobotNet10.CANOpen.Services;
|
||||
|
||||
namespace RobotNet10.CANOpen.Interfaces;
|
||||
|
||||
public interface ICanOpenDevice
|
||||
{
|
||||
byte NodeId { get; }
|
||||
NmtState State { get; }
|
||||
|
||||
/// <summary>
|
||||
/// PDO Manager để quản lý PDOs
|
||||
/// </summary>
|
||||
PdoManager Pdo { get; }
|
||||
|
||||
// Events
|
||||
event EventHandler<PdoReceivedEventArgs>? PdoReceived;
|
||||
event EventHandler<EmergencyReceivedEventArgs>? EmergencyReceived;
|
||||
event EventHandler<HeartbeatReceivedEventArgs>? HeartbeatReceived;
|
||||
event EventHandler<HeartbeatTimeoutEventArgs>? HeartbeatTimeout;
|
||||
|
||||
// SDO Operations
|
||||
Task<byte[]> ReadObjectAsync(ushort index, byte subIndex, CancellationToken cancellationToken = default);
|
||||
Task WriteObjectAsync(ushort index, byte subIndex, byte[] data, CancellationToken cancellationToken = default);
|
||||
|
||||
// Helper methods for reading
|
||||
Task<byte> ReadUInt8Async(ushort index, byte subIndex, CancellationToken cancellationToken = default);
|
||||
Task<ushort> ReadUInt16Async(ushort index, byte subIndex, CancellationToken cancellationToken = default);
|
||||
Task<uint> ReadUInt32Async(ushort index, byte subIndex, CancellationToken cancellationToken = default);
|
||||
Task<short> ReadInt16Async(ushort index, byte subIndex, CancellationToken cancellationToken = default);
|
||||
Task<int> ReadInt32Async(ushort index, byte subIndex, CancellationToken cancellationToken = default);
|
||||
|
||||
// Helper methods for writing
|
||||
Task WriteUInt8Async(ushort index, byte subIndex, byte value, CancellationToken cancellationToken = default);
|
||||
Task WriteUInt16Async(ushort index, byte subIndex, ushort value, CancellationToken cancellationToken = default);
|
||||
Task WriteUInt32Async(ushort index, byte subIndex, uint value, CancellationToken cancellationToken = default);
|
||||
Task WriteInt16Async(ushort index, byte subIndex, short value, CancellationToken cancellationToken = default);
|
||||
Task WriteInt32Async(ushort index, byte subIndex, int value, CancellationToken cancellationToken = default);
|
||||
|
||||
// NMT Operations
|
||||
Task SendNmtCommandAsync(NmtCommand command, CancellationToken cancellationToken = default);
|
||||
Task StartNodeAsync(CancellationToken cancellationToken = default);
|
||||
Task StopNodeAsync(CancellationToken cancellationToken = default);
|
||||
Task ResetNodeAsync(CancellationToken cancellationToken = default);
|
||||
Task ResetCommunicationAsync(CancellationToken cancellationToken = default);
|
||||
|
||||
// PDO Configuration Operations
|
||||
/// <summary>
|
||||
/// Ghi PDO configuration xuống device
|
||||
/// Device phải ở PreOperational state trước khi gọi method này
|
||||
/// </summary>
|
||||
Task WritePdoConfigurationToDeviceAsync(bool isTpdo, byte pdoNumber, CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Ghi tất cả PDO configurations xuống device
|
||||
/// Device phải ở PreOperational state trước khi gọi method này
|
||||
/// </summary>
|
||||
/// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support trước khi configure)</param>
|
||||
/// <param name="cancellationToken">Cancellation token</param>
|
||||
Task WriteAllPdoConfigurationsToDeviceAsync(string? edsFilePath = null, CancellationToken cancellationToken = default);
|
||||
}
|
||||
|
||||
public interface ICanBus : IDisposable
|
||||
{
|
||||
string InterfaceName { get; }
|
||||
bool IsConnected { get; }
|
||||
|
||||
Task ConnectAsync(CancellationToken cancellationToken = default);
|
||||
Task DisconnectAsync(CancellationToken cancellationToken = default);
|
||||
Task SendFrameAsync(uint canId, byte[] data, CancellationToken cancellationToken = default);
|
||||
|
||||
event EventHandler<CanFrameReceivedEventArgs>? FrameReceived;
|
||||
}
|
||||
|
||||
public class CanFrameReceivedEventArgs(uint canId, byte[] data, DateTime timestamp) : EventArgs
|
||||
{
|
||||
public uint CanId { get; init; } = canId;
|
||||
public byte[] Data { get; init; } = data;
|
||||
public DateTime Timestamp { get; init; } = timestamp;
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
using RobotNet10.CANOpen.Interfaces;
|
||||
|
||||
namespace RobotNet10.CANOpen;
|
||||
|
||||
/// <summary>
|
||||
/// Service quản lý ICanBus và CanOpenDevice instances
|
||||
/// Đảm bảo chỉ có một ICanBus instance cho mỗi CAN interface
|
||||
/// </summary>
|
||||
public interface ICanOpenManager : IDisposable
|
||||
{
|
||||
/// <summary>
|
||||
/// Lấy hoặc tạo ICanBus instance cho interface name
|
||||
/// Nếu đã tồn tại, trả về instance hiện có
|
||||
/// </summary>
|
||||
/// <param name="interfaceName">Tên CAN interface (ví dụ: "can0", "can1")</param>
|
||||
/// <param name="cancellationToken">Cancellation token</param>
|
||||
/// <returns>ICanBus instance (đã được connect)</returns>
|
||||
Task<ICanBus> GetOrCreateCanBusAsync(string interfaceName, CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Lấy ICanBus instance đã tồn tại (không tạo mới nếu chưa có)
|
||||
/// </summary>
|
||||
/// <param name="interfaceName">Tên CAN interface</param>
|
||||
/// <returns>ICanBus instance hoặc null nếu chưa tồn tại</returns>
|
||||
ICanBus? GetCanBus(string interfaceName);
|
||||
|
||||
/// <summary>
|
||||
/// Lấy hoặc tạo CanOpenDevice instance cho (interfaceName, nodeId) pair
|
||||
/// </summary>
|
||||
/// <param name="interfaceName">Tên CAN interface</param>
|
||||
/// <param name="nodeId">NodeId của device (1-127)</param>
|
||||
/// <param name="cancellationToken">Cancellation token</param>
|
||||
/// <returns>CanOpenDevice instance</returns>
|
||||
Task<CanOpenDevice> GetOrCreateDeviceAsync(string interfaceName, byte nodeId, CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Lấy CanOpenDevice instance đã tồn tại (không tạo mới nếu chưa có)
|
||||
/// </summary>
|
||||
/// <param name="interfaceName">Tên CAN interface</param>
|
||||
/// <param name="nodeId">NodeId của device</param>
|
||||
/// <returns>CanOpenDevice instance hoặc null nếu chưa tồn tại</returns>
|
||||
CanOpenDevice? GetDevice(string interfaceName, byte nodeId);
|
||||
|
||||
/// <summary>
|
||||
/// Xóa CanOpenDevice instance khỏi cache và dispose nó
|
||||
/// </summary>
|
||||
/// <param name="interfaceName">Tên CAN interface</param>
|
||||
/// <param name="nodeId">NodeId của device</param>
|
||||
/// <returns>True nếu đã xóa thành công</returns>
|
||||
bool RemoveDevice(string interfaceName, byte nodeId);
|
||||
|
||||
/// <summary>
|
||||
/// Xóa ICanBus instance khỏi cache và dispose nó
|
||||
/// Lưu ý: Sẽ dispose tất cả CanOpenDevice đang sử dụng ICanBus này
|
||||
/// </summary>
|
||||
/// <param name="interfaceName">Tên CAN interface</param>
|
||||
/// <returns>True nếu đã xóa thành công</returns>
|
||||
Task<bool> RemoveCanBusAsync(string interfaceName, CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>
|
||||
/// Lấy danh sách tất cả interface names đang được quản lý
|
||||
/// </summary>
|
||||
IReadOnlyList<string> GetManagedInterfaces();
|
||||
|
||||
/// <summary>
|
||||
/// Lấy danh sách tất cả nodeIds đang được quản lý cho một interface
|
||||
/// </summary>
|
||||
/// <param name="interfaceName">Tên CAN interface</param>
|
||||
/// <returns>Danh sách nodeIds</returns>
|
||||
IReadOnlyList<byte> GetManagedNodeIds(string interfaceName);
|
||||
|
||||
/// <summary>
|
||||
/// Kiểm tra xem interface có đang được quản lý không
|
||||
/// </summary>
|
||||
bool IsInterfaceManaged(string interfaceName);
|
||||
|
||||
/// <summary>
|
||||
/// Kiểm tra xem device có đang được quản lý không
|
||||
/// </summary>
|
||||
bool IsDeviceManaged(string interfaceName, byte nodeId);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,167 @@
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
/// <summary>
|
||||
/// Emergency (EMCY) Error Codes theo CANopen standard
|
||||
/// </summary>
|
||||
public enum EmergencyErrorCode : ushort
|
||||
{
|
||||
// 0x00xx: Error Reset / No Error
|
||||
ErrorReset = 0x0000,
|
||||
|
||||
// 0x10xx: Generic Error
|
||||
GenericError = 0x1000,
|
||||
|
||||
// 0x20xx: Current
|
||||
CurrentGeneric = 0x2000,
|
||||
CurrentInputSide = 0x2100,
|
||||
CurrentInsideDevice = 0x2200,
|
||||
CurrentOutputSide = 0x2300,
|
||||
|
||||
// 0x21xx: Current, device input side
|
||||
OverCurrent = 0x2110,
|
||||
|
||||
// 0x22xx: Current inside the device
|
||||
ShortCircuit = 0x2220,
|
||||
|
||||
// 0x23xx: Current, device output side
|
||||
LoadDump = 0x2330,
|
||||
|
||||
// 0x30xx: Voltage
|
||||
VoltageGeneric = 0x3000,
|
||||
MainsVoltage = 0x3100,
|
||||
VoltageInsideDevice = 0x3200,
|
||||
OutputVoltage = 0x3300,
|
||||
|
||||
// 0x31xx: Mains Voltage
|
||||
UnderVoltage = 0x3110,
|
||||
OverVoltage = 0x3120,
|
||||
|
||||
// 0x40xx: Temperature
|
||||
TemperatureGeneric = 0x4000,
|
||||
AmbientTemperature = 0x4100,
|
||||
DeviceTemperature = 0x4200,
|
||||
|
||||
// 0x41xx: Ambient Temperature
|
||||
TooHigh = 0x4110,
|
||||
TooLow = 0x4120,
|
||||
|
||||
// 0x50xx: Device Hardware
|
||||
DeviceHardware = 0x5000,
|
||||
|
||||
// 0x60xx: Device Software
|
||||
DeviceSoftware = 0x6000,
|
||||
InternalSoftware = 0x6100,
|
||||
UserSoftware = 0x6200,
|
||||
DataSet = 0x6300,
|
||||
|
||||
// 0x70xx: Additional Modules
|
||||
AdditionalModules = 0x7000,
|
||||
|
||||
// 0x80xx: Monitoring
|
||||
Monitoring = 0x8000,
|
||||
Communication = 0x8100,
|
||||
ProtocolError = 0x8200,
|
||||
|
||||
// 0x81xx: Communication
|
||||
CanOverrun = 0x8110,
|
||||
ErrorPassive = 0x8120,
|
||||
HeartbeatError = 0x8130,
|
||||
BusOffRecovered = 0x8140,
|
||||
|
||||
// 0x82xx: Protocol Error
|
||||
PdoNotProcessed = 0x8210,
|
||||
PdoLengthExceeded = 0x8220,
|
||||
DamMpdo = 0x8230,
|
||||
SyncDataLength = 0x8240,
|
||||
|
||||
// 0x90xx: External Error
|
||||
ExternalError = 0x9000,
|
||||
|
||||
// 0xF0xx: Additional Functions
|
||||
AdditionalFunctions = 0xF000,
|
||||
|
||||
// 0xFFxx: Device specific
|
||||
DeviceSpecific = 0xFF00
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Emergency Message theo CANopen
|
||||
/// </summary>
|
||||
public readonly struct EmergencyMessage(byte nodeId, EmergencyErrorCode errorCode, byte errorRegister,
|
||||
byte[] manufacturerError, DateTime timestamp)
|
||||
{
|
||||
/// <summary>
|
||||
/// Node ID của device gửi emergency
|
||||
/// </summary>
|
||||
public byte NodeId { get; init; } = nodeId;
|
||||
|
||||
/// <summary>
|
||||
/// Emergency Error Code (16-bit)
|
||||
/// </summary>
|
||||
public EmergencyErrorCode ErrorCode { get; init; } = errorCode;
|
||||
|
||||
/// <summary>
|
||||
/// Error Register (8-bit)
|
||||
/// </summary>
|
||||
public byte ErrorRegister { get; init; } = errorRegister;
|
||||
|
||||
/// <summary>
|
||||
/// Manufacturer Specific Error Code (40-bit / 5 bytes)
|
||||
/// </summary>
|
||||
public byte[] ManufacturerError { get; init; } = manufacturerError;
|
||||
|
||||
/// <summary>
|
||||
/// Timestamp khi nhận emergency
|
||||
/// </summary>
|
||||
public DateTime Timestamp { get; init; } = timestamp;
|
||||
|
||||
/// <summary>
|
||||
/// Parse Emergency message từ CAN frame
|
||||
/// </summary>
|
||||
public static EmergencyMessage FromCanFrame(uint canId, byte[] data, DateTime timestamp)
|
||||
{
|
||||
if (data.Length < 8)
|
||||
throw new ArgumentException($"Emergency message must be 8 bytes: {BitConverter.ToString(data)}");
|
||||
|
||||
byte nodeId = (byte)(canId & 0x7F);
|
||||
ushort errorCode = (ushort)(data[0] | (data[1] << 8));
|
||||
byte errorRegister = data[2];
|
||||
byte[] manufacturerError = new byte[5];
|
||||
Array.Copy(data, 3, manufacturerError, 0, 5);
|
||||
|
||||
return new EmergencyMessage(nodeId, (EmergencyErrorCode)errorCode, errorRegister,
|
||||
manufacturerError, timestamp);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert sang CAN frame data
|
||||
/// </summary>
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
byte[] data = new byte[8];
|
||||
data[0] = (byte)((ushort)ErrorCode & 0xFF);
|
||||
data[1] = (byte)(((ushort)ErrorCode >> 8) & 0xFF);
|
||||
data[2] = ErrorRegister;
|
||||
|
||||
if (ManufacturerError != null && ManufacturerError.Length >= 5)
|
||||
{
|
||||
Array.Copy(ManufacturerError, 0, data, 3, 5);
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
// Error Register bit definitions
|
||||
public bool GenericErrorBit => (ErrorRegister & 0x01) != 0;
|
||||
public bool CurrentErrorBit => (ErrorRegister & 0x02) != 0;
|
||||
public bool VoltageErrorBit => (ErrorRegister & 0x04) != 0;
|
||||
public bool TemperatureErrorBit => (ErrorRegister & 0x08) != 0;
|
||||
public bool CommunicationErrorBit => (ErrorRegister & 0x10) != 0;
|
||||
public bool DeviceProfileErrorBit => (ErrorRegister & 0x20) != 0;
|
||||
public bool ManufacturerErrorBit => (ErrorRegister & 0x80) != 0;
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return $"EMCY[Node {NodeId}]: Code=0x{(ushort)ErrorCode:X4}, Register=0x{ErrorRegister:X2}";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
public readonly struct NmtMessage(NmtCommand command, byte nodeId)
|
||||
{
|
||||
public NmtCommand Command { get; init; } = command;
|
||||
public byte NodeId { get; init; } = nodeId;
|
||||
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
return [(byte)Command, NodeId];
|
||||
}
|
||||
|
||||
public static NmtMessage FromBytes(byte[] data)
|
||||
{
|
||||
if (data.Length < 2)
|
||||
throw new ArgumentException("NMT message must be at least 2 bytes");
|
||||
|
||||
return new NmtMessage
|
||||
{
|
||||
Command = (NmtCommand)data[0],
|
||||
NodeId = data[1]
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
public readonly struct HeartbeatMessage(byte nodeId, NmtState state)
|
||||
{
|
||||
public byte NodeId { get; init; } = nodeId;
|
||||
public NmtState State { get; init; } = state;
|
||||
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
return [(byte)State];
|
||||
}
|
||||
|
||||
public static HeartbeatMessage FromBytes(uint canId, byte[] data)
|
||||
{
|
||||
if (data.Length < 1)
|
||||
throw new ArgumentException("Heartbeat message must be at least 1 byte");
|
||||
|
||||
byte nodeId = (byte)(canId & 0x7F);
|
||||
|
||||
return new HeartbeatMessage
|
||||
{
|
||||
NodeId = nodeId,
|
||||
State = (NmtState)data[0]
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
public readonly struct ObjectDictionaryAddress(ushort index, byte subIndex = 0)
|
||||
{
|
||||
public ushort Index { get; init; } = index;
|
||||
public byte SubIndex { get; init; } = subIndex;
|
||||
|
||||
public override string ToString() => $"0x{Index:X4}:{SubIndex:X2}";
|
||||
|
||||
public static bool operator ==(ObjectDictionaryAddress left, ObjectDictionaryAddress right)
|
||||
=> left.Index == right.Index && left.SubIndex == right.SubIndex;
|
||||
|
||||
public static bool operator !=(ObjectDictionaryAddress left, ObjectDictionaryAddress right)
|
||||
=> !(left == right);
|
||||
|
||||
public override bool Equals(object? obj)
|
||||
=> obj is ObjectDictionaryAddress address && this == address;
|
||||
|
||||
public override int GetHashCode()
|
||||
=> HashCode.Combine(Index, SubIndex);
|
||||
}
|
||||
@@ -0,0 +1,276 @@
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
/// <summary>
|
||||
/// PDO Mapping entry (đại diện cho 1 object trong PDO)
|
||||
/// </summary>
|
||||
public readonly struct PdoMapping(ushort index, byte subIndex, byte bitLength)
|
||||
{
|
||||
public ushort Index { get; init; } = index;
|
||||
public byte SubIndex { get; init; } = subIndex;
|
||||
public byte BitLength { get; init; } = bitLength;
|
||||
|
||||
/// <summary>
|
||||
/// Convert sang mapping value format (theo DS301)
|
||||
/// Format: [Index:16][SubIndex:8][BitLength:8]
|
||||
/// </summary>
|
||||
public uint ToMappingValue()
|
||||
{
|
||||
return ((uint)Index << 16) | ((uint)SubIndex << 8) | BitLength;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Parse từ mapping value
|
||||
/// </summary>
|
||||
public static PdoMapping FromMappingValue(uint mappingValue)
|
||||
{
|
||||
ushort index = (ushort)((mappingValue >> 16) & 0xFFFF);
|
||||
byte subIndex = (byte)((mappingValue >> 8) & 0xFF);
|
||||
byte bitLength = (byte)(mappingValue & 0xFF);
|
||||
|
||||
return new PdoMapping(index, subIndex, bitLength);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO Configuration (TPDO hoặc RPDO)
|
||||
/// </summary>
|
||||
public class PdoConfiguration
|
||||
{
|
||||
private readonly List<PdoMapping> _mappings = [];
|
||||
|
||||
public PdoConfiguration()
|
||||
{
|
||||
}
|
||||
|
||||
public PdoConfiguration(byte pdoNumber, uint cobId, PdoTransmissionType transmissionType = PdoTransmissionType.Asynchronous)
|
||||
{
|
||||
if (pdoNumber < 1 || pdoNumber > 4)
|
||||
throw new ArgumentException("PDO number must be between 1 and 4", nameof(pdoNumber));
|
||||
|
||||
PdoNumber = pdoNumber;
|
||||
CobId = cobId;
|
||||
TransmissionType = transmissionType;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO number (1-4 cho TPDO, 1-4 cho RPDO)
|
||||
/// </summary>
|
||||
public byte PdoNumber { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// COB-ID của PDO
|
||||
/// </summary>
|
||||
public uint CobId { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Transmission type
|
||||
/// </summary>
|
||||
public PdoTransmissionType TransmissionType { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Inhibit time (x100 microseconds)
|
||||
/// </summary>
|
||||
public ushort InhibitTime { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Event timer (milliseconds)
|
||||
/// </summary>
|
||||
public ushort EventTimer { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Danh sách các mapped objects (read-only)
|
||||
/// </summary>
|
||||
public IReadOnlyList<PdoMapping> Mappings => _mappings.AsReadOnly();
|
||||
|
||||
/// <summary>
|
||||
/// Check if PDO is valid (bit 31 của COB-ID = 0)
|
||||
/// </summary>
|
||||
public bool IsValid => (CobId & 0x80000000) == 0;
|
||||
|
||||
/// <summary>
|
||||
/// Check if RTR is allowed (bit 30 của COB-ID = 0)
|
||||
/// </summary>
|
||||
public bool RtrAllowed => (CobId & 0x40000000) == 0;
|
||||
|
||||
/// <summary>
|
||||
/// Total bits mapped in this PDO
|
||||
/// </summary>
|
||||
public int TotalMappedBits => _mappings.Sum(m => m.BitLength);
|
||||
|
||||
/// <summary>
|
||||
/// Check if mapping is valid (không vượt quá 64 bits)
|
||||
/// </summary>
|
||||
public bool IsMappingValid => TotalMappedBits <= 64;
|
||||
|
||||
/// <summary>
|
||||
/// Add mapping với validation
|
||||
/// </summary>
|
||||
public void AddMapping(PdoMapping mapping)
|
||||
{
|
||||
if (mapping.BitLength == 0 || mapping.BitLength > 64)
|
||||
throw new ArgumentException("BitLength must be between 1 and 64", nameof(mapping));
|
||||
|
||||
if (TotalMappedBits + mapping.BitLength > 64)
|
||||
throw new InvalidOperationException($"Adding this mapping would exceed 64 bits limit. Current: {TotalMappedBits}, Adding: {mapping.BitLength}");
|
||||
|
||||
_mappings.Add(mapping);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Remove mapping
|
||||
/// </summary>
|
||||
public bool RemoveMapping(PdoMapping mapping)
|
||||
{
|
||||
return _mappings.Remove(mapping);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clear all mappings
|
||||
/// </summary>
|
||||
public void ClearMappings()
|
||||
{
|
||||
_mappings.Clear();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Validate entire configuration
|
||||
/// </summary>
|
||||
public ValidationResult ValidateConfiguration()
|
||||
{
|
||||
var errors = new List<string>();
|
||||
|
||||
if (PdoNumber < 1 || PdoNumber > 4)
|
||||
errors.Add("PDO number must be between 1 and 4");
|
||||
|
||||
if (!IsValid)
|
||||
errors.Add("PDO is disabled (bit 31 of COB-ID is set)");
|
||||
|
||||
if (!IsMappingValid)
|
||||
errors.Add($"Total mapped bits ({TotalMappedBits}) exceeds 64 bits limit");
|
||||
|
||||
if (_mappings.Count == 0)
|
||||
errors.Add("No mappings configured");
|
||||
|
||||
// Validate individual mappings
|
||||
for (int i = 0; i < _mappings.Count; i++)
|
||||
{
|
||||
var mapping = _mappings[i];
|
||||
if (mapping.Index == 0)
|
||||
errors.Add($"Mapping {i}: Invalid index (0)");
|
||||
|
||||
if (mapping.BitLength == 0)
|
||||
errors.Add($"Mapping {i}: Invalid bit length (0)");
|
||||
}
|
||||
|
||||
return new ValidationResult(errors.Count == 0, errors);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO Data (received or to-send)
|
||||
/// </summary>
|
||||
public readonly struct PdoData(byte pdoNumber, uint cobId, byte[] data, DateTime timestamp)
|
||||
{
|
||||
public byte PdoNumber { get; init; } = pdoNumber;
|
||||
public uint CobId { get; init; } = cobId;
|
||||
public byte[] Data { get; init; } = data;
|
||||
public DateTime Timestamp { get; init; } = timestamp;
|
||||
|
||||
/// <summary>
|
||||
/// Extract một value từ PDO data theo mapping
|
||||
/// </summary>
|
||||
public T ExtractValue<T>(PdoMapping mapping) where T : struct
|
||||
{
|
||||
if (mapping.BitLength == 0 || mapping.BitLength > 64)
|
||||
throw new ArgumentException("BitLength must be 1-64");
|
||||
|
||||
int bitOffset = 0;
|
||||
foreach (var _ in Enumerable.Range(0, mapping.SubIndex))
|
||||
{
|
||||
// Tính bit offset dựa trên các mappings trước đó
|
||||
// (simplified - trong thực tế cần biết tất cả mappings)
|
||||
}
|
||||
|
||||
return ExtractValue<T>(bitOffset, mapping.BitLength);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Extract value tại bit offset cụ thể
|
||||
/// </summary>
|
||||
public T ExtractValue<T>(int bitOffset, int bitLength) where T : struct
|
||||
{
|
||||
ulong value = 0;
|
||||
int byteOffset = bitOffset / 8;
|
||||
int bitInByteOffset = bitOffset % 8;
|
||||
int bitsRead = 0;
|
||||
|
||||
while (bitsRead < bitLength && byteOffset < Data.Length)
|
||||
{
|
||||
int bitsToRead = Math.Min(8 - bitInByteOffset, bitLength - bitsRead);
|
||||
byte mask = (byte)((1 << bitsToRead) - 1);
|
||||
byte byteValue = (byte)((Data[byteOffset] >> bitInByteOffset) & mask);
|
||||
|
||||
value |= (ulong)byteValue << bitsRead;
|
||||
|
||||
bitsRead += bitsToRead;
|
||||
byteOffset++;
|
||||
bitInByteOffset = 0;
|
||||
}
|
||||
|
||||
// Convert to target type with sign extension if needed
|
||||
return ConvertValue<T>(value, bitLength);
|
||||
}
|
||||
|
||||
private static T ConvertValue<T>(ulong value, int bitLength) where T : struct
|
||||
{
|
||||
var type = typeof(T);
|
||||
|
||||
if (type == typeof(bool))
|
||||
return (T)(object)(value != 0);
|
||||
|
||||
if (type == typeof(byte))
|
||||
return (T)(object)(byte)value;
|
||||
|
||||
if (type == typeof(sbyte))
|
||||
{
|
||||
// Sign extension
|
||||
if (bitLength < 8 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= (ulong)((1L << bitLength) - 1) << bitLength;
|
||||
return (T)(object)(sbyte)value;
|
||||
}
|
||||
|
||||
if (type == typeof(ushort))
|
||||
return (T)(object)(ushort)value;
|
||||
|
||||
if (type == typeof(short))
|
||||
{
|
||||
if (bitLength < 16 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= (ulong)((1L << bitLength) - 1) << bitLength;
|
||||
return (T)(object)(short)value;
|
||||
}
|
||||
|
||||
if (type == typeof(uint))
|
||||
return (T)(object)(uint)value;
|
||||
|
||||
if (type == typeof(int))
|
||||
{
|
||||
if (bitLength < 32 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= (ulong)((1L << bitLength) - 1) << bitLength;
|
||||
return (T)(object)(int)value;
|
||||
}
|
||||
|
||||
if (type == typeof(ulong))
|
||||
return (T)(object)value;
|
||||
|
||||
if (type == typeof(long))
|
||||
{
|
||||
if (bitLength < 64 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= ulong.MaxValue << bitLength;
|
||||
return (T)(object)(long)value;
|
||||
}
|
||||
|
||||
throw new NotSupportedException($"Type {type.Name} is not supported");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,204 @@
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
public readonly struct SdoRequest
|
||||
{
|
||||
public byte CommandSpecifier { get; init; }
|
||||
public ushort Index { get; init; }
|
||||
public byte SubIndex { get; init; }
|
||||
public uint Data { get; init; }
|
||||
|
||||
public SdoRequest(SdoCommand command, ushort index, byte subIndex, uint data = 0)
|
||||
{
|
||||
CommandSpecifier = (byte)command;
|
||||
Index = index;
|
||||
SubIndex = subIndex;
|
||||
Data = data;
|
||||
}
|
||||
|
||||
public SdoRequest(SdoCommand command, ObjectDictionaryAddress address, uint data = 0)
|
||||
: this(command, address.Index, address.SubIndex, data)
|
||||
{
|
||||
}
|
||||
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
var bytes = new byte[8];
|
||||
bytes[0] = CommandSpecifier;
|
||||
bytes[1] = (byte)(Index & 0xFF);
|
||||
bytes[2] = (byte)((Index >> 8) & 0xFF);
|
||||
bytes[3] = SubIndex;
|
||||
bytes[4] = (byte)(Data & 0xFF);
|
||||
bytes[5] = (byte)((Data >> 8) & 0xFF);
|
||||
bytes[6] = (byte)((Data >> 16) & 0xFF);
|
||||
bytes[7] = (byte)((Data >> 24) & 0xFF);
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static SdoRequest CreateUpload(ushort index, byte subIndex)
|
||||
=> new(SdoCommand.UploadInitiate, index, subIndex);
|
||||
|
||||
public static SdoRequest CreateDownload(ushort index, byte subIndex, byte[] data)
|
||||
{
|
||||
if (data.Length > 4)
|
||||
throw new ArgumentException("Use CreateDownloadInitiate for data > 4 bytes");
|
||||
|
||||
byte cs = (byte)((byte)SdoCommand.DownloadInitiate | 0x03);
|
||||
int n = 4 - data.Length;
|
||||
cs |= (byte)((n << 2) & 0x0C);
|
||||
|
||||
uint dataValue = 0;
|
||||
for (int i = 0; i < data.Length; i++)
|
||||
dataValue |= (uint)(data[i] << (i * 8));
|
||||
|
||||
return new SdoRequest { CommandSpecifier = cs, Index = index, SubIndex = subIndex, Data = dataValue };
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create download initiate request for segmented transfer (data > 4 bytes)
|
||||
/// </summary>
|
||||
public static SdoRequest CreateDownloadInitiate(ushort index, byte subIndex, int dataLength)
|
||||
{
|
||||
if (dataLength <= 4)
|
||||
throw new ArgumentException("Use CreateDownload for data <= 4 bytes");
|
||||
|
||||
// Download Initiate: bit 0 = 0 (not expedited), bit 1 = 1 (size indicated), bit 2 = 0 (not complete)
|
||||
byte cs = (byte)SdoCommand.DownloadInitiate;
|
||||
cs |= 0x02; // Size indicated
|
||||
|
||||
// Data contains size (lower 3 bytes) and number of segments not used (upper byte = 0)
|
||||
uint sizeData = (uint)dataLength;
|
||||
|
||||
return new SdoRequest { CommandSpecifier = cs, Index = index, SubIndex = subIndex, Data = sizeData };
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create download segment request
|
||||
/// </summary>
|
||||
/// <param name="toggle">Toggle bit (0 or 1), alternates for each segment</param>
|
||||
/// <param name="isLastSegment">True if this is the last segment</param>
|
||||
/// <param name="segmentData">Data for this segment (up to 7 bytes)</param>
|
||||
public static SdoRequest CreateDownloadSegment(byte toggle, bool isLastSegment, byte[] segmentData)
|
||||
{
|
||||
if (segmentData.Length > 7)
|
||||
throw new ArgumentException("Segment data cannot exceed 7 bytes");
|
||||
|
||||
byte cs = (byte)SdoCommand.DownloadSegment;
|
||||
if (toggle != 0)
|
||||
cs |= 0x10; // Toggle bit
|
||||
|
||||
if (isLastSegment)
|
||||
cs |= 0x01; // Last segment bit
|
||||
|
||||
int n = 7 - segmentData.Length;
|
||||
cs |= (byte)((n << 1) & 0x0E);
|
||||
|
||||
uint dataValue = 0;
|
||||
for (int i = 0; i < segmentData.Length; i++)
|
||||
dataValue |= (uint)(segmentData[i] << (i * 8));
|
||||
|
||||
return new SdoRequest { CommandSpecifier = cs, Index = 0, SubIndex = 0, Data = dataValue };
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Create upload segment request
|
||||
/// </summary>
|
||||
/// <param name="toggle">Toggle bit (0 or 1), alternates for each segment</param>
|
||||
public static SdoRequest CreateUploadSegment(byte toggle)
|
||||
{
|
||||
byte cs = (byte)SdoCommand.UploadSegment;
|
||||
if (toggle != 0)
|
||||
cs |= 0x10; // Toggle bit
|
||||
|
||||
return new SdoRequest { CommandSpecifier = cs, Index = 0, SubIndex = 0, Data = 0 };
|
||||
}
|
||||
}
|
||||
|
||||
public readonly struct SdoResponse
|
||||
{
|
||||
public byte CommandSpecifier { get; init; }
|
||||
public ushort Index { get; init; }
|
||||
public byte SubIndex { get; init; }
|
||||
public uint Data { get; init; }
|
||||
|
||||
public bool IsAbort => (CommandSpecifier & 0xE0) == (byte)SdoCommand.Abort;
|
||||
public SdoAbortCode AbortCode => (SdoAbortCode)Data;
|
||||
|
||||
public static SdoResponse FromBytes(byte[] data)
|
||||
{
|
||||
if (data.Length < 8)
|
||||
throw new ArgumentException("SDO response must be 8 bytes");
|
||||
|
||||
return new SdoResponse
|
||||
{
|
||||
CommandSpecifier = data[0],
|
||||
Index = (ushort)(data[1] | (data[2] << 8)),
|
||||
SubIndex = data[3],
|
||||
Data = (uint)(data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24))
|
||||
};
|
||||
}
|
||||
|
||||
public byte[] GetDataBytes()
|
||||
{
|
||||
if ((CommandSpecifier & 0x02) != 0)
|
||||
{
|
||||
int n = (CommandSpecifier >> 2) & 0x03;
|
||||
int dataLength = 4 - n;
|
||||
|
||||
var bytes = new byte[dataLength];
|
||||
for (int i = 0; i < dataLength; i++)
|
||||
bytes[i] = (byte)((Data >> (i * 8)) & 0xFF);
|
||||
|
||||
return bytes;
|
||||
}
|
||||
|
||||
return BitConverter.GetBytes(Data);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if response is expedited transfer
|
||||
/// </summary>
|
||||
public bool IsExpedited => (CommandSpecifier & 0x02) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// Check if response indicates size (for segmented transfer)
|
||||
/// </summary>
|
||||
public bool IsSizeIndicated => (CommandSpecifier & 0x02) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// Get data size from upload initiate response (for segmented transfer)
|
||||
/// </summary>
|
||||
public int GetDataSize()
|
||||
{
|
||||
if (!IsSizeIndicated)
|
||||
return 4; // Expedited transfer
|
||||
|
||||
return (int)(Data & 0xFFFFFF); // Lower 3 bytes contain size
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get segment data from upload segment response
|
||||
/// </summary>
|
||||
public byte[] GetSegmentData()
|
||||
{
|
||||
int n = (CommandSpecifier >> 1) & 0x07;
|
||||
int dataLength = 7 - n;
|
||||
|
||||
var bytes = new byte[dataLength];
|
||||
for (int i = 0; i < dataLength; i++)
|
||||
bytes[i] = (byte)((Data >> (i * 8)) & 0xFF);
|
||||
|
||||
return bytes;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check if this is the last segment
|
||||
/// </summary>
|
||||
public bool IsLastSegment => (CommandSpecifier & 0x01) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// Get toggle bit from segment response
|
||||
/// </summary>
|
||||
public byte GetToggle() => (byte)((CommandSpecifier >> 4) & 0x01);
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
/// <summary>
|
||||
/// Validation result container
|
||||
/// </summary>
|
||||
public class ValidationResult
|
||||
{
|
||||
public bool IsValid { get; }
|
||||
public IReadOnlyList<string> Errors { get; }
|
||||
|
||||
public ValidationResult(bool isValid, IEnumerable<string> errors)
|
||||
{
|
||||
IsValid = isValid;
|
||||
Errors = errors.ToList().AsReadOnly();
|
||||
}
|
||||
|
||||
public static ValidationResult Success() => new(true, Array.Empty<string>());
|
||||
|
||||
public static ValidationResult Failure(params string[] errors) => new(false, errors);
|
||||
|
||||
public static ValidationResult Failure(IEnumerable<string> errors) => new(false, errors);
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
if (IsValid)
|
||||
return "Valid";
|
||||
|
||||
return $"Invalid: {string.Join(", ", Errors)}";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net10.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<FrameworkReference Include="Microsoft.AspNetCore.App" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="SocketCANSharp" Version="0.13.0" />
|
||||
</ItemGroup>
|
||||
|
||||
|
||||
</Project>
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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"
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user