256 lines
12 KiB
C#
256 lines
12 KiB
C#
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);
|
||
|
|
}
|
||
|
|
}
|