Files
hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/DongleLink.cs
T

256 lines
12 KiB
C#
Raw Normal View History

using System;
using System.Collections.Concurrent;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
namespace HexapodSim
{
// The USB nRF24 dongle (2026-current/dongle/) in bridge mode: it stands in for
// the robot's radio. Every packet the real remote sends comes in here; the
// simulated robot's Telemetry goes back out as the radio's ack payload, so the
// remote and its display see the simulation exactly like the real robot.
//
// Wire format both ways: A5 type len payload[len] sum (sum = type+len+payload).
// to dongle 'L' listen (bridge on), 'X' stop, 'A' ack payload (<= 32 B)
// from dongle 'R' received packet, 'S' status (packets u16, acks loaded u16)
// The dongle drops the ack payload if it isn't refreshed for ~0.5 s.
//
// Windows only (the dev PC): the port is opened through kernel32, so there's
// no dependency on System.IO.Ports or the project's API compatibility level.
public sealed class DongleLink : IDisposable
{
public string PortName { get; private set; }
public bool IsOpen => handle_ != IntPtr.Zero && handle_ != InvalidHandle;
public string Error { get; private set; }
public int DonglePacketsPerSec { get; private set; } // from the dongle's status frame
public int DongleAcksPerSec { get; private set; }
public int BadFrames { get; private set; }
readonly ConcurrentQueue<byte[]> packets_ = new ConcurrentQueue<byte[]>();
byte[] pendingAck_; // newest telemetry frame, written by the I/O thread
IntPtr handle_ = IntPtr.Zero;
Thread io_;
volatile bool running_;
// ---- Opening ----
// "auto" = the first USB-serial CH340 port Windows knows about (the dongle's
// CH330N shows up as VID 1A86 / PID 7523); otherwise e.g. "COM7".
public static DongleLink Open(string port)
{
var link = new DongleLink();
string name = port;
if (string.IsNullOrEmpty(name) || name.Equals("auto", StringComparison.OrdinalIgnoreCase))
{
name = FindCh340Port();
if (name == null) { link.Error = "no USB nRF24 dongle (CH340, VID 1A86 PID 7523) found"; return link; }
}
link.PortName = name;
link.OpenPort(name);
if (link.IsOpen)
{
link.Write(Frame((byte)'L', null, 0));
link.running_ = true;
link.io_ = new Thread(link.IoLoop) { IsBackground = true, Name = "DongleLink" };
link.io_.Start();
}
return link;
}
void OpenPort(string name)
{
// Retry briefly: SignalRGB and similar tools grab CH340 ports on enumeration.
for (int attempt = 0; attempt < 25; attempt++)
{
handle_ = CreateFile(@"\\.\" + name, GENERIC_READ | GENERIC_WRITE, 0, IntPtr.Zero, OPEN_EXISTING, 0, IntPtr.Zero);
if (handle_ != InvalidHandle) break;
Thread.Sleep(20);
}
if (handle_ == InvalidHandle)
{
Error = $"can't open {name} (Win32 error {Marshal.GetLastWin32Error()}; is SignalRGB or a serial monitor holding it?)";
handle_ = IntPtr.Zero;
return;
}
var dcb = new DCB { DCBlength = (uint)Marshal.SizeOf<DCB>() };
GetCommState(handle_, ref dcb);
dcb.BaudRate = 115200;
dcb.ByteSize = 8;
dcb.Parity = 0;
dcb.StopBits = 0;
dcb.Flags = 0x0001 | 0x0010; // fBinary, fDtrControl = DTR_CONTROL_ENABLE
SetCommState(handle_, ref dcb);
// Reads return within ~3 ms, so queued telemetry goes out promptly
// (reads and writes on one handle take turns: both run on the I/O thread).
var to = new COMMTIMEOUTS { ReadIntervalTimeout = 1, ReadTotalTimeoutConstant = 3, WriteTotalTimeoutConstant = 200 };
SetCommTimeouts(handle_, ref to);
PurgeComm(handle_, 0x0004 | 0x0008); // RXCLEAR | TXCLEAR
}
public void Dispose()
{
running_ = false;
io_?.Join(300);
if (IsOpen)
{
Write(Frame((byte)'X', null, 0)); // bridge off: the remote sees the link drop
CloseHandle(handle_);
}
handle_ = IntPtr.Zero;
}
// ---- Traffic ----
public bool TryTakePacket(out byte[] packet) => packets_.TryDequeue(out packet);
// Queues the robot's telemetry for the ack of the remote's next packet.
// Only the newest one matters, so an unsent older one is replaced.
public void SendAckPayload(byte[] data, int len)
{
if (len <= 32) Volatile.Write(ref pendingAck_, Frame((byte)'A', data, len));
}
static byte[] Frame(byte type, byte[] data, int len)
{
var f = new byte[4 + len];
f[0] = 0xA5; f[1] = type; f[2] = (byte)len;
int sum = type + len;
for (int i = 0; i < len; i++) { f[3 + i] = data[i]; sum += data[i]; }
f[3 + len] = (byte)sum;
return f;
}
bool Write(byte[] f)
{
if (WriteFile(handle_, f, (uint)f.Length, out _, IntPtr.Zero)) return true;
Error = $"write failed (Win32 error {Marshal.GetLastWin32Error()}): dongle unplugged?";
return false;
}
void IoLoop()
{
var chunk = new byte[512];
var buf = new byte[4096];
int have = 0;
while (running_)
{
byte[] ack = Interlocked.Exchange(ref pendingAck_, null);
if (ack != null && !Write(ack)) break;
if (!ReadFile(handle_, chunk, (uint)chunk.Length, out uint got, IntPtr.Zero))
{
if (running_) Error = $"read failed (Win32 error {Marshal.GetLastWin32Error()}): dongle unplugged?";
break;
}
if (got == 0) continue;
if (have + got > buf.Length) have = 0; // garbage flood: start over
Buffer.BlockCopy(chunk, 0, buf, have, (int)got);
have += (int)got;
have = ParseFrames(buf, have);
}
}
// Consumes complete frames from buf[0..len), returns how many bytes remain.
int ParseFrames(byte[] buf, int len)
{
int i = 0;
while (true)
{
while (i < len && buf[i] != 0xA5) i++; // skip text between frames
if (len - i < 4) break;
byte type = buf[i + 1], n = buf[i + 2];
if (n > 32) { i++; continue; }
if (len - i < 4 + n) break;
int sum = type + n;
for (int k = 0; k < n; k++) sum += buf[i + 3 + k];
if ((byte)sum != buf[i + 3 + n]) { BadFrames++; i++; continue; }
var payload = new byte[n];
Buffer.BlockCopy(buf, i + 3, payload, 0, n);
i += 4 + n;
if (type == 'R') packets_.Enqueue(payload);
else if (type == 'S' && n >= 4)
{
DonglePacketsPerSec = payload[0] | payload[1] << 8;
DongleAcksPerSec = payload[2] | payload[3] << 8;
}
}
int rest = len - i;
if (rest > 0 && i > 0) Buffer.BlockCopy(buf, i, buf, 0, rest);
return rest;
}
// ---- Finding the port: HKLM\SYSTEM\CurrentControlSet\Enum\USB\VID_1A86&PID_7523\<inst>\Device Parameters\PortName ----
static string FindCh340Port()
{
const string baseKey = @"SYSTEM\CurrentControlSet\Enum\USB\VID_1A86&PID_7523";
if (RegOpenKeyEx(HKEY_LOCAL_MACHINE, baseKey, 0, KEY_READ, out IntPtr key) != 0) return null;
try
{
var sb = new StringBuilder(256);
for (uint idx = 0; ; idx++)
{
uint len = (uint)sb.Capacity;
if (RegEnumKeyEx(key, idx, sb, ref len, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero) != 0) break;
string port = ReadString(HKEY_LOCAL_MACHINE, baseKey + "\\" + sb + "\\Device Parameters", "PortName");
if (port != null && PortExists(port)) return port;
}
}
finally { RegCloseKey(key); }
return null;
}
static string ReadString(IntPtr root, string path, string value)
{
var data = new StringBuilder(64);
uint size = (uint)(data.Capacity * 2);
return RegGetValue(root, path, value, RRF_RT_REG_SZ, IntPtr.Zero, data, ref size) == 0 ? data.ToString() : null;
}
static bool PortExists(string name) => QueryDosDevice(name, new StringBuilder(256), 256) != 0;
// ---- Win32 ----
static readonly IntPtr InvalidHandle = new IntPtr(-1);
static readonly IntPtr HKEY_LOCAL_MACHINE = new IntPtr(unchecked((int)0x80000002));
const uint GENERIC_READ = 0x80000000, GENERIC_WRITE = 0x40000000, OPEN_EXISTING = 3;
const int KEY_READ = 0x20019;
const uint RRF_RT_REG_SZ = 0x00000002;
[StructLayout(LayoutKind.Sequential)]
struct DCB
{
public uint DCBlength, BaudRate, Flags;
public ushort wReserved, XonLim, XoffLim;
public byte ByteSize, Parity, StopBits;
public sbyte XonChar, XoffChar, ErrorChar, EofChar, EvtChar;
public ushort wReserved1;
}
[StructLayout(LayoutKind.Sequential)]
struct COMMTIMEOUTS
{
public uint ReadIntervalTimeout, ReadTotalTimeoutMultiplier, ReadTotalTimeoutConstant,
WriteTotalTimeoutMultiplier, WriteTotalTimeoutConstant;
}
[DllImport("kernel32.dll", SetLastError = true, CharSet = CharSet.Unicode)]
static extern IntPtr CreateFile(string name, uint access, uint share, IntPtr sec, uint disposition, uint flags, IntPtr template);
[DllImport("kernel32.dll", SetLastError = true)] static extern bool CloseHandle(IntPtr h);
[DllImport("kernel32.dll", SetLastError = true)] static extern bool GetCommState(IntPtr h, ref DCB dcb);
[DllImport("kernel32.dll", SetLastError = true)] static extern bool SetCommState(IntPtr h, ref DCB dcb);
[DllImport("kernel32.dll", SetLastError = true)] static extern bool SetCommTimeouts(IntPtr h, ref COMMTIMEOUTS t);
[DllImport("kernel32.dll", SetLastError = true)] static extern bool PurgeComm(IntPtr h, uint flags);
[DllImport("kernel32.dll", SetLastError = true)] static extern bool ReadFile(IntPtr h, byte[] buf, uint n, out uint got, IntPtr ov);
[DllImport("kernel32.dll", SetLastError = true)] static extern bool WriteFile(IntPtr h, byte[] buf, uint n, out uint put, IntPtr ov);
[DllImport("kernel32.dll", SetLastError = true, CharSet = CharSet.Unicode)]
static extern uint QueryDosDevice(string device, StringBuilder target, int max);
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
static extern int RegOpenKeyEx(IntPtr key, string sub, int options, int sam, out IntPtr result);
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
static extern int RegEnumKeyEx(IntPtr key, uint index, StringBuilder name, ref uint nameLen, IntPtr reserved, IntPtr cls, IntPtr clsLen, IntPtr lastWrite);
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
static extern int RegGetValue(IntPtr key, string sub, string value, uint flags, IntPtr type, StringBuilder data, ref uint size);
[DllImport("advapi32.dll")] static extern int RegCloseKey(IntPtr key);
}
}