Initial commit
This commit is contained in:
@@ -0,0 +1,380 @@
|
||||
using System.Diagnostics;
|
||||
using System.IO.Ports;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.YNZDH;
|
||||
|
||||
public class ModbusRtuClient : IDisposable
|
||||
{
|
||||
private readonly ILogger? _logger;
|
||||
private SerialPort? _port;
|
||||
|
||||
private readonly string _portName;
|
||||
private readonly int _baud;
|
||||
private readonly Parity _parity;
|
||||
private readonly int _dataBits;
|
||||
private readonly StopBits _stopBits;
|
||||
private readonly int _readTimeoutMs;
|
||||
private readonly int _writeTimeoutMs;
|
||||
|
||||
private readonly object _sync = new();
|
||||
private bool _faulted = false;
|
||||
private DateTime _lastRetry = DateTime.MinValue;
|
||||
private int _retryDelayMs = 1000; // backoff min = 1s
|
||||
private DateTime _lastDataTime = DateTime.MinValue;
|
||||
private readonly int _dataTimeoutMs = 10_000; // 10 giây
|
||||
private int _consecutiveFails = 0;
|
||||
private readonly int _maxFails = 3; // sau 3 lần fail liên tiếp thì coi như lost
|
||||
|
||||
public bool IsFaulted => _faulted;
|
||||
|
||||
public ModbusRtuClient(string portName,
|
||||
int baud = 9600,
|
||||
Parity parity = Parity.None,
|
||||
int dataBits = 8,
|
||||
StopBits stopBits = StopBits.One,
|
||||
int readTimeoutMs = 50,
|
||||
int writeTimeoutMs = 50,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
_portName = portName;
|
||||
_baud = baud;
|
||||
_parity = parity;
|
||||
_dataBits = dataBits;
|
||||
_stopBits = stopBits;
|
||||
_readTimeoutMs = readTimeoutMs;
|
||||
_writeTimeoutMs = writeTimeoutMs;
|
||||
_lastDataTime = DateTime.Now;
|
||||
_consecutiveFails = 0;
|
||||
|
||||
EnsureConnected();
|
||||
}
|
||||
|
||||
// -------------------------
|
||||
// AUTO RECONNECT (giống TadaRs485Client)
|
||||
// -------------------------
|
||||
// NOTE: This method uses lock (_sync) which may cause contention if called from multiple threads.
|
||||
// If called from a non-realtime thread, it may delay realtime polling thread.
|
||||
private void EnsureConnected()
|
||||
{
|
||||
var ensureStartTicks = Stopwatch.GetTimestamp();
|
||||
double lockAcquisitionMs = 0;
|
||||
double checkTimeMs = 0;
|
||||
double disposeTimeMs = 0;
|
||||
double createTimeMs = 0;
|
||||
double openTimeMs = 0;
|
||||
int currentThreadId = Thread.CurrentThread.ManagedThreadId;
|
||||
string? currentThreadName = Thread.CurrentThread.Name;
|
||||
|
||||
var lockStartTicks = Stopwatch.GetTimestamp();
|
||||
lock (_sync)
|
||||
{
|
||||
var lockEndTicks = Stopwatch.GetTimestamp();
|
||||
lockAcquisitionMs = ((lockEndTicks - lockStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
|
||||
var checkStartTicks = Stopwatch.GetTimestamp();
|
||||
if (_port != null && _port.IsOpen && !_faulted)
|
||||
{
|
||||
var checkEndTicks = Stopwatch.GetTimestamp();
|
||||
checkTimeMs = ((checkEndTicks - checkStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
return;
|
||||
}
|
||||
|
||||
if ((DateTime.Now - _lastRetry).TotalMilliseconds < _retryDelayMs)
|
||||
{
|
||||
var checkEndTicks = Stopwatch.GetTimestamp();
|
||||
checkTimeMs = ((checkEndTicks - checkStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
return;
|
||||
}
|
||||
var checkEndTicks2 = Stopwatch.GetTimestamp();
|
||||
checkTimeMs = ((checkEndTicks2 - checkStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
|
||||
_lastRetry = DateTime.Now;
|
||||
|
||||
try
|
||||
{
|
||||
var disposeStartTicks = Stopwatch.GetTimestamp();
|
||||
_port?.Dispose();
|
||||
var disposeEndTicks = Stopwatch.GetTimestamp();
|
||||
disposeTimeMs = ((disposeEndTicks - disposeStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
|
||||
var createStartTicks = Stopwatch.GetTimestamp();
|
||||
_port = new SerialPort(_portName, _baud, _parity, _dataBits, _stopBits)
|
||||
{
|
||||
ReadTimeout = _readTimeoutMs,
|
||||
WriteTimeout = _writeTimeoutMs,
|
||||
// Set buffer sizes to minimize kernel delays
|
||||
ReadBufferSize = 4096,
|
||||
WriteBufferSize = 4096
|
||||
};
|
||||
var createEndTicks = Stopwatch.GetTimestamp();
|
||||
createTimeMs = ((createEndTicks - createStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
|
||||
var openStartTicks = Stopwatch.GetTimestamp();
|
||||
_port.Open();
|
||||
var openEndTicks = Stopwatch.GetTimestamp();
|
||||
openTimeMs = ((openEndTicks - openStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
|
||||
_faulted = false;
|
||||
_retryDelayMs = 1000; // reset backoff
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] Connect failed: {ex.Message}", ex.Message);
|
||||
_faulted = true;
|
||||
|
||||
// exponential backoff giống Tada
|
||||
_retryDelayMs = Math.Min(_retryDelayMs * 2, 30_000);
|
||||
}
|
||||
}
|
||||
|
||||
var ensureEndTicks = Stopwatch.GetTimestamp();
|
||||
var ensureTotalMs = ((ensureEndTicks - ensureStartTicks) * 1000.0) / Stopwatch.Frequency;
|
||||
|
||||
// Log if EnsureConnected took longer than 10ms (should be very fast if already connected)
|
||||
if (ensureTotalMs > 10.0 && _logger != null)
|
||||
{
|
||||
_logger.LogWarning(
|
||||
"[ModbusRtuClient] Slow EnsureConnected: Total={TotalMs:F1}ms, ThreadId={ThreadId}, ThreadName={ThreadName}, " +
|
||||
"LockAcquisition={LockAcquisitionMs:F1}ms, Check={CheckTimeMs:F1}ms, " +
|
||||
"Dispose={DisposeTimeMs:F1}ms, Create={CreateTimeMs:F1}ms, Open={OpenTimeMs:F1}ms. " +
|
||||
"NOTE: High LockAcquisition time indicates lock contention from other threads.",
|
||||
ensureTotalMs, currentThreadId, currentThreadName ?? "Unknown",
|
||||
lockAcquisitionMs, checkTimeMs, disposeTimeMs, createTimeMs, openTimeMs);
|
||||
}
|
||||
}
|
||||
|
||||
private void CheckDataTimeout()
|
||||
{
|
||||
if (_lastDataTime != DateTime.MinValue &&
|
||||
(DateTime.Now - _lastDataTime).TotalMilliseconds > _dataTimeoutMs)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] Communication lost (data timeout).");
|
||||
_faulted = true;
|
||||
_lastDataTime = DateTime.MinValue; // reset để tránh spam log
|
||||
}
|
||||
}
|
||||
|
||||
public void ForceReconnect()
|
||||
{
|
||||
lock (_sync)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_port != null)
|
||||
{
|
||||
try { if (_port.IsOpen) _port.Close(); } catch { }
|
||||
_port.Dispose();
|
||||
_port = null;
|
||||
}
|
||||
}
|
||||
catch { }
|
||||
|
||||
_faulted = false;
|
||||
_lastRetry = DateTime.MinValue;
|
||||
_retryDelayMs = 1000;
|
||||
_consecutiveFails = 0;
|
||||
_lastDataTime = DateTime.Now;
|
||||
|
||||
EnsureConnected();
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------
|
||||
// CRC
|
||||
// -------------------------
|
||||
private static ushort Crc16(byte[] data, int len)
|
||||
{
|
||||
ushort crc = 0xFFFF;
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
crc ^= data[i];
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
bool lsb = (crc & 0x0001) != 0;
|
||||
crc >>= 1;
|
||||
if (lsb) crc ^= 0xA001;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
// -------------------------
|
||||
// TX/RX WITH RECONNECT
|
||||
// -------------------------
|
||||
private byte[] TxRx(byte[] req, int respLen)
|
||||
{
|
||||
EnsureConnected();
|
||||
if (_port == null || !_port.IsOpen || _faulted)
|
||||
throw new Exception("Modbus port not available");
|
||||
|
||||
try
|
||||
{
|
||||
// Build frame
|
||||
ushort crc = Crc16(req, req.Length);
|
||||
byte[] frame = new byte[req.Length + 2];
|
||||
Array.Copy(req, frame, req.Length);
|
||||
frame[^2] = (byte)(crc & 0xFF); // CRC Lo
|
||||
frame[^1] = (byte)(crc >> 8 & 0xFF); // CRC Hi
|
||||
|
||||
// Discard buffer and write
|
||||
_port.DiscardInBuffer();
|
||||
_port.DiscardOutBuffer();
|
||||
_port.Write(frame, 0, frame.Length);
|
||||
|
||||
// Read response
|
||||
byte[] buf = new byte[respLen];
|
||||
int got = 0;
|
||||
while (got < respLen)
|
||||
{
|
||||
int bytesToRead = respLen - got;
|
||||
int bytesRead = _port.Read(buf, got, bytesToRead); // may throw TimeoutException
|
||||
got += bytesRead;
|
||||
}
|
||||
|
||||
// Verify CRC
|
||||
if (got < 3)
|
||||
{
|
||||
_consecutiveFails++;
|
||||
if (_consecutiveFails >= _maxFails)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] Communication lost (response too short).");
|
||||
_faulted = true;
|
||||
}
|
||||
CheckDataTimeout();
|
||||
throw new Exception("Response too short");
|
||||
}
|
||||
|
||||
ushort rxCrc = (ushort)(buf[got - 2] | buf[got - 1] << 8);
|
||||
ushort calc = Crc16(buf, got - 2);
|
||||
if (rxCrc != calc)
|
||||
{
|
||||
_consecutiveFails++;
|
||||
if (_consecutiveFails >= _maxFails)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] Communication lost (CRC mismatch).");
|
||||
_faulted = true;
|
||||
}
|
||||
CheckDataTimeout();
|
||||
throw new Exception("CRC mismatch");
|
||||
}
|
||||
|
||||
// Success - reset fail counter and update last data time
|
||||
_lastDataTime = DateTime.Now;
|
||||
_consecutiveFails = 0;
|
||||
return buf;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] IO error: {ExMessage}", ex.Message);
|
||||
_consecutiveFails++;
|
||||
if (_consecutiveFails >= _maxFails)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] Communication lost (too many failed reads).");
|
||||
_faulted = true;
|
||||
}
|
||||
CheckDataTimeout();
|
||||
EnsureConnected(); // thử reconnect
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Read Holding Registers (FC 0x03)
|
||||
/// </summary>
|
||||
public ushort[] ReadHoldingRegisters(byte slave, ushort startAddr, ushort quantity)
|
||||
{
|
||||
byte[] pdu =
|
||||
[
|
||||
slave, 0x03,
|
||||
(byte)(startAddr >> 8), (byte)(startAddr & 0xFF),
|
||||
(byte)(quantity >> 8), (byte)(quantity & 0xFF),
|
||||
];
|
||||
|
||||
// Expected response: [slave][0x03][byteCount][data...][CRClo][CRChi]
|
||||
int byteCount = quantity * 2;
|
||||
int respLen = 3 + byteCount + 2;
|
||||
var resp = TxRx(pdu, respLen);
|
||||
|
||||
if (resp[0] != slave || resp[1] != 0x03)
|
||||
{
|
||||
_consecutiveFails++;
|
||||
if (_consecutiveFails >= _maxFails)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] Communication lost (invalid response function).");
|
||||
_faulted = true;
|
||||
}
|
||||
CheckDataTimeout();
|
||||
throw new Exception("Invalid response function");
|
||||
}
|
||||
|
||||
if (resp[2] != byteCount)
|
||||
{
|
||||
_consecutiveFails++;
|
||||
if (_consecutiveFails >= _maxFails)
|
||||
{
|
||||
_logger?.LogError("[ModbusRtuClient] Communication lost (unexpected byte count).");
|
||||
_faulted = true;
|
||||
}
|
||||
CheckDataTimeout();
|
||||
throw new Exception("Unexpected byte count");
|
||||
}
|
||||
|
||||
// Success - reset fail counter and update last data time (TxRx already did this, but ensure it's updated)
|
||||
// Note: _consecutiveFails and _lastDataTime are already reset in TxRx() on success
|
||||
_lastDataTime = DateTime.Now;
|
||||
|
||||
ushort[] regs = new ushort[quantity];
|
||||
for (int i = 0; i < quantity; i++)
|
||||
{
|
||||
int idx = 3 + i * 2;
|
||||
regs[i] = (ushort)(resp[idx] << 8 | resp[idx + 1]); // Big-endian to ushort
|
||||
}
|
||||
return regs;
|
||||
}
|
||||
|
||||
public void WriteMultipleRegisters(byte slave, ushort startAddr, ushort[] values)
|
||||
{
|
||||
int byteCount = values.Length * 2;
|
||||
byte[] pdu = new byte[7 + byteCount];
|
||||
|
||||
pdu[0] = slave;
|
||||
pdu[1] = 0x10;
|
||||
pdu[2] = (byte)(startAddr >> 8);
|
||||
pdu[3] = (byte)startAddr;
|
||||
pdu[4] = (byte)(values.Length >> 8);
|
||||
pdu[5] = (byte)values.Length;
|
||||
pdu[6] = (byte)byteCount;
|
||||
|
||||
for (int i = 0; i < values.Length; i++)
|
||||
{
|
||||
pdu[7 + i * 2] = (byte)(values[i] >> 8);
|
||||
pdu[7 + i * 2 + 1] = (byte)values[i];
|
||||
}
|
||||
|
||||
int respLen = 8;
|
||||
TxRx(pdu, respLen);
|
||||
}
|
||||
|
||||
// -------------------------
|
||||
// Dispose
|
||||
// -------------------------
|
||||
public void Dispose()
|
||||
{
|
||||
lock (_sync)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_port != null)
|
||||
{
|
||||
if (_port.IsOpen) _port.Close();
|
||||
_port.Dispose();
|
||||
}
|
||||
}
|
||||
catch { }
|
||||
_port = null;
|
||||
}
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user