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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user