2026 sim: drive the Unity simulation with the real remote over the radio
Dongle (app):
- bridge mode: stands in for the robot's radio (ch 96 / 250k / HXPD1,
auto-ack, dynamic payloads, ack payloads). Received packets go to the PC
as A5-framed 'R' frames; the PC's 'A' frames become the ack payload of
the next packet, like robot/Comms.cpp; dropped after ~0.5 s without a
refresh. 'S' status once a second.
- USART RX through circular DMA into a ring buffer, overrun stall
disabled, error flags cleared: a burst (e.g. SignalRGB probing the port)
used to deafen the app until a replug.
- RX test mode ('x') accepts protocol v4's dynamic-length packets.
- safeboot_flash.py --knock: keeps knocking across a replug to catch
safeboot's cold-boot window when the app doesn't answer.
Unity:
- SimRemote sources: Keyboard, or the real remote via the dongle
(DongleLink.cs: kernel32 serial, auto-finds the CH340, all port I/O on
one background thread). Packets go into the firmware byte for byte, the
robot's telemetry goes back on the ack, so the remote's display shows
the sim. Tested end to end with the real remote.
- Dropped the USB joystick and robot-serial-harness inputs and the old
Serial/Serial2/Serial3/HandleHoystick scripts (and their components in
Main.unity): no USB links to remote or robot any more.
- Test panel: dongle status; no sideways scroll bar.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
1 parent
6b3b2629cd
commit
2b4750f8f4
24 files changed
+682
-710
No files matched your search
@@ -0,0 +1,256 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user