save
This commit is contained in:
456
SickBlazorApp/Services/Windows/PcanCanService.cs
Normal file
456
SickBlazorApp/Services/Windows/PcanCanService.cs
Normal file
@@ -0,0 +1,456 @@
|
||||
using Microsoft.Extensions.Options;
|
||||
using Peak.Can.Basic.BackwardCompatibility;
|
||||
using SickBlazorApp.Models;
|
||||
using SickBlazorApp.Options;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Reflection;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace SickBlazorApp.Services.Windows;
|
||||
|
||||
public sealed class PcanCanService : ICanBusService
|
||||
{
|
||||
private readonly CanBusOptions _options;
|
||||
private readonly EdsParser _edsParser; // Parser EDS mới
|
||||
private CancellationTokenSource? _cts;
|
||||
private bool _initialized;
|
||||
|
||||
public event EventHandler<PositionPdo>? PositionReceived;
|
||||
private DateTime _lastHeartbeat = DateTime.MinValue;
|
||||
private readonly TimeSpan _heartbeatTimeout = TimeSpan.FromSeconds(2);
|
||||
public event EventHandler<CanNodeState>? NodeStateChanged;
|
||||
private DateTime _lastSeen = DateTime.MinValue;
|
||||
private CanNodeState _currentState = CanNodeState.Unknown;
|
||||
public event EventHandler<CanFrame>? FrameReceived;
|
||||
private int _currentBitrate;
|
||||
public int CurrentBitrate { get; private set; }
|
||||
private bool _waitingStopAck = false;
|
||||
private bool _autoResumeEnabled = true;
|
||||
private DateTime _lastBootup = DateTime.MinValue;
|
||||
private byte _currentNodeId;
|
||||
public byte CurrentNodeId => _currentNodeId;
|
||||
public byte GetCurrentNodeId() => _currentNodeId;
|
||||
public event EventHandler<byte>? NodeIdChanged;
|
||||
|
||||
private Dictionary<byte, int> _baudMap = new(); // Map baudrate động từ EDS
|
||||
|
||||
public PcanCanService(IOptions<CanBusOptions> options)
|
||||
{
|
||||
_options = options.Value;
|
||||
|
||||
_edsParser = new EdsParser("SickBlazorApp.eds.AHM36_I_CO.eds"); // hard-code tên resource
|
||||
|
||||
_options.PositionBaseCobId = _edsParser.GetTpdo1BaseCobId(); // vẫn động
|
||||
_options.PositionScale = _edsParser.GetPositionScale(); // động từ 230 mm/rev
|
||||
|
||||
if (!_edsParser.IsPositionMappedToTpdo1())
|
||||
throw new InvalidOperationException("Position mapping sai");
|
||||
|
||||
_baudMap = _edsParser.GetBaudrateMap();
|
||||
CurrentBitrate = _currentBitrate;
|
||||
}
|
||||
|
||||
// ================= INIT =================
|
||||
public Task InitAsync()
|
||||
{
|
||||
ushort channel = ParseChannel(_options.Channel);
|
||||
|
||||
if (_initialized)
|
||||
return Task.CompletedTask;
|
||||
|
||||
var baud = MapBaudrate(_currentBitrate);
|
||||
var status = PCANBasic.Initialize(channel, baud);
|
||||
|
||||
if (status != TPCANStatus.PCAN_ERROR_OK)
|
||||
throw new InvalidOperationException($"PCAN init failed: {status}");
|
||||
|
||||
_initialized = true;
|
||||
CurrentBitrate = _currentBitrate;
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
// ================= START READ =================
|
||||
public void Start()
|
||||
{
|
||||
if (_cts != null)
|
||||
return;
|
||||
|
||||
_cts = new CancellationTokenSource();
|
||||
|
||||
ushort channel = ParseChannel(_options.Channel);
|
||||
uint pdoCobId = GetPositionCobId();
|
||||
uint heartbeatCobId = 0x700u + _currentNodeId;
|
||||
double scale = _options.PositionScale;
|
||||
|
||||
Task.Run(() =>
|
||||
{
|
||||
while (!_cts!.IsCancellationRequested)
|
||||
{
|
||||
TPCANMsg msg;
|
||||
TPCANTimestamp ts;
|
||||
|
||||
var result = PCANBasic.Read(channel, out msg, out ts);
|
||||
|
||||
// ================= BOOT-UP DETECT =================
|
||||
if (msg.ID >= 0x701 && msg.ID <= 0x77F && msg.LEN >= 1 && msg.DATA[0] == 0x00)
|
||||
{
|
||||
byte detectedNodeId = (byte)(msg.ID - 0x700);
|
||||
if (_currentNodeId != detectedNodeId)
|
||||
{
|
||||
_currentNodeId = detectedNodeId;
|
||||
NodeIdChanged?.Invoke(this, detectedNodeId);
|
||||
}
|
||||
}
|
||||
|
||||
if (result != TPCANStatus.PCAN_ERROR_OK)
|
||||
{
|
||||
CheckHeartbeatTimeout();
|
||||
Thread.Sleep(5);
|
||||
continue;
|
||||
}
|
||||
|
||||
string dataHex = string.Join(" ", msg.DATA.Take(msg.LEN).Select(b => b.ToString("X2")));
|
||||
FrameReceived?.Invoke(this, new CanFrame
|
||||
{
|
||||
CobId = msg.ID,
|
||||
Length = msg.LEN,
|
||||
DataHex = dataHex,
|
||||
Timestamp = DateTime.Now
|
||||
});
|
||||
|
||||
if (!msg.MSGTYPE.HasFlag(TPCANMessageType.PCAN_MESSAGE_STANDARD))
|
||||
continue;
|
||||
|
||||
// ================= HEARTBEAT =================
|
||||
if (msg.ID >= 0x701 && msg.ID <= 0x77F && msg.LEN >= 1)
|
||||
{
|
||||
_lastHeartbeat = DateTime.Now;
|
||||
_lastSeen = DateTime.Now;
|
||||
|
||||
byte nodeIdFromHb = (byte)(msg.ID - 0x700);
|
||||
byte hbValue = msg.DATA[0];
|
||||
|
||||
if (_currentNodeId != nodeIdFromHb)
|
||||
{
|
||||
_currentNodeId = nodeIdFromHb;
|
||||
NodeIdChanged?.Invoke(this, nodeIdFromHb);
|
||||
}
|
||||
|
||||
var hbState = hbValue switch
|
||||
{
|
||||
0x00 => CanNodeState.Bootup,
|
||||
0x04 => CanNodeState.Stopped,
|
||||
0x05 => CanNodeState.Operational,
|
||||
0x7F => CanNodeState.PreOperational,
|
||||
_ => CanNodeState.Unknown
|
||||
};
|
||||
|
||||
UpdateState(hbState);
|
||||
|
||||
if (_autoResumeEnabled && !_waitingStopAck && hbState == CanNodeState.Bootup)
|
||||
{
|
||||
if (DateTime.Now - _lastBootup > TimeSpan.FromSeconds(1))
|
||||
{
|
||||
_lastBootup = DateTime.Now;
|
||||
Task.Run(async () =>
|
||||
{
|
||||
await Task.Delay(500);
|
||||
SendNmtStart(_currentNodeId);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
if (_waitingStopAck && hbState == CanNodeState.Stopped)
|
||||
{
|
||||
_waitingStopAck = false;
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
// ================= POSITION PDO =================
|
||||
if (msg.ID < 0x180 || msg.ID > 0x1FF || msg.LEN < 6)
|
||||
continue;
|
||||
|
||||
_lastSeen = DateTime.Now;
|
||||
|
||||
uint raw = (uint)(
|
||||
msg.DATA[0] |
|
||||
(msg.DATA[1] << 8) |
|
||||
(msg.DATA[2] << 16) |
|
||||
(msg.DATA[3] << 24)
|
||||
);
|
||||
|
||||
double positionMm = raw * scale;
|
||||
positionMm = Math.Min(positionMm, 3000.0);
|
||||
positionMm = Math.Max(positionMm, 0.0);
|
||||
PositionReceived?.Invoke(this, new PositionPdo
|
||||
{
|
||||
CobId = msg.ID,
|
||||
Length = msg.LEN,
|
||||
DataHex = dataHex,
|
||||
RawValue = raw,
|
||||
PositionMm = positionMm,
|
||||
Timestamp = DateTime.Now
|
||||
});
|
||||
FrameReceived?.Invoke(this, new CanFrame
|
||||
{
|
||||
CobId = msg.ID,
|
||||
Length = msg.LEN,
|
||||
DataHex = dataHex,
|
||||
Timestamp = DateTime.Now,
|
||||
PositionMm = positionMm,
|
||||
Description = "TPDO1 Position"
|
||||
});
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// ================= STOP =================
|
||||
public void Stop()
|
||||
{
|
||||
if (_cts == null)
|
||||
return;
|
||||
|
||||
_waitingStopAck = true;
|
||||
SendNmtStop(_currentNodeId);
|
||||
|
||||
_cts.Cancel();
|
||||
_cts = null;
|
||||
|
||||
UpdateState(CanNodeState.Stopped);
|
||||
}
|
||||
|
||||
public void Dispose() => Stop();
|
||||
|
||||
// ================= NMT =================
|
||||
public void SendNmtStart(byte nodeId)
|
||||
{
|
||||
ushort channel = ParseChannel(_options.Channel);
|
||||
var msg = new TPCANMsg
|
||||
{
|
||||
ID = 0x000,
|
||||
LEN = 2,
|
||||
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
|
||||
DATA = new byte[8]
|
||||
};
|
||||
msg.DATA[0] = 0x01; // NMT Start
|
||||
msg.DATA[1] = nodeId;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
PCANBasic.Write(channel, ref msg);
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
}
|
||||
|
||||
public void SendNmtStop(byte nodeId)
|
||||
{
|
||||
ushort channel = ParseChannel(_options.Channel);
|
||||
var msg = new TPCANMsg
|
||||
{
|
||||
ID = 0x000,
|
||||
LEN = 2,
|
||||
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
|
||||
DATA = new byte[8]
|
||||
};
|
||||
msg.DATA[0] = 0x02; // NMT Stop
|
||||
msg.DATA[1] = nodeId;
|
||||
PCANBasic.Write(channel, ref msg);
|
||||
}
|
||||
|
||||
public void SendNmtReset(byte nodeId)
|
||||
{
|
||||
ushort channel = ParseChannel(_options.Channel);
|
||||
var msg = new TPCANMsg
|
||||
{
|
||||
ID = 0x000,
|
||||
LEN = 2,
|
||||
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
|
||||
DATA = new byte[8]
|
||||
};
|
||||
msg.DATA[0] = 0x81; // Reset node
|
||||
msg.DATA[1] = nodeId;
|
||||
PCANBasic.Write(channel, ref msg);
|
||||
}
|
||||
|
||||
// ================= HELPERS =================
|
||||
private static ushort ParseChannel(string ch) => ch switch
|
||||
{
|
||||
"PCAN_USBBUS1" => 0x51,
|
||||
"PCAN_USBBUS2" => 0x52,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(ch))
|
||||
};
|
||||
|
||||
private static TPCANBaudrate MapBaudrate(int b) => b switch
|
||||
{
|
||||
1000000 => TPCANBaudrate.PCAN_BAUD_1M,
|
||||
800000 => TPCANBaudrate.PCAN_BAUD_800K,
|
||||
500000 => TPCANBaudrate.PCAN_BAUD_500K,
|
||||
250000 => TPCANBaudrate.PCAN_BAUD_250K,
|
||||
125000 => TPCANBaudrate.PCAN_BAUD_125K,
|
||||
100000 => TPCANBaudrate.PCAN_BAUD_100K,
|
||||
50000 => TPCANBaudrate.PCAN_BAUD_50K,
|
||||
20000 => TPCANBaudrate.PCAN_BAUD_20K,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(b))
|
||||
};
|
||||
|
||||
private uint GetPositionCobId()
|
||||
{
|
||||
return _options.PositionBaseCobId + _currentNodeId;
|
||||
}
|
||||
|
||||
private void CheckHeartbeatTimeout()
|
||||
{
|
||||
if (_currentState == CanNodeState.PreOperational || _currentState == CanNodeState.Stopped)
|
||||
return;
|
||||
|
||||
if (_lastHeartbeat == DateTime.MinValue)
|
||||
return;
|
||||
|
||||
if (DateTime.Now - _lastHeartbeat > _heartbeatTimeout)
|
||||
{
|
||||
UpdateState(CanNodeState.Timeout);
|
||||
}
|
||||
}
|
||||
|
||||
private void UpdateState(CanNodeState newState)
|
||||
{
|
||||
if (_currentState == newState)
|
||||
return;
|
||||
|
||||
_currentState = newState;
|
||||
NodeStateChanged?.Invoke(this, newState);
|
||||
}
|
||||
|
||||
public void ChangeBitrate(int bitrate)
|
||||
{
|
||||
if (_currentBitrate == bitrate)
|
||||
return;
|
||||
|
||||
ushort channel = ParseChannel(_options.Channel);
|
||||
|
||||
Stop();
|
||||
|
||||
if (_initialized)
|
||||
{
|
||||
PCANBasic.Uninitialize(channel);
|
||||
_initialized = false;
|
||||
}
|
||||
|
||||
_currentBitrate = bitrate;
|
||||
}
|
||||
|
||||
private void SendRaw(uint cobId, byte[] data)
|
||||
{
|
||||
ushort channel = ParseChannel(_options.Channel);
|
||||
var msg = new TPCANMsg
|
||||
{
|
||||
ID = cobId,
|
||||
LEN = (byte)data.Length,
|
||||
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
|
||||
DATA = new byte[8]
|
||||
};
|
||||
Array.Copy(data, msg.DATA, data.Length);
|
||||
PCANBasic.Write(channel, ref msg);
|
||||
}
|
||||
|
||||
private uint GetAccessCode() => _edsParser.GetAccessCode(); // 0x98127634
|
||||
|
||||
private byte GetBitrateIndex(int bitrate)
|
||||
{
|
||||
foreach (var kv in _baudMap)
|
||||
{
|
||||
if (kv.Value == bitrate) return kv.Key;
|
||||
}
|
||||
throw new ArgumentOutOfRangeException(nameof(bitrate), $"Bitrate {bitrate} not supported in EDS");
|
||||
}
|
||||
|
||||
public async Task ApplyBitrateAsync(byte nodeId, int bitrate)
|
||||
{
|
||||
// Pre-Operational
|
||||
SendRaw(0x000, new byte[] { 0x80, nodeId });
|
||||
await Task.Delay(100);
|
||||
|
||||
// Unlock access code (đọc động từ EDS/manual)
|
||||
uint accessCode = GetAccessCode();
|
||||
SendRaw(
|
||||
(uint)(0x600 + nodeId),
|
||||
new byte[] { 0x23, 0x09, 0x20, 0x01,
|
||||
(byte)(accessCode & 0xFF),
|
||||
(byte)((accessCode >> 8) & 0xFF),
|
||||
(byte)((accessCode >> 16) & 0xFF),
|
||||
(byte)((accessCode >> 24) & 0xFF) }
|
||||
);
|
||||
await Task.Delay(100);
|
||||
|
||||
// Set bitrate (index từ EDS map)
|
||||
byte bitrateValue = GetBitrateIndex(bitrate);
|
||||
SendRaw(
|
||||
(uint)(0x600 + nodeId),
|
||||
new byte[] { 0x2F, 0x09, 0x20, 0x03, bitrateValue, 0x00, 0x00, 0x00 }
|
||||
);
|
||||
await Task.Delay(100);
|
||||
|
||||
// Save EEPROM
|
||||
SendRaw(
|
||||
(uint)(0x600 + nodeId),
|
||||
new byte[] { 0x23, 0x10, 0x10, 0x01, 0x73, 0x61, 0x76, 0x65 }
|
||||
);
|
||||
await Task.Delay(100);
|
||||
|
||||
// Reset node
|
||||
SendRaw(0x000u, new byte[] { 0x81, nodeId });
|
||||
|
||||
// Update runtime
|
||||
await Task.Delay(1200);
|
||||
ChangeBitrate(bitrate);
|
||||
CurrentBitrate = bitrate;
|
||||
}
|
||||
|
||||
public async Task ApplyNodeIdAsync(byte oldNodeId, byte newNodeId)
|
||||
{
|
||||
if (newNodeId < 1 || newNodeId > 127)
|
||||
throw new ArgumentOutOfRangeException(nameof(newNodeId));
|
||||
|
||||
// Pre-Operational
|
||||
SendRaw(0x000, new byte[] { 0x80, oldNodeId });
|
||||
await Task.Delay(100);
|
||||
|
||||
// Unlock access code (động từ EDS)
|
||||
uint accessCode = GetAccessCode();
|
||||
SendRaw(
|
||||
(uint)(0x600 + oldNodeId),
|
||||
new byte[] { 0x23, 0x09, 0x20, 0x01,
|
||||
(byte)(accessCode & 0xFF),
|
||||
(byte)((accessCode >> 8) & 0xFF),
|
||||
(byte)((accessCode >> 16) & 0xFF),
|
||||
(byte)((accessCode >> 24) & 0xFF) }
|
||||
);
|
||||
await Task.Delay(100);
|
||||
|
||||
// Set Node ID
|
||||
SendRaw(
|
||||
(uint)(0x600 + oldNodeId),
|
||||
new byte[] { 0x2F, 0x09, 0x20, 0x02, newNodeId, 0x00, 0x00, 0x00 }
|
||||
);
|
||||
await Task.Delay(100);
|
||||
|
||||
// Save EEPROM
|
||||
SendRaw(
|
||||
(uint)(0x600 + oldNodeId),
|
||||
new byte[] { 0x23, 0x10, 0x10, 0x01, 0x73, 0x61, 0x76, 0x65 }
|
||||
);
|
||||
await Task.Delay(100);
|
||||
|
||||
// Reset node
|
||||
SendRaw(0x000, new byte[] { 0x81, oldNodeId });
|
||||
|
||||
// Thông báo UI
|
||||
_currentNodeId = newNodeId;
|
||||
NodeIdChanged?.Invoke(this, newNodeId);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user