Initial commit
This commit is contained in:
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user