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:
mangavalkandClaude Opus 5.5 committed 2026-11-17 22:48:41 +01:00
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);
}
}
@@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: d16e6c95081705a4788b93525709e796
@@ -103,8 +103,8 @@ namespace HexapodSim
VoidFn start_;
UIntArgFn step_;
PacketFn radioSend_;
BytesInFn radioSendBytes_;
ImuFn setImu_;
BytesInFn serialInput_;
BytesOutFn serialOutput_;
ServosFn getServos_;
FloatsFn getJoints_, getFeet_;
@@ -142,8 +142,8 @@ namespace HexapodSim
n.start_ = n.Fn<VoidFn>("hxsim_start");
n.step_ = n.Fn<UIntArgFn>("hxsim_step");
n.radioSend_ = n.Fn<PacketFn>("hxsim_radio_send");
n.radioSendBytes_ = n.Fn<BytesInFn>("hxsim_radio_send");
n.setImu_ = n.Fn<ImuFn>("hxsim_set_imu");
n.serialInput_ = n.Fn<BytesInFn>("hxsim_serial_input");
n.serialOutput_ = n.Fn<BytesOutFn>("hxsim_serial_output");
n.getServos_ = n.Fn<ServosFn>("hxsim_get_servos");
n.getJoints_ = n.Fn<FloatsFn>("hxsim_get_joints_deg");
@@ -184,16 +184,24 @@ namespace HexapodSim
public void Start() => start_();
public void Step(uint dtMicros) => step_(dtMicros);
public void RadioSend(CmdPacket p) => radioSend_(ref p, Marshal.SizeOf<CmdPacket>());
// A packet exactly as it came over the air (the real remote, via the dongle).
// Any length: the firmware itself drops ones that aren't sizeof(CmdPacket).
public void RadioSend(byte[] bytes) => radioSendBytes_(bytes, bytes.Length);
// Decodes on-air bytes for display; false if the length doesn't match.
public static bool TryDecode(byte[] bytes, out CmdPacket p)
{
p = default;
if (bytes == null || bytes.Length != Marshal.SizeOf<CmdPacket>()) return false;
var h = GCHandle.Alloc(bytes, GCHandleType.Pinned);
try { p = Marshal.PtrToStructure<CmdPacket>(h.AddrOfPinnedObject()); }
finally { h.Free(); }
return true;
}
// Overrides the body-driven IMU with fixed readings from then on (normally unused).
public void SetImu(Vector3 accelG, Vector3 gyroDps) =>
setImu_(accelG.x, accelG.y, accelG.z, gyroDps.x, gyroDps.y, gyroDps.z);
public void SerialInput(string text)
{
if (string.IsNullOrEmpty(text)) return;
byte[] b = System.Text.Encoding.ASCII.GetBytes(text);
serialInput_(b, b.Length);
}
readonly byte[] serialBuf_ = new byte[4096];
public string SerialOutput()
@@ -81,7 +81,7 @@ namespace HexapodSim
StringBuilder serialLine_ = new StringBuilder();
static readonly string[] LegacyMovementScripts =
{ "Walk", "RotationControl", "Leg", "HandleHoystick", "Serial2", "Serial3" };
{ "Walk", "RotationControl", "Leg" };
// Robot-frame axes, for readability (Unity's Vector3.up etc. are Unity axes).
static readonly Vector3 AxisX = new Vector3(1, 0, 0); // forward
@@ -1,47 +1,28 @@
using System;
using UnityEngine;
namespace HexapodSim
{
// Stand-in for the handheld remote. Produces CmdPackets exactly per the
// protocol contract and sends them over the simulated radio. Toggles and
// interlocks mirror remote/RemoteState.cpp and remote/PacketBuilder.cpp -
// keep them in sync if those change.
//
// Input sources:
// Joystick - the real remote as a USB game controller, read through
// Unity input axes "1".."6" like the old HandleHoystick.
// Keyboard - test without any remote (keys listed in SimTestPanel).
// RobotSerialHarness - no radio at all; typed keys go to the firmware's own
// TEST_SERIAL_INPUT harness, like the Arduino Serial Monitor.
// Where the simulated robot's commands come from. Two sources:
// Keyboard - a stand-in remote: builds CmdPackets per the protocol contract
// and sends them over the simulated radio. Toggles and
// interlocks mirror remote/RemoteState.cpp and
// remote/PacketBuilder.cpp - keep them in sync if those change.
// RealRemote - the real handheld remote, over the air: the USB nRF24 dongle
// (2026-current/dongle/, bridge mode) receives its packets, they
// go into the firmware unchanged, and the robot's Telemetry goes
// back as the radio's ack payload. The remote's display then
// shows the simulated robot. Nothing about the remote is
// mirrored here: its own firmware does all of it.
// Toggles, the speed slider and the handset tilt sliders live in SimTestPanel.
public class SimRemote : MonoBehaviour
{
public enum InputSource { Joystick, Keyboard, RobotSerialHarness }
[Serializable]
public class AxisBinding
{
public string axis;
public bool invert;
public AxisBinding(string axis, bool invert) { this.axis = axis; this.invert = invert; }
}
public enum InputSource { Keyboard, RealRemote }
public InputSource source = InputSource.Keyboard;
// Defaults follow the old HandleHoystick: axes 4/6/5 were the walk stick
// (b2/b1/b3), 2/1/3 the rotation stick (a1/a2/a3), with 1 and 2 inverted.
// Which of 4/6 is forward vs sideways wasn't recoverable from the old code:
// check the raw axis readout in the test panel and fix here.
[Header("Joystick axes (Input Manager names)")]
[Tooltip("Stick convention after invert: X + = right, Y + = forward, twist + = counterclockwise.")]
public AxisBinding leftX = new AxisBinding("4", false);
public AxisBinding leftY = new AxisBinding("6", false);
public AxisBinding leftTwist = new AxisBinding("5", false);
public AxisBinding rightX = new AxisBinding("2", true);
public AxisBinding rightY = new AxisBinding("1", true);
public AxisBinding rightTwist = new AxisBinding("3", false);
[Range(0, 0.5f)] public float deadzone = 0.08f;
[Header("Real remote (USB nRF24 dongle)")]
[Tooltip("\"auto\" finds the dongle's CH340 port; or e.g. COM7.")]
public string donglePort = "auto";
[Header("Operator state (same toggles as the Nextion screen)")]
public bool armed;
@@ -66,49 +47,61 @@ namespace HexapodSim
public float armCenterTolerance = 0.05f; // ARM_CENTER_TOLERANCE / 1000
public float keyStickRatePerSec = 4f; // how fast a held key moves the virtual stick
// Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST].
// Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST] (keyboard).
public readonly float[] sticks = new float[6];
public readonly float[] rawAxes = new float[6]; // Input axes "1".."6" as read
public CmdPacket LastPacket { get; private set; }
public string Message { get; private set; } = "";
public bool JoystickAxesMissing { get; private set; }
// Real remote, for the panel.
public DongleLink Dongle { get; private set; }
public int RemotePacketsPerSec { get; private set; }
public int LastPacketLength { get; private set; }
public float SecondsSinceRemotePacket => Time.time - lastRemotePacket_;
float speedCurrent_ = 128;
float lastWalkInputTime_ = -999f;
byte seq_;
string serialKeys_ = "";
float messageUntil_;
float lastRemotePacket_ = -999f;
int packetsThisSecond_;
float secondStart_;
readonly byte[] ack_ = new byte[32];
public void ResetState()
{
armed = stand = false;
speedCurrent_ = speedTarget;
lastWalkInputTime_ = -999f;
serialKeys_ = "";
}
void Awake()
{
if (Input.GetJoystickNames().Length > 0 && !string.IsNullOrEmpty(Input.GetJoystickNames()[0]))
source = InputSource.Joystick;
}
void Update()
{
for (int i = 0; i < 6; i++) rawAxes[i] = ReadAxis((i + 1).ToString());
// Open the dongle while the real remote is selected, release it otherwise.
if (source == InputSource.RealRemote && Dongle == null) ConnectDongle();
else if (source != InputSource.RealRemote && Dongle != null) DisconnectDongle();
if (source == InputSource.RobotSerialHarness)
if (source == InputSource.Keyboard)
{
serialKeys_ += Input.inputString; // raw keystrokes for the firmware's harness
return;
HandleToggleKeys();
ReadKeyboardSticks(Time.deltaTime);
}
HandleToggleKeys();
if (source == InputSource.Joystick) ReadJoystick();
else ReadKeyboardSticks(Time.deltaTime);
if (Time.time > messageUntil_) Message = "";
}
void OnDisable() => DisconnectDongle();
public void ConnectDongle()
{
DisconnectDongle();
Dongle = DongleLink.Open(donglePort);
}
public void DisconnectDongle()
{
Dongle?.Dispose();
Dongle = null;
}
// ---- Operator commands (also called by SimTestPanel buttons) ----
public void ToggleArm()
@@ -144,32 +137,6 @@ namespace HexapodSim
if (Input.GetKeyDown(KeyCode.PageDown)) height = Mathf.Max(-1000, height - 100);
}
// ---- Sticks ----
float ReadAxis(string name)
{
try { return Input.GetAxis(name); }
catch (ArgumentException) { JoystickAxesMissing = true; return 0; } // axis not defined in Input Manager
}
float Read(AxisBinding b)
{
float v = ReadAxis(b.axis) * (b.invert ? -1 : 1);
float mag = Mathf.Abs(v);
if (mag <= deadzone) return 0;
return Mathf.Sign(v) * Mathf.Clamp01((mag - deadzone) / (1 - deadzone));
}
void ReadJoystick()
{
sticks[0] = Read(leftX);
sticks[1] = Read(leftY);
sticks[2] = Read(leftTwist);
sticks[3] = Read(rightX);
sticks[4] = Read(rightY);
sticks[5] = Read(rightTwist);
}
// WASD + Q/E = left stick (walk, turn), IJKL + U/O = right stick.
void ReadKeyboardSticks(float dt)
{
@@ -187,17 +154,36 @@ namespace HexapodSim
// Called by HexapodSimulation once per remote TX period.
public void Transmit(HexapodSimNative native, float dt)
{
if (source == InputSource.RobotSerialHarness)
{
native.SerialInput(serialKeys_); // no radio: the firmware's harness injects its own packets
serialKeys_ = "";
return;
}
if (source == InputSource.RealRemote) { ForwardRealRemote(native); return; }
speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt);
LastPacket = BuildPacket();
native.RadioSend(LastPacket);
}
// Everything the dongle heard since last time goes into the firmware in
// order, and each packet's ack carries the telemetry the robot loaded in
// reply - the same exchange as on the real robot (robot/Comms.cpp).
void ForwardRealRemote(HexapodSimNative native)
{
if (Time.time - secondStart_ >= 1f)
{
RemotePacketsPerSec = packetsThisSecond_;
packetsThisSecond_ = 0;
secondStart_ = Time.time;
}
if (Dongle == null || !Dongle.IsOpen) return;
while (Dongle.TryTakePacket(out byte[] pkt))
{
native.RadioSend(pkt);
int n = native.GetAckPayload(ack_);
if (n > 0) Dongle.SendAckPayload(ack_, n);
LastPacketLength = pkt.Length;
if (HexapodSimNative.TryDecode(pkt, out var p)) LastPacket = p;
lastRemotePacket_ = Time.time;
packetsThisSecond_++;
}
}
static short Wire(float v) => (short)Mathf.RoundToInt(Mathf.Clamp(v, -1f, 1f) * 1000f);
// Same mapping as remote/PacketBuilder.cpp.
@@ -208,6 +194,7 @@ namespace HexapodSim
seq = seq_++,
speed_scale = (byte)Mathf.RoundToInt(speedCurrent_),
body_height = (short)Mathf.Clamp(height, -1000, 1000), // a setting: sent even while disarmed
cal_servo = 0xFF, // CAL_NO_SERVO: never calibrates
};
byte f = 0;
if (armed) f |= CmdPacket.FLAG_ARMED;
@@ -16,7 +16,7 @@ namespace HexapodSim
Vector2 scroll_;
GUIStyle mono_;
static readonly string[] SourceNames = { "Joystick", "Keyboard", "Robot serial" };
static readonly string[] SourceNames = { "Keyboard", "Real remote (radio)" };
void Awake()
{
@@ -35,8 +35,10 @@ namespace HexapodSim
if (mono_ == null)
mono_ = new GUIStyle(GUI.skin.label) { font = Font.CreateDynamicFontFromOSFont("Consolas", 12), fontSize = 12, wordWrap = false };
GUILayout.BeginArea(new Rect(10, 10, 390, Screen.height - 20), GUI.skin.box);
scroll_ = GUILayout.BeginScrollView(scroll_);
GUILayout.BeginArea(new Rect(10, 10, 420, Screen.height - 20), GUI.skin.box);
// Vertical scrolling only: the monospace lines run a little past the
// width, and a sideways bar for that is just noise.
scroll_ = GUILayout.BeginScrollView(scroll_, false, false, GUIStyle.none, GUI.skin.verticalScrollbar);
GUILayout.Label("<b>Hexapod firmware simulation</b> (F1 hides)", new GUIStyle(GUI.skin.label) { richText = true });
if (sim_.LoadError != null)
@@ -65,18 +67,13 @@ namespace HexapodSim
// ---- Input source ----
GUILayout.Space(6);
remote_.source = (SimRemote.InputSource)GUILayout.Toolbar((int)remote_.source, SourceNames);
if (remote_.source == SimRemote.InputSource.RobotSerialHarness)
if (remote_.source == SimRemote.InputSource.RealRemote)
{
GUILayout.Label("Keys go to robot.ino's TEST_SERIAL_INPUT harness (no radio):\n" +
"a arm s sit/stand w/x fwd/back q/e strafe L/R\n" +
"z/c yaw g gait m mode space stop", mono_);
DongleSection();
}
else
{
if (remote_.source == SimRemote.InputSource.Keyboard)
GUILayout.Label("WASD walk Q/E turn IJKL + U/O right stick\n1 arm 2 stand 3 gait 4 mode 5 R-stick 6/7 gyros\nBackspace disarm +/- speed PgUp/PgDn height", mono_);
if (remote_.source == SimRemote.InputSource.Joystick && remote_.JoystickAxesMissing)
GUILayout.Label("Input axes \"1\"..\"6\" are missing from the Input Manager.");
GUILayout.Label("WASD walk Q/E turn IJKL + U/O right stick\n1 arm 2 stand 3 gait 4 mode 5 R-stick 6/7 gyros\nBackspace disarm +/- speed PgUp/PgDn height", mono_);
// ---- Toggles: same set as the remote's touchscreen ----
GUILayout.BeginHorizontal();
@@ -116,11 +113,10 @@ namespace HexapodSim
// ---- Live values ----
GUILayout.Space(6);
var st = remote_.sticks;
var raw = remote_.rawAxes;
var p = remote_.LastPacket;
GUILayout.Label(
$"sticks L {st[0],5:F2} {st[1],5:F2} {st[2],5:F2} R {st[3],5:F2} {st[4],5:F2} {st[5],5:F2}\n" +
$"axes 1-6 {raw[0],5:F2} {raw[1],5:F2} {raw[2],5:F2} {raw[3],5:F2} {raw[4],5:F2} {raw[5],5:F2}\n" +
(remote_.source == SimRemote.InputSource.Keyboard
? $"sticks L {st[0],5:F2} {st[1],5:F2} {st[2],5:F2} R {st[3],5:F2} {st[4],5:F2} {st[5],5:F2}\n" : "") +
$"packet #{p.seq,3} vx {p.vx,5} vy {p.vy,5} yaw {p.yaw_rate,5}\n" +
$" r {p.rx,5} {p.ry,5} {p.rz,5} tilt {p.remote_roll,5} {p.remote_pitch,5}\n" +
$" height {p.body_height,5} speed {p.speed_scale,3} flags {System.Convert.ToString(p.flags, 2).PadLeft(8, '0')}",
@@ -136,6 +132,36 @@ namespace HexapodSim
GUILayout.EndArea();
}
// The real remote drives the simulation over the air through the USB dongle;
// every control is on the remote itself, so this only shows the link.
void DongleSection()
{
var d = remote_.Dongle;
var rich = new GUIStyle(GUI.skin.label) { richText = true, wordWrap = true };
GUILayout.BeginHorizontal();
GUILayout.Label("dongle port", GUILayout.Width(80));
remote_.donglePort = GUILayout.TextField(remote_.donglePort, GUILayout.Width(70));
if (GUILayout.Button("Reconnect")) remote_.ConnectDongle();
GUILayout.EndHorizontal();
if (d == null) { GUILayout.Label("connecting..."); return; }
if (!d.IsOpen || d.Error != null)
{
GUILayout.Label("<color=#ff6060>" + (d.Error ?? "not open") + "</color>", rich);
if (!d.IsOpen) return;
}
float age = remote_.SecondsSinceRemotePacket;
string link = age < 0.5f ? "<color=#60ff60>receiving</color>" : "<color=#ffb040>no packets - is the remote on?</color>";
GUILayout.Label(
$"{d.PortName}: {link}\n" +
$"remote {remote_.RemotePacketsPerSec} pkt/s here, dongle {d.DonglePacketsPerSec} pkt/s, acks {d.DongleAcksPerSec}/s" +
(remote_.LastPacketLength != 0 && remote_.LastPacketLength != System.Runtime.InteropServices.Marshal.SizeOf<CmdPacket>()
? $"\n<color=#ff6060>packets are {remote_.LastPacketLength} bytes: remote and sim built from different protocol versions</color>" : "") +
(d.BadFrames > 0 ? $"\n{d.BadFrames} corrupt frames" : ""),
rich);
GUILayout.Label("Use the remote's sticks and touchscreen. Its display shows this simulated robot.", rich);
}
static string StanceString(int mask)
{
var sb = new StringBuilder(6);