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