409 lines
16 KiB
C#
409 lines
16 KiB
C#
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;
|
|
|
|
/// <summary>
|
|
/// Lock cho trình tự cấu hình cấp node (PDO config, NMT state, tham số chung).
|
|
/// Nhiều facade dùng chung một node vật lý (vd. 2 trục của ZLAC8015D) phải giữ lock này
|
|
/// trong suốt trình tự connect để không xen kẽ NMT/PDO của nhau.
|
|
/// </summary>
|
|
public SemaphoreSlim ConfigurationLock { get; } = new(1, 1);
|
|
|
|
// 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);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ghi nhiều giá trị int32 trong một lần giữ kênh SDO liên tục — không giao dịch nào
|
|
/// khác chen được vào giữa các lệnh. Dùng cho lệnh cần đi liền nhau (vd vận tốc 2 bánh).
|
|
/// </summary>
|
|
public async Task WriteInt32BatchAsync(IReadOnlyList<(ushort Index, byte SubIndex, int Value)> items, CancellationToken cancellationToken = default)
|
|
{
|
|
var batch = new (ushort, byte, byte[])[items.Count];
|
|
for (int i = 0; i < items.Count; i++)
|
|
batch[i] = (items[i].Index, items[i].SubIndex, BitConverter.GetBytes(items[i].Value));
|
|
|
|
await _sdoClient.DownloadBatchAsync(batch, 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);
|
|
}
|
|
}
|