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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,432 @@
|
||||
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.RobotApp.Interfaces;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
using System.Diagnostics;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.YNZDH;
|
||||
|
||||
[Device(DeviceType.RfHandle, "YNZDH", "YNZDH_RfHandle", "1.0.0",
|
||||
Description = "YNZDH RF Handle (minimal but full simulation properties)")]
|
||||
public class YNZDH_RfHandle : DeviceBase, IRfHandle
|
||||
{
|
||||
private readonly string _port;
|
||||
private readonly int _baud;
|
||||
private readonly ILogger<YNZDH_RfHandle> _logger;
|
||||
|
||||
private ModbusRtuClient? _modbus;
|
||||
|
||||
// High-priority polling thread for real-time data acquisition
|
||||
private Thread? _pollingThread;
|
||||
private CancellationTokenSource? _pollingCts;
|
||||
private volatile bool _shouldPoll = false;
|
||||
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
public event Action? Updated;
|
||||
|
||||
public YNZDH_RfHandle(string deviceId, string deviceName, IConfigurationSection cfg, IServiceProvider serviceProvider)
|
||||
: base(deviceId, deviceName, DeviceType.RfHandle)
|
||||
{
|
||||
_port = cfg.GetValue<string>("Port") ?? throw new Exception("Port is required");
|
||||
_baud = cfg.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");
|
||||
|
||||
var loggerFactory = serviceProvider.GetRequiredService<ILoggerFactory>();
|
||||
_logger = loggerFactory.CreateLogger<YNZDH_RfHandle>();
|
||||
|
||||
AutoReconnectEnabled = true;
|
||||
ReconnectDelayMs = 2000;
|
||||
MaxReconnectAttempts = 0;
|
||||
|
||||
// Khởi tạo PropertyDescriptions → DeviceBase validation pass
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
// ====================== STATES ===========================
|
||||
|
||||
public DateTime LastUpdateTime { get; private set; }
|
||||
|
||||
public int Heartbeat { get; private set; }
|
||||
public bool RemoteReady { get; private set; }
|
||||
public bool EStop { get; private set; }
|
||||
|
||||
public bool LiftUp { get; private set; }
|
||||
public bool LiftDown { get; private set; }
|
||||
public bool RotateLeft { get; private set; }
|
||||
public bool RotateRight { get; private set; }
|
||||
|
||||
public bool ModeSelect { get; private set; }
|
||||
public bool Enable { get; private set; }
|
||||
|
||||
public int Speed { get; private set; } // 0–100
|
||||
public double Linear { get; private set; } // -1 → +1
|
||||
public double Angular { get; private set; } // -1 → +1
|
||||
|
||||
public RFMode Mode { get; private set; } = RFMode.None;
|
||||
|
||||
private Joy? _cachedJoyState;
|
||||
|
||||
// IRfHandle Implementation
|
||||
public Joy? CurrentJoyState
|
||||
{
|
||||
get { lock (_lock) { return _cachedJoyState; } }
|
||||
}
|
||||
|
||||
// ====================== SIM PROPERTIES ====================
|
||||
|
||||
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
||||
{
|
||||
return
|
||||
[
|
||||
new("Heartbeat", "Heartbeat"),
|
||||
new("RemoteReady", "Remote Ready"),
|
||||
new("EStop", "Emergency Stop"),
|
||||
|
||||
new("LiftUp", "Lift Up"),
|
||||
new("LiftDown", "Lift Down"),
|
||||
new("RotateLeft", "Rotate Left"),
|
||||
new("RotateRight", "Rotate Right"),
|
||||
|
||||
new("ModeSelect", "Mode Select"),
|
||||
new("Enable", "Enable"),
|
||||
|
||||
new("Speed", "Speed"),
|
||||
new("Mode", "Mode"),
|
||||
|
||||
new("LastUpdate", "Last Update Time")
|
||||
];
|
||||
}
|
||||
|
||||
private void UpdateProperties()
|
||||
{
|
||||
SetProperty("Heartbeat", Heartbeat.ToString());
|
||||
SetProperty("RemoteReady", RemoteReady.ToString());
|
||||
SetProperty("EStop", EStop.ToString());
|
||||
|
||||
SetProperty("LiftUp", LiftUp.ToString());
|
||||
SetProperty("LiftDown", LiftDown.ToString());
|
||||
SetProperty("RotateLeft", RotateLeft.ToString());
|
||||
SetProperty("RotateRight", RotateRight.ToString());
|
||||
|
||||
SetProperty("ModeSelect", ModeSelect.ToString());
|
||||
SetProperty("Enable", Enable.ToString());
|
||||
|
||||
SetProperty("Speed", Speed.ToString());
|
||||
SetProperty("Mode", Mode.ToString());
|
||||
|
||||
SetProperty("LastUpdate", LastUpdateTime == default
|
||||
? "Never"
|
||||
: LastUpdateTime.ToString("yyyy-MM-dd HH:mm:ss"));
|
||||
}
|
||||
|
||||
// ====================== DEVICEBASE ========================
|
||||
|
||||
protected override Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
try
|
||||
{
|
||||
//_modbus = new ModbusRtuClient(_port, _baud, logger: _logger);
|
||||
_modbus = new ModbusRtuClient(_port, _baud);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Modbus init failed");
|
||||
LastError = ex;
|
||||
OnErrorOccurred(ex, "Modbus init failed");
|
||||
}
|
||||
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnConnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StartPolling();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StopPolling();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
// Stop polling thread before disposing base class
|
||||
StopPolling();
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
protected override Task OnResetAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StopPolling();
|
||||
StartPolling();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||
=> Task.FromResult(_modbus != null);
|
||||
|
||||
// ===================== POLLING LOOP ======================
|
||||
|
||||
/// <summary>
|
||||
/// Start high-priority polling thread for real-time data acquisition at 10Hz
|
||||
/// </summary>
|
||||
private void StartPolling()
|
||||
{
|
||||
StopPolling(); // Đảm bảo không có thread nào đang chạy
|
||||
|
||||
_shouldPoll = true;
|
||||
|
||||
// Tạo mới CancellationTokenSource cho thread mới
|
||||
_pollingCts?.Dispose();
|
||||
_pollingCts = new CancellationTokenSource();
|
||||
|
||||
_pollingThread = new Thread(() => PollingThreadLoop(_pollingCts.Token))
|
||||
{
|
||||
Name = $"YNZDH-RfHandle-Polling-{DeviceId}",
|
||||
IsBackground = false, // Không phải background thread để đảm bảo chạy liên tục
|
||||
Priority = ThreadPriority.Highest // Priority cao để đảm bảo real-time polling
|
||||
};
|
||||
|
||||
_pollingThread.Start();
|
||||
_logger.LogDebug("YNZDH_RfHandle: Started high-priority polling thread at 10Hz for device {DeviceId}", DeviceId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Stop polling thread gracefully
|
||||
/// </summary>
|
||||
private void StopPolling()
|
||||
{
|
||||
_shouldPoll = false;
|
||||
|
||||
_pollingCts?.Cancel();
|
||||
|
||||
if (_pollingThread != null)
|
||||
{
|
||||
if (!_pollingThread.Join(1000)) // Đợi tối đa 1 giây
|
||||
{
|
||||
_logger.LogWarning("YNZDH_RfHandle: Polling thread did not stop gracefully for device {DeviceId}", DeviceId);
|
||||
}
|
||||
_pollingThread = null;
|
||||
}
|
||||
|
||||
// Dispose CancellationTokenSource sau khi thread đã dừng
|
||||
_pollingCts?.Dispose();
|
||||
_pollingCts = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// High-priority polling thread loop - runs at 10Hz (100ms interval)
|
||||
/// Uses Stopwatch for high-precision timing to ensure accurate 10Hz polling rate
|
||||
/// </summary>
|
||||
private void PollingThreadLoop(CancellationToken cancellationToken)
|
||||
{
|
||||
var modbusClient = _modbus;
|
||||
if (modbusClient == null)
|
||||
{
|
||||
_logger.LogError("YNZDH_RfHandle: Modbus client is null");
|
||||
return;
|
||||
}
|
||||
|
||||
Thread.BeginThreadAffinity();
|
||||
try
|
||||
{
|
||||
const int pollingIntervalMs = 100; // 10Hz = 100ms
|
||||
var intervalTicks = pollingIntervalMs * TimeSpan.TicksPerMillisecond;
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
var nextPollTime = stopwatch.ElapsedTicks + intervalTicks;
|
||||
var spinWait = new SpinWait();
|
||||
long currentTicks = 0;
|
||||
|
||||
while (_shouldPoll && !cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
currentTicks = stopwatch.ElapsedTicks;
|
||||
|
||||
// Check if it's time to poll
|
||||
if (currentTicks >= nextPollTime)
|
||||
{
|
||||
try
|
||||
{
|
||||
ushort[] regs = modbusClient.ReadHoldingRegisters(1, 1, 4);
|
||||
DecodeRegisters(regs);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LastError = ex;
|
||||
OnErrorOccurred(ex, "Polling error");
|
||||
ResetToDefaultValues();
|
||||
}
|
||||
|
||||
// Calculate next poll time
|
||||
nextPollTime = currentTicks + intervalTicks;
|
||||
}
|
||||
|
||||
// SpinWait for precise timing (10 spins then reset)
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.SpinOnce();
|
||||
spinWait.Reset();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "YNZDH_RfHandle: Error in polling thread loop for device {DeviceId}", DeviceId);
|
||||
LastError = ex;
|
||||
OnErrorOccurred(ex, "Polling thread error");
|
||||
}
|
||||
finally
|
||||
{
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
}
|
||||
|
||||
// ===================== DECODE ============================
|
||||
|
||||
private void ResetToDefaultValues()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
Heartbeat = 0;
|
||||
RemoteReady = false;
|
||||
EStop = false;
|
||||
|
||||
LiftUp = false;
|
||||
LiftDown = false;
|
||||
RotateLeft = false;
|
||||
RotateRight = false;
|
||||
|
||||
ModeSelect = false;
|
||||
Enable = false;
|
||||
|
||||
Speed = 0;
|
||||
Linear = 0.0;
|
||||
Angular = 0.0;
|
||||
Mode = RFMode.None;
|
||||
|
||||
_cachedJoyState = null;
|
||||
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
Updated?.Invoke();
|
||||
}
|
||||
|
||||
private void DecodeRegisters(ushort[] regs)
|
||||
{
|
||||
byte d0 = (byte)(regs[0] >> 8); // Word0H
|
||||
byte d1 = (byte)(regs[0]); // Word0L
|
||||
byte d2 = (byte)(regs[1] >> 8); // Word1H
|
||||
byte d3 = (byte)(regs[1]); // Word1L
|
||||
byte d6 = (byte)(regs[3] >> 8); // Word3H (JOY FB)
|
||||
byte d7 = (byte)(regs[3]); // Word3L (JOY LR)
|
||||
lock (_lock)
|
||||
{
|
||||
// ===== System =====
|
||||
Heartbeat = (d0 >> 4) & 0x0F;
|
||||
RemoteReady = (d0 & 0x04) != 0;
|
||||
RemoteReady = !RemoteReady;
|
||||
EStop = (d0 & 0x01) != 0;
|
||||
|
||||
// ===== Buttons =====
|
||||
Enable = (d1 & 0x80) != 0;
|
||||
ModeSelect = (d1 & 0x40) != 0;
|
||||
|
||||
LiftUp = (d1 & 0x01) != 0;
|
||||
LiftDown = (d1 & 0x02) != 0;
|
||||
RotateLeft = (d1 & 0x04) != 0;
|
||||
RotateRight = (d1 & 0x08) != 0;
|
||||
|
||||
// ===== Speed =====
|
||||
Speed = Math.Clamp((int)d3, 0, 100);
|
||||
// ===== Mode =====
|
||||
Mode = DecodeMode(d2);
|
||||
|
||||
// ===== Safety =====
|
||||
if (!RemoteReady || !Enable || EStop)
|
||||
{
|
||||
Linear = 0;
|
||||
Angular = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// ===== Joystick ANALOG =====
|
||||
Linear = (d6 - 127f) / 127f;
|
||||
Angular = (127f - d7) / 127f;
|
||||
}
|
||||
|
||||
LastUpdateTime = DateTime.UtcNow;
|
||||
|
||||
// Update cached JoyState
|
||||
_cachedJoyState = CreateJoyStateFromCache();
|
||||
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
Updated?.Invoke();
|
||||
}
|
||||
|
||||
private static RFMode DecodeMode(byte d2) =>
|
||||
(d2 & 0x0F) switch
|
||||
{
|
||||
0x00 => RFMode.Default,
|
||||
0x01 => RFMode.Maintenance,
|
||||
0x02 => RFMode.Override,
|
||||
_ => RFMode.None
|
||||
};
|
||||
|
||||
// ===================== IRfHandle Implementation ===========
|
||||
|
||||
public Task<Joy> ReadJoyStateAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (_cachedJoyState.HasValue)
|
||||
{
|
||||
return Task.FromResult(_cachedJoyState.Value);
|
||||
}
|
||||
|
||||
return Task.FromResult(CreateJoyStateFromCache());
|
||||
}
|
||||
}
|
||||
|
||||
private Joy CreateJoyStateFromCache()
|
||||
{
|
||||
return new Joy
|
||||
{
|
||||
Header = new Header
|
||||
{
|
||||
Stamp = LastUpdateTime == default ? DateTime.UtcNow : LastUpdateTime,
|
||||
FrameId = "rfhandle_frame"
|
||||
},
|
||||
Axes =
|
||||
[
|
||||
Linear, // Axis 0: Forward / Backward
|
||||
Angular, // Axis 1: Left / Right
|
||||
Speed / 100f // Axis 2: Speed
|
||||
],
|
||||
Buttons =
|
||||
[
|
||||
LiftUp ? 1 : 0,
|
||||
LiftDown ? 1 : 0,
|
||||
RotateLeft ? 1 : 0,
|
||||
RotateRight ? 1 : 0,
|
||||
ModeSelect ? 1 : 0,
|
||||
Enable ? 1 : 0,
|
||||
EStop ? 1 : 0
|
||||
]
|
||||
};
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user