335 lines
14 KiB
C#
335 lines
14 KiB
C#
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);
|
|
}
|
|
}
|
|
|
|
|