433 lines
14 KiB
C#
433 lines
14 KiB
C#
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
|
||
]
|
||
};
|
||
}
|
||
|
||
}
|