513 lines
21 KiB
C#
513 lines
21 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Runtime.InteropServices;
|
||
using System.Text;
|
||
using System.Threading;
|
||
using HC_APTBS.Infrastructure.Logging;
|
||
using HC_APTBS.Models;
|
||
using HC_APTBS.Services;
|
||
using Peak.Can.Basic;
|
||
using TPCANHandle = System.UInt16;
|
||
|
||
namespace HC_APTBS.Infrastructure.Pcan
|
||
{
|
||
/// <summary>
|
||
/// Wraps the PCAN-Basic native API behind <see cref="ICanService"/>.
|
||
/// The raw P/Invoke declarations live in <see cref="PcanBasic"/> (vendor file,
|
||
/// unchanged). This class handles lifecycle, threading, OEM legitimation,
|
||
/// message dispatch, and parameter decoding.
|
||
/// </summary>
|
||
public sealed class PcanAdapter : ICanService, IDisposable
|
||
{
|
||
// ── Constants ───────────────────────────────────────────────────────────
|
||
|
||
/// <summary>PEAK-System distributor code embedded into the OEM legitimation token.</summary>
|
||
private const ulong OemDistributorCode = 20120378UL;
|
||
|
||
/// <summary>PEAK-System OEM-ID for the PCAN-USB device.</summary>
|
||
private const ulong OemId = 21200UL;
|
||
|
||
private const string LogId = "PcanAdapter";
|
||
|
||
// ── State ────────────────────────────────────────────────────────────────
|
||
|
||
private readonly TPCANHandle _channel;
|
||
private TPCANBaudrate _baudrate;
|
||
private readonly IAppLogger _log;
|
||
|
||
/// <summary>
|
||
/// Live parameter map: CAN message ID → list of parameters decoded from that frame.
|
||
/// All reads/writes on this dictionary happen on the CAN read thread, except for
|
||
/// <see cref="AddParameters"/> / <see cref="RemoveParameters"/> which are guarded
|
||
/// by <see cref="_mapLock"/>.
|
||
/// </summary>
|
||
private Dictionary<uint, List<CanBusParameter>> _parameterMap = new();
|
||
private readonly object _mapLock = new();
|
||
|
||
private Thread? _readThread;
|
||
private AutoResetEvent? _receiveEvent;
|
||
private volatile bool _stopRead = true;
|
||
|
||
// ── ICanService ──────────────────────────────────────────────────────────
|
||
|
||
/// <inheritdoc/>
|
||
public event Action<string, bool>? StatusChanged;
|
||
|
||
/// <inheritdoc/>
|
||
public TPCANStatus CurrentStatus { get; private set; } = TPCANStatus.PCAN_ERROR_OK;
|
||
|
||
/// <inheritdoc/>
|
||
public bool IsConnected => !_stopRead;
|
||
|
||
// ── Construction ─────────────────────────────────────────────────────────
|
||
|
||
/// <summary>
|
||
/// Creates a new adapter for the given PCAN channel and baudrate.
|
||
/// Call <see cref="Connect"/> to open the hardware.
|
||
/// </summary>
|
||
/// <param name="channel">PCAN channel handle, e.g. <c>PCANBasic.PCAN_USBBUS1</c>.</param>
|
||
/// <param name="baudrate">CAN baudrate, e.g. <c>TPCANBaudrate.PCAN_BAUD_500K</c>.</param>
|
||
/// <param name="logger">Application logger.</param>
|
||
public PcanAdapter(TPCANHandle channel, TPCANBaudrate baudrate, IAppLogger logger)
|
||
{
|
||
_channel = channel;
|
||
_baudrate = baudrate;
|
||
_log = logger;
|
||
}
|
||
|
||
// ── ICanService: lifecycle ────────────────────────────────────────────────
|
||
|
||
/// <inheritdoc/>
|
||
public bool Connect()
|
||
{
|
||
try
|
||
{
|
||
// If the channel is already open, reuse it; otherwise initialize.
|
||
CurrentStatus = PCANBasic.GetStatus(_channel);
|
||
|
||
if (CurrentStatus == TPCANStatus.PCAN_ERROR_INITIALIZE ||
|
||
CurrentStatus == TPCANStatus.PCAN_ERROR_INITIALIZE2)
|
||
{
|
||
CurrentStatus = PCANBasic.Initialize(_channel, _baudrate, (TPCANType)0, 0, 0);
|
||
EmitStatusChanged(CurrentStatus);
|
||
}
|
||
|
||
if (CurrentStatus == TPCANStatus.PCAN_ERROR_NETINUSE)
|
||
{
|
||
_log.Error(LogId, "CAN channel is already in use by another application.");
|
||
EmitStatusChanged(CurrentStatus);
|
||
return false;
|
||
}
|
||
|
||
if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
|
||
{
|
||
LogPcanError("Connect: initialization failed");
|
||
EmitStatusChanged(CurrentStatus);
|
||
return false;
|
||
}
|
||
|
||
// OEM legitimation: token = (DistributorCode << 32) | OemId
|
||
// This authenticates our application with the PCAN hardware.
|
||
ulong token = (OemDistributorCode << 32) | OemId;
|
||
CurrentStatus = PCANBasic.SetValue(
|
||
_channel, TPCANParameter.PCAN_CHANNEL_LEGITIMATION, ref token, 8);
|
||
|
||
if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
|
||
{
|
||
_log.Error(LogId, "OEM legitimation failed — adapter has no OEM token.");
|
||
PCANBasic.Uninitialize(_channel);
|
||
EmitStatusChanged(CurrentStatus);
|
||
return false;
|
||
}
|
||
|
||
StartReadThread();
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_log.Error(LogId, $"Connect exception: {ex}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc/>
|
||
public void Disconnect()
|
||
{
|
||
_stopRead = true;
|
||
PCANBasic.Uninitialize(_channel);
|
||
}
|
||
|
||
/// <inheritdoc/>
|
||
public void SwitchBaudrate(TPCANBaudrate newBaudrate, uint baudrateMessageId)
|
||
{
|
||
// Send the baudrate-change command to the bench firmware before switching.
|
||
SendMessageById(baudrateMessageId);
|
||
|
||
_stopRead = true;
|
||
Thread.Sleep(250);
|
||
|
||
_baudrate = newBaudrate;
|
||
PCANBasic.Uninitialize(_channel);
|
||
PCANBasic.Reset(_channel);
|
||
Connect();
|
||
}
|
||
|
||
// ── ICanService: parameter map ────────────────────────────────────────────
|
||
|
||
/// <inheritdoc/>
|
||
public void SetParameters(Dictionary<uint, List<CanBusParameter>> parameters)
|
||
{
|
||
lock (_mapLock)
|
||
{
|
||
_parameterMap = parameters;
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc/>
|
||
public void AddParameters(Dictionary<uint, List<CanBusParameter>> parameters)
|
||
{
|
||
lock (_mapLock)
|
||
{
|
||
foreach (var kv in parameters)
|
||
{
|
||
if (!_parameterMap.ContainsKey(kv.Key))
|
||
_parameterMap.Add(kv.Key, kv.Value);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc/>
|
||
public void RemoveParameters(Dictionary<uint, List<CanBusParameter>> parameters)
|
||
{
|
||
lock (_mapLock)
|
||
{
|
||
foreach (var key in parameters.Keys)
|
||
_parameterMap.Remove(key);
|
||
}
|
||
}
|
||
|
||
// ── ICanService: transmit ─────────────────────────────────────────────────
|
||
|
||
/// <inheritdoc/>
|
||
public void SendMessageById(uint messageId)
|
||
{
|
||
Dictionary<uint, List<CanBusParameter>> snapshot;
|
||
lock (_mapLock) { snapshot = _parameterMap; }
|
||
|
||
if (!snapshot.TryGetValue(messageId, out var parameters) || parameters.Count == 0)
|
||
return;
|
||
|
||
var msg = new TPCANMsg
|
||
{
|
||
ID = messageId,
|
||
LEN = 8,
|
||
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
|
||
DATA = new byte[8]
|
||
};
|
||
|
||
// Write only transmit (non-receive) parameters into their assigned byte positions.
|
||
foreach (var param in parameters)
|
||
{
|
||
if (param.IsReceive) continue;
|
||
uint raw = (uint)param.GetTransmitValue();
|
||
msg.DATA[param.ByteH] = (byte)((raw & 0xFF00) >> 8);
|
||
msg.DATA[param.ByteL] = (byte)(raw & 0x00FF);
|
||
}
|
||
|
||
CurrentStatus = PCANBasic.Write(_channel, ref msg);
|
||
if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
|
||
_log.Warning(LogId, $"SendMessageById({messageId:X}): {CurrentStatus}");
|
||
}
|
||
|
||
/// <inheritdoc/>
|
||
public void SendRawMessage(uint messageId, byte[] data)
|
||
{
|
||
if (data.Length != 8)
|
||
throw new ArgumentException("CAN standard frame payload must be exactly 8 bytes.", nameof(data));
|
||
|
||
var msg = new TPCANMsg
|
||
{
|
||
ID = messageId,
|
||
LEN = 8,
|
||
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
|
||
DATA = data
|
||
};
|
||
|
||
CurrentStatus = PCANBasic.Write(_channel, ref msg);
|
||
if (CurrentStatus != TPCANStatus.PCAN_ERROR_OK)
|
||
_log.Warning(LogId, $"SendRawMessage({messageId:X}): {CurrentStatus}");
|
||
}
|
||
|
||
// ── Read thread ───────────────────────────────────────────────────────────
|
||
|
||
private void StartReadThread()
|
||
{
|
||
_stopRead = false;
|
||
_receiveEvent = new AutoResetEvent(false);
|
||
|
||
_readThread = new Thread(ReadThreadEntry) { IsBackground = true, Name = "CAN-Read" };
|
||
_readThread.Start();
|
||
}
|
||
|
||
private void ReadThreadEntry()
|
||
{
|
||
// Bind the AutoResetEvent handle to the PCAN receive event so the driver
|
||
// signals us whenever a new frame arrives in the hardware FIFO.
|
||
uint eventHandle = Convert.ToUInt32(_receiveEvent!.SafeWaitHandle.DangerousGetHandle().ToInt32());
|
||
var result = PCANBasic.SetValue(
|
||
_channel, TPCANParameter.PCAN_RECEIVE_EVENT, ref eventHandle, sizeof(uint));
|
||
|
||
if (result != TPCANStatus.PCAN_ERROR_OK)
|
||
{
|
||
_log.Error(LogId, $"Failed to bind receive event: {result}");
|
||
return;
|
||
}
|
||
|
||
DrainMessageQueue();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Continuously drains the hardware receive FIFO until stopped.
|
||
/// Runs on the dedicated CAN read background thread.
|
||
/// </summary>
|
||
private void DrainMessageQueue()
|
||
{
|
||
while (!_stopRead)
|
||
{
|
||
var status = ReadOnce();
|
||
|
||
if (status == TPCANStatus.PCAN_ERROR_ILLOPERATION)
|
||
{
|
||
_log.Error(LogId, "Read thread: illegal operation — stopping.");
|
||
_stopRead = true;
|
||
break;
|
||
}
|
||
|
||
if (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY && status != TPCANStatus.PCAN_ERROR_OK)
|
||
{
|
||
_log.Warning(LogId, $"DrainMessageQueue: {status}");
|
||
EmitStatusChanged(status);
|
||
}
|
||
|
||
// Configurable polling interval to avoid pegging the CPU.
|
||
// Typical value: 2–50 ms depending on operational phase.
|
||
Thread.Sleep(2);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Reads and decodes all frames currently available in the hardware FIFO.
|
||
/// Returns the last <see cref="TPCANStatus"/> encountered.
|
||
/// </summary>
|
||
private TPCANStatus ReadOnce()
|
||
{
|
||
TPCANStatus status;
|
||
do
|
||
{
|
||
status = PCANBasic.Read(_channel, out TPCANMsg frame);
|
||
if (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY)
|
||
DecodeFrame(frame);
|
||
|
||
if (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY && status != TPCANStatus.PCAN_ERROR_OK)
|
||
{
|
||
_log.Warning(LogId, $"ReadOnce: {status}");
|
||
EmitStatusChanged(status);
|
||
}
|
||
}
|
||
while (status != TPCANStatus.PCAN_ERROR_QRCVEMPTY &&
|
||
status != TPCANStatus.PCAN_ERROR_BUSHEAVY);
|
||
|
||
return status;
|
||
}
|
||
|
||
// ── Frame decoding ────────────────────────────────────────────────────────
|
||
|
||
/// <summary>
|
||
/// Decodes a received CAN frame and updates the associated <see cref="CanBusParameter"/>
|
||
/// values, applying the calibration transfer function and exponential smoothing filter.
|
||
/// </summary>
|
||
private void DecodeFrame(TPCANMsg frame)
|
||
{
|
||
// Message ID 0 carries internal bus-status info — ignore.
|
||
if (frame.ID == 0) return;
|
||
|
||
Dictionary<uint, List<CanBusParameter>> snapshot;
|
||
lock (_mapLock) { snapshot = _parameterMap; }
|
||
|
||
if (!snapshot.TryGetValue(frame.ID, out var parameters)) return;
|
||
|
||
byte[] data = frame.DATA;
|
||
|
||
foreach (var param in parameters)
|
||
{
|
||
// Only decode receive parameters — skip send-only params to avoid
|
||
// overwriting outgoing values with received frame bytes.
|
||
if (!param.IsReceive) continue;
|
||
|
||
double previousValue = param.Value;
|
||
|
||
if (param.Name == BenchParameterNames.Temp || param.Name == PumpParameterNames.Temp)
|
||
{
|
||
// Temperature uses a special packed BCD / signed format depending on sensor type.
|
||
param.Value = DecodeTempValue(data, param);
|
||
}
|
||
else if (param.Name == PumpParameterNames.Rpm)
|
||
{
|
||
// RPM is packed in the upper 12 bits across two bytes (two encoding variants).
|
||
param.Value = DecodeRpmValue(data, param);
|
||
param.Value = param.GetTransformResult();
|
||
}
|
||
else
|
||
{
|
||
// Generic 1-byte, 2-byte, or 3-byte big-endian integer.
|
||
int byteSpan = Math.Abs(param.ByteH - param.ByteL);
|
||
if (byteSpan == 0)
|
||
{
|
||
param.Value = data[param.ByteL];
|
||
}
|
||
else if (byteSpan == 1)
|
||
{
|
||
param.Value = (data[param.ByteH] << 8) | data[param.ByteL];
|
||
}
|
||
else
|
||
{
|
||
// 3-byte little-endian variant used for encoder/pulse counters.
|
||
param.Value = (data[param.ByteL + 2] << 16)
|
||
| (data[param.ByteL + 1] << 8)
|
||
| data[param.ByteL];
|
||
}
|
||
|
||
param.Value = param.GetTransformResult();
|
||
if (double.IsInfinity(param.Value)) param.Value = 0;
|
||
}
|
||
|
||
// Spike rejection for BenchRPM: the bench controller occasionally sends
|
||
// a spurious value of 1 RPM — discard it and retain the previous value.
|
||
if (param.Name == BenchParameterNames.BenchRpm && param.Value == 1)
|
||
{
|
||
param.Value = previousValue;
|
||
return;
|
||
}
|
||
|
||
// Spike rejection for QDelivery: discard values that are more than
|
||
// 100x the previous reading (caused by relay switching noise).
|
||
if (param.Name == BenchParameterNames.QDelivery)
|
||
{
|
||
if (previousValue > 0.1 && param.Value > previousValue * 100)
|
||
{
|
||
_log.Warning(LogId,
|
||
$"QDelivery spike suppressed: prev={previousValue:F3}, new={param.Value:F3}");
|
||
param.Value = previousValue;
|
||
}
|
||
}
|
||
|
||
// Apply single-pole IIR low-pass filter.
|
||
// result = prev + alpha * (new - prev)
|
||
param.Value = PassFilterUpdate(previousValue, param.Value, param.Alpha);
|
||
param.NeedsUpdate = true;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Decodes a temperature value from the CAN frame.
|
||
/// The bench uses three different encoding schemes identified by <c>param.Type</c>:
|
||
/// <list type="bullet">
|
||
/// <item>0 — 4-nibble BCD with sign extension: [−256…+256] + fractional nibble</item>
|
||
/// <item>1 — Compact signed: [−54…+∞] with 1/16 fractional resolution</item>
|
||
/// <item>2 — Kelvin raw integer: value = (raw − 273.15) via calibration transform</item>
|
||
/// </list>
|
||
/// </summary>
|
||
private double DecodeTempValue(byte[] data, CanBusParameter param)
|
||
{
|
||
switch (param.Type)
|
||
{
|
||
case 0:
|
||
{
|
||
// Full BCD signed: high byte nibbles encode hundreds/tens, low byte encodes units/fraction.
|
||
double val = -256
|
||
+ 256 * ((data[param.ByteH] & 0xF0) >> 4)
|
||
+ -16
|
||
+ 16 * (data[param.ByteH] & 0x0F)
|
||
+ ((data[param.ByteL] & 0xF0) >> 4);
|
||
val += (data[param.ByteL] & 0x0F) * (1.0 / 16.0);
|
||
return val;
|
||
}
|
||
case 1:
|
||
{
|
||
// Compact signed encoding.
|
||
double val = -54
|
||
+ 16 * ((data[param.ByteH] & 0xF0) >> 4)
|
||
+ -1
|
||
+ (data[param.ByteH] & 0x0F);
|
||
val += ((data[param.ByteL] & 0xF0) >> 4) * (1.0 / 16.0);
|
||
return val;
|
||
}
|
||
case 2:
|
||
{
|
||
// Raw Kelvin integer, converted via calibration formula (subtracts 273.15).
|
||
param.Value = (data[param.ByteH] << 8) | data[param.ByteL];
|
||
return param.GetTransformResult() - 273.15;
|
||
}
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Decodes an RPM value from the CAN frame.
|
||
/// Two encoding variants exist depending on <c>param.Type</c>:
|
||
/// <list type="bullet">
|
||
/// <item>0/1 — Upper 12 bits: ByteH shifted left 4, ByteL shifted right 4</item>
|
||
/// <item>2 — Lower 12 bits: lower nibble of ByteH as upper bits, ByteL as lower byte</item>
|
||
/// </list>
|
||
/// </summary>
|
||
private static double DecodeRpmValue(byte[] data, CanBusParameter param)
|
||
{
|
||
int raw = param.Type switch
|
||
{
|
||
0 or 1 => (data[param.ByteH] << 4) | (data[param.ByteL] >> 4),
|
||
2 => ((data[param.ByteH] & 0x0F) << 8) | data[param.ByteL],
|
||
_ => 0
|
||
};
|
||
return raw;
|
||
}
|
||
|
||
// ── IIR low-pass filter ───────────────────────────────────────────────────
|
||
|
||
/// <summary>
|
||
/// Single-pole exponential moving average: result = prev + alpha * (value − prev).
|
||
/// Rounds to 4 decimal places to avoid floating-point drift accumulation.
|
||
/// </summary>
|
||
private static double PassFilterUpdate(double prev, double value, double alpha)
|
||
=> Math.Round(prev + alpha * (value - prev), 4);
|
||
|
||
// ── Helpers ───────────────────────────────────────────────────────────────
|
||
|
||
private void EmitStatusChanged(TPCANStatus status)
|
||
{
|
||
string text = status.ToString();
|
||
// Strip the "PCAN_ERROR_" prefix for brevity in the UI status bar.
|
||
string shortText = text.StartsWith("PCAN_ERROR_", StringComparison.Ordinal)
|
||
? text[11..]
|
||
: text;
|
||
StatusChanged?.Invoke(shortText, status == TPCANStatus.PCAN_ERROR_OK);
|
||
}
|
||
|
||
private void LogPcanError(string context)
|
||
{
|
||
var sb = new StringBuilder(256);
|
||
PCANBasic.GetErrorText(CurrentStatus, 0, sb);
|
||
_log.Error(LogId, $"{context}: {CurrentStatus} — {sb}");
|
||
}
|
||
|
||
// ── IDisposable ───────────────────────────────────────────────────────────
|
||
|
||
/// <summary>Stops the read thread and releases the PCAN channel.</summary>
|
||
public void Dispose()
|
||
{
|
||
Disconnect();
|
||
_receiveEvent?.Dispose();
|
||
}
|
||
}
|
||
}
|