Initial commit
This commit is contained in:
@@ -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