QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update motor

Browse Source 
Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
QUYVN
2026-07-13 16:28:07 +07:00
parent bccfb156d7
commit dddf3fc594
9 changed files with 2142 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/PdoManager.cs
View File

@@ -360,6 +360,97 @@ public class PdoManager : IDisposable
return supportedIndices.Contains(mappingParamIndex);
}
/// <summary>
/// Poll 0x1000 (device type) cho tới khi drive trả lời SDO, tối đa maxWaitMs.
/// Dùng sau NMT Reset Communication để bước SDO kế tiếp không bị timeout oan.
/// </summary>
private async Task WaitUntilSdoRespondsAsync(ICanOpenDevice device, CancellationToken ct, int maxWaitMs = 3000)
{
const int retryDelayMs = 200;
int waited = 0;
while (waited < maxWaitMs)
{
try
{
await device.ReadUInt32Async(0x1000, 0x00, ct);
return; // Drive đã trả lời -> stack sẵn sàng
}
catch (OperationCanceledException) when (ct.IsCancellationRequested)
{
throw;
}
catch
{
await Task.Delay(retryDelayMs, ct);
waited += retryDelayMs;
}
}
_logger?.LogWarning(
"Device Node {NodeId} chưa trả lời SDO sau {MaxWaitMs}ms kể từ Reset Communication. Tiếp tục cấu hình PDO...",
device.NodeId, maxWaitMs);
}
/// <summary>
/// Đọc ngược cấu hình TPDO từ drive để biết giá trị nào THỰC SỰ được chấp nhận.
/// Nhiều firmware im lặng bỏ qua hoặc ép lại COB-ID / transmission type / event timer,
/// khiến TPDO không bao giờ phát dù mọi lệnh ghi đều báo thành công.
/// </summary>
private async Task LogActualTpdoConfigAsync(ICanOpenDevice device, byte pdoNumber, CancellationToken ct)
{
ushort commParamIndex = (ushort)(0x1800 + (pdoNumber - 1));
ushort mappingParamIndex = (ushort)(0x1A00 + (pdoNumber - 1));
try
{
uint cobId = await device.ReadUInt32Async(commParamIndex, 0x01, ct);
byte transmissionType = await device.ReadUInt8Async(commParamIndex, 0x02, ct);
byte mappingCount = await device.ReadUInt8Async(mappingParamIndex, 0x00, ct);
ushort eventTimer = 0;
try
{
eventTimer = await device.ReadUInt16Async(commParamIndex, 0x05, ct);
}
catch
{
// Firmware không có event timer -> giữ 0
}
bool enabled = (cobId & 0x80000000) == 0;
_logger?.LogInformation(
"TPDO{PDONumber} readback Node {NodeId}: COB-ID=0x{CobId:X3} ({State}), TransmissionType=0x{Type:X2}, EventTimer={EventTimer}ms, MappingCount={Count}",
pdoNumber, device.NodeId, cobId & 0x7FF, enabled ? "ENABLED" : "DISABLED",
transmissionType, eventTimer, mappingCount);
if (!enabled)
{
_logger?.LogWarning("TPDO{PDONumber} Node {NodeId}: drive vẫn để bit 31 = 1 (PDO bị DISABLE) -> sẽ không phát frame nào.",
pdoNumber, device.NodeId);
}
if (mappingCount == 0)
{
_logger?.LogWarning("TPDO{PDONumber} Node {NodeId}: MappingCount = 0 -> drive không giữ mapping đã ghi, TPDO sẽ không có dữ liệu.",
pdoNumber, device.NodeId);
}
if (transmissionType is not (0xFE or 0xFF) && eventTimer == 0)
{
_logger?.LogWarning(
"TPDO{PDONumber} Node {NodeId}: TransmissionType=0x{Type:X2} là kiểu đồng bộ (theo SYNC) và EventTimer=0. " +
"Không có SYNC producer thì TPDO sẽ không bao giờ phát.",
pdoNumber, device.NodeId, transmissionType);
}
}
catch (Exception ex)
{
_logger?.LogWarning(ex, "Không đọc ngược được cấu hình TPDO{PDONumber} trên Node {NodeId}", pdoNumber, device.NodeId);
}
}
/// <summary>
/// Stop và clear tất cả TPDOs trên device
/// </summary>
@@ -426,7 +517,12 @@ public class PdoManager : IDisposable
try
{
await device.ResetCommunicationAsync(ct);
await Task.Delay(100, ct); // Wait for device to reset
await Task.Delay(500, ct);
// Chờ drive dựng xong CANopen stack: poll 0x1000 (device type) cho tới khi
// nó trả lời. Delay cố định không đủ tin cậy — SDO đầu tiên sau reset hay bị
// timeout, gây warning "Failed to stop/clear TPDO1" ở bước sau.
await WaitUntilSdoRespondsAsync(device, ct);
}
catch (Exception ex)
{
@@ -534,6 +630,9 @@ public class PdoManager : IDisposable
await WritePdoConfigurationToDeviceAsync(device, true, kvp.Key, ct);
_configuredTpdoNumbers.Add(kvp.Key);
// Đọc ngược để xác nhận drive thực sự chấp nhận cấu hình (không chỉ ACK lệnh ghi)
await LogActualTpdoConfigAsync(device, kvp.Key, ct);
}
catch (Exception ex)
{

128
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SdoClient.cs
View File

@@ -12,6 +12,9 @@ public class SdoClient : IDisposable
private readonly ICanBus _canBus;
private readonly byte _nodeId;
private readonly ConcurrentDictionary<string, TaskCompletionSource<SdoResponse>> _pendingRequests;
// CANopen chỉ cho phép 1 giao dịch SDO tại một thời điểm trên mỗi node.
// Hai facade trái/phải của ZLAC8015D dùng chung SdoClient nên phải xếp hàng ở đây.
private readonly SemaphoreSlim _transactionLock = new(1, 1);
private readonly TimeSpan _timeout;
private readonly ILogger<SdoClient>? _logger;
private bool _disposed;
@@ -31,26 +34,37 @@ public class SdoClient : IDisposable
{
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
var request = SdoRequest.CreateUpload(index, subIndex);
var response = await SendRequestAsync(request, cancellationToken);
if (response.IsAbort)
await _transactionLock.WaitAsync(cancellationToken);
try
{
var abortCode = (SdoAbortCode)(uint)response.AbortCode;
var message = $"SDO Upload failed: {abortCode.GetDescription()}";
throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message);
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);
}
}
// Check if expedited or segmented
if (response.IsExpedited)
finally
{
return response.GetDataBytes();
}
else
{
// Segmented transfer
return await UploadSegmentedAsync(index, subIndex, response, cancellationToken);
_transactionLock.Release();
}
}
@@ -126,27 +140,81 @@ public class SdoClient : IDisposable
{
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
if (data.Length <= 4)
await _transactionLock.WaitAsync(cancellationToken);
try
{
// Expedited transfer (≤4 bytes)
var request = SdoRequest.CreateDownload(index, subIndex, data);
var response = await SendRequestAsync(request, cancellationToken);
if (response.IsAbort)
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
if (data.Length <= 4)
{
var abortCode = (SdoAbortCode)(uint)response.AbortCode;
var message = $"SDO Download failed: {abortCode.GetDescription()}";
throw new SdoException(_nodeId, index, subIndex, (uint)response.AbortCode, message);
// 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);
}
}
else
finally
{
// Segmented transfer (>4 bytes)
await DownloadSegmentedAsync(index, subIndex, data, cancellationToken);
_transactionLock.Release();
}
}
/// <summary>
/// Ghi nhiều object liên tiếp trong MỘT lần giữ transaction lock.
/// Dùng khi các lệnh phải đi liền nhau không bị giao dịch SDO khác chen giữa,
/// ví dụ lệnh vận tốc hai bánh của ZLAC8015D khi không có RPDO: nếu ghi bằng hai
/// DownloadAsync riêng, poll feedback có thể chiếm kênh giữa hai lệnh làm bánh sau
/// nhận lệnh muộn hơn hẳn. Chỉ hỗ trợ expedited transfer (data ≤ 4 byte mỗi mục).
/// </summary>
public async Task DownloadBatchAsync(IReadOnlyList<(ushort Index, byte SubIndex, byte[] Data)> items, CancellationToken cancellationToken = default)
{
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
foreach (var item in items)
{
if (item.Data.Length > 4)
throw new NotSupportedException("DownloadBatchAsync chỉ hỗ trợ expedited transfer (≤4 byte mỗi mục)");
}
await _transactionLock.WaitAsync(cancellationToken);
try
{
if (_disposed)
throw new ObjectDisposedException(nameof(SdoClient));
foreach (var (index, subIndex, data) in items)
{
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);
}
}
}
finally
{
_transactionLock.Release();
}
}
/// <summary>
/// Download data using segmented transfer (for data > 4 bytes)
/// </summary>