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

+16 -10
View File
@@ -188,8 +188,10 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
`Knie.x = −(tibia + 90)`. `Knie.x = −(tibia + 90)`.
- **Unity (`Assets/HexapodSim/`):** - **Unity (`Assets/HexapodSim/`):**
- `HexapodSimBootstrap` auto-adds `HexapodSimulation` in any scene and - `HexapodSimBootstrap` auto-adds `HexapodSimulation` in any scene and
disables the old scripts (`Walk`, `RotationControl`, `Leg`, disables the old scripts (`Walk`, `RotationControl`, `Leg`). Those are
`HandleHoystick`). Those are kept for reference only. kept for reference only. The old USB serial / joystick scripts
(`Serial*`, `HandleHoystick`) were removed (owner, 2026-11-17: no USB
links to remote or robot any more, everything goes over the radio).
- `OpenScadModel` replaces the old scene's robot with the OpenSCAD model - `OpenScadModel` replaces the old scene's robot with the OpenSCAD model
(meshes from `hardware/openscad/export_unity.py` in (meshes from `hardware/openscad/export_unity.py` in
`Resources/OpenScadModel/`): it builds a new rig with the same names at `Resources/OpenScadModel/`): it builds a new rig with the same names at
@@ -198,9 +200,16 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
- `HexapodSimNative` loads a temp copy of the DLL via - `HexapodSimNative` loads a temp copy of the DLL via
LoadLibrary/GetProcAddress, so there's no file lock and each Play is a LoadLibrary/GetProcAddress, so there's no file lock and each Play is a
fresh power-on. fresh power-on.
- `SimRemote` stands in for the remote (Joystick axes "1"–"6" as in the - `SimRemote` has two sources. **Keyboard** stands in for the remote and
old project, Keyboard, or the robot's own serial harness) and mirrors mirrors `remote/PacketBuilder.cpp` + `RemoteState.cpp` rules (keep them
`remote/PacketBuilder.cpp` + `RemoteState.cpp` rules. Keep them in sync. in sync). **Real remote** takes the real remote's packets over the air
from the USB nRF24 dongle (`DongleLink.cs`, the dongle's bridge mode,
see `dongle/README.md`), feeds them to the firmware unchanged
(`HexapodSimNative.RadioSend(byte[])`) and returns the robot's
telemetry as the ack payload, so the remote's display shows the sim.
`DongleLink` opens the port through kernel32 (no `System.IO.Ports`),
auto-finds the CH340 (VID 1A86 PID 7523), and does all port I/O on one
background thread.
- `SimTestPanel` is the IMGUI panel (F1). - `SimTestPanel` is the IMGUI panel (F1).
- Body motion is kinematic and lives **in the DLL** (`sim/SimBody.cpp`, - Body motion is kinematic and lives **in the DLL** (`sim/SimBody.cpp`,
`hxsim_get_body`), so Unity shows exactly what `sim_smoke` checks. `hxsim_get_body`), so Unity shows exactly what `sim_smoke` checks.
@@ -225,8 +234,8 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
stand → walk. The harness does not exercise the right stick or remote stand → walk. The harness does not exercise the right stick or remote
tilt. Add keys if needed. tilt. Add keys if needed.
- **Robot firmware in simulation:** `sim/build/sim_smoke.exe` (headless, - **Robot firmware in simulation:** `sim/build/sim_smoke.exe` (headless,
scripted checks), or Unity Play mode with Keyboard / Joystick / Robot scripted checks), or Unity Play mode with Keyboard input or the real
serial input. remote over the radio (USB nRF24 dongle).
- **Remote without robot or display:** the USB serial console (see - **Remote without robot or display:** the USB serial console (see
`remote/README.md`). `p` shows raw/mapped sticks, IMU, link and state. `remote/README.md`). `p` shows raw/mapped sticks, IMU, link and state.
`d` dumps packets at 10 Hz. With no robot, the link shows DOWN and `d` dumps packets at 10 Hz. With no robot, the link shows DOWN and
@@ -441,9 +450,6 @@ Remote (`remote/README.md` has detail):
- USB serial at 250000. It received 18 servo bytes + checksum + `0xFE` - USB serial at 250000. It received 18 servo bytes + checksum + `0xFE`
from the old Unity `Serial.cs` (servo passthrough, forwarded over RF from the old Unity `Serial.cs` (servo passthrough, forwarded over RF
to the robot) and printed `i_value,...` stick lines. to the robot) and printed `i_value,...` stick lines.
- The Unity remote stand-in's joystick axis assignment (which of axes 4/6
is forward) couldn't be recovered from the old code. Check it with the
panel's raw-axis readout.
- Radio retry timing (`RADIO_RETRY_*`) is untested under real link loss. - Radio retry timing (`RADIO_RETRY_*`) is untested under real link loss.
## Working rules for this repo ## Working rules for this repo
@@ -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_; VoidFn start_;
UIntArgFn step_; UIntArgFn step_;
PacketFn radioSend_; PacketFn radioSend_;
BytesInFn radioSendBytes_;
ImuFn setImu_; ImuFn setImu_;
BytesInFn serialInput_;
BytesOutFn serialOutput_; BytesOutFn serialOutput_;
ServosFn getServos_; ServosFn getServos_;
FloatsFn getJoints_, getFeet_; FloatsFn getJoints_, getFeet_;
@@ -142,8 +142,8 @@ namespace HexapodSim
n.start_ = n.Fn<VoidFn>("hxsim_start"); n.start_ = n.Fn<VoidFn>("hxsim_start");
n.step_ = n.Fn<UIntArgFn>("hxsim_step"); n.step_ = n.Fn<UIntArgFn>("hxsim_step");
n.radioSend_ = n.Fn<PacketFn>("hxsim_radio_send"); 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.setImu_ = n.Fn<ImuFn>("hxsim_set_imu");
n.serialInput_ = n.Fn<BytesInFn>("hxsim_serial_input");
n.serialOutput_ = n.Fn<BytesOutFn>("hxsim_serial_output"); n.serialOutput_ = n.Fn<BytesOutFn>("hxsim_serial_output");
n.getServos_ = n.Fn<ServosFn>("hxsim_get_servos"); n.getServos_ = n.Fn<ServosFn>("hxsim_get_servos");
n.getJoints_ = n.Fn<FloatsFn>("hxsim_get_joints_deg"); n.getJoints_ = n.Fn<FloatsFn>("hxsim_get_joints_deg");
@@ -184,16 +184,24 @@ namespace HexapodSim
public void Start() => start_(); public void Start() => start_();
public void Step(uint dtMicros) => step_(dtMicros); public void Step(uint dtMicros) => step_(dtMicros);
public void RadioSend(CmdPacket p) => radioSend_(ref p, Marshal.SizeOf<CmdPacket>()); 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). // Overrides the body-driven IMU with fixed readings from then on (normally unused).
public void SetImu(Vector3 accelG, Vector3 gyroDps) => public void SetImu(Vector3 accelG, Vector3 gyroDps) =>
setImu_(accelG.x, accelG.y, accelG.z, gyroDps.x, gyroDps.y, gyroDps.z); 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]; readonly byte[] serialBuf_ = new byte[4096];
public string SerialOutput() public string SerialOutput()
@@ -81,7 +81,7 @@ namespace HexapodSim
StringBuilder serialLine_ = new StringBuilder(); StringBuilder serialLine_ = new StringBuilder();
static readonly string[] LegacyMovementScripts = 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). // Robot-frame axes, for readability (Unity's Vector3.up etc. are Unity axes).
static readonly Vector3 AxisX = new Vector3(1, 0, 0); // forward static readonly Vector3 AxisX = new Vector3(1, 0, 0); // forward
@@ -1,47 +1,28 @@
using System;
using UnityEngine; using UnityEngine;
namespace HexapodSim namespace HexapodSim
{ {
// Stand-in for the handheld remote. Produces CmdPackets exactly per the // Where the simulated robot's commands come from. Two sources:
// protocol contract and sends them over the simulated radio. Toggles and // Keyboard - a stand-in remote: builds CmdPackets per the protocol contract
// interlocks mirror remote/RemoteState.cpp and remote/PacketBuilder.cpp - // and sends them over the simulated radio. Toggles and
// keep them in sync if those change. // interlocks mirror remote/RemoteState.cpp and
// // remote/PacketBuilder.cpp - keep them in sync if those change.
// Input sources: // RealRemote - the real handheld remote, over the air: the USB nRF24 dongle
// Joystick - the real remote as a USB game controller, read through // (2026-current/dongle/, bridge mode) receives its packets, they
// Unity input axes "1".."6" like the old HandleHoystick. // go into the firmware unchanged, and the robot's Telemetry goes
// Keyboard - test without any remote (keys listed in SimTestPanel). // back as the radio's ack payload. The remote's display then
// RobotSerialHarness - no radio at all; typed keys go to the firmware's own // shows the simulated robot. Nothing about the remote is
// TEST_SERIAL_INPUT harness, like the Arduino Serial Monitor. // mirrored here: its own firmware does all of it.
// Toggles, the speed slider and the handset tilt sliders live in SimTestPanel. // Toggles, the speed slider and the handset tilt sliders live in SimTestPanel.
public class SimRemote : MonoBehaviour public class SimRemote : MonoBehaviour
{ {
public enum InputSource { Joystick, Keyboard, RobotSerialHarness } public enum InputSource { Keyboard, RealRemote }
[Serializable]
public class AxisBinding
{
public string axis;
public bool invert;
public AxisBinding(string axis, bool invert) { this.axis = axis; this.invert = invert; }
}
public InputSource source = InputSource.Keyboard; public InputSource source = InputSource.Keyboard;
// Defaults follow the old HandleHoystick: axes 4/6/5 were the walk stick [Header("Real remote (USB nRF24 dongle)")]
// (b2/b1/b3), 2/1/3 the rotation stick (a1/a2/a3), with 1 and 2 inverted. [Tooltip("\"auto\" finds the dongle's CH340 port; or e.g. COM7.")]
// Which of 4/6 is forward vs sideways wasn't recoverable from the old code: public string donglePort = "auto";
// 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("Operator state (same toggles as the Nextion screen)")] [Header("Operator state (same toggles as the Nextion screen)")]
public bool armed; public bool armed;
@@ -66,49 +47,61 @@ namespace HexapodSim
public float armCenterTolerance = 0.05f; // ARM_CENTER_TOLERANCE / 1000 public float armCenterTolerance = 0.05f; // ARM_CENTER_TOLERANCE / 1000
public float keyStickRatePerSec = 4f; // how fast a held key moves the virtual stick 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[] sticks = new float[6];
public readonly float[] rawAxes = new float[6]; // Input axes "1".."6" as read
public CmdPacket LastPacket { get; private set; } public CmdPacket LastPacket { get; private set; }
public string Message { 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 speedCurrent_ = 128;
float lastWalkInputTime_ = -999f; float lastWalkInputTime_ = -999f;
byte seq_; byte seq_;
string serialKeys_ = "";
float messageUntil_; float messageUntil_;
float lastRemotePacket_ = -999f;
int packetsThisSecond_;
float secondStart_;
readonly byte[] ack_ = new byte[32];
public void ResetState() public void ResetState()
{ {
armed = stand = false; armed = stand = false;
speedCurrent_ = speedTarget; speedCurrent_ = speedTarget;
lastWalkInputTime_ = -999f; lastWalkInputTime_ = -999f;
serialKeys_ = "";
}
void Awake()
{
if (Input.GetJoystickNames().Length > 0 && !string.IsNullOrEmpty(Input.GetJoystickNames()[0]))
source = InputSource.Joystick;
} }
void Update() 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 HandleToggleKeys();
return; ReadKeyboardSticks(Time.deltaTime);
} }
HandleToggleKeys();
if (source == InputSource.Joystick) ReadJoystick();
else ReadKeyboardSticks(Time.deltaTime);
if (Time.time > messageUntil_) Message = ""; 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) ---- // ---- Operator commands (also called by SimTestPanel buttons) ----
public void ToggleArm() public void ToggleArm()
@@ -144,32 +137,6 @@ namespace HexapodSim
if (Input.GetKeyDown(KeyCode.PageDown)) height = Mathf.Max(-1000, height - 100); 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. // WASD + Q/E = left stick (walk, turn), IJKL + U/O = right stick.
void ReadKeyboardSticks(float dt) void ReadKeyboardSticks(float dt)
{ {
@@ -187,17 +154,36 @@ namespace HexapodSim
// Called by HexapodSimulation once per remote TX period. // Called by HexapodSimulation once per remote TX period.
public void Transmit(HexapodSimNative native, float dt) public void Transmit(HexapodSimNative native, float dt)
{ {
if (source == InputSource.RobotSerialHarness) if (source == InputSource.RealRemote) { ForwardRealRemote(native); return; }
{
native.SerialInput(serialKeys_); // no radio: the firmware's harness injects its own packets
serialKeys_ = "";
return;
}
speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt); speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt);
LastPacket = BuildPacket(); LastPacket = BuildPacket();
native.RadioSend(LastPacket); 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); static short Wire(float v) => (short)Mathf.RoundToInt(Mathf.Clamp(v, -1f, 1f) * 1000f);
// Same mapping as remote/PacketBuilder.cpp. // Same mapping as remote/PacketBuilder.cpp.
@@ -208,6 +194,7 @@ namespace HexapodSim
seq = seq_++, seq = seq_++,
speed_scale = (byte)Mathf.RoundToInt(speedCurrent_), speed_scale = (byte)Mathf.RoundToInt(speedCurrent_),
body_height = (short)Mathf.Clamp(height, -1000, 1000), // a setting: sent even while disarmed 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; byte f = 0;
if (armed) f |= CmdPacket.FLAG_ARMED; if (armed) f |= CmdPacket.FLAG_ARMED;
@@ -16,7 +16,7 @@ namespace HexapodSim
Vector2 scroll_; Vector2 scroll_;
GUIStyle mono_; GUIStyle mono_;
static readonly string[] SourceNames = { "Joystick", "Keyboard", "Robot serial" }; static readonly string[] SourceNames = { "Keyboard", "Real remote (radio)" };
void Awake() void Awake()
{ {
@@ -35,8 +35,10 @@ namespace HexapodSim
if (mono_ == null) if (mono_ == null)
mono_ = new GUIStyle(GUI.skin.label) { font = Font.CreateDynamicFontFromOSFont("Consolas", 12), fontSize = 12, wordWrap = false }; 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); GUILayout.BeginArea(new Rect(10, 10, 420, Screen.height - 20), GUI.skin.box);
scroll_ = GUILayout.BeginScrollView(scroll_); // 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 }); GUILayout.Label("<b>Hexapod firmware simulation</b> (F1 hides)", new GUIStyle(GUI.skin.label) { richText = true });
if (sim_.LoadError != null) if (sim_.LoadError != null)
@@ -65,18 +67,13 @@ namespace HexapodSim
// ---- Input source ---- // ---- Input source ----
GUILayout.Space(6); GUILayout.Space(6);
remote_.source = (SimRemote.InputSource)GUILayout.Toolbar((int)remote_.source, SourceNames); 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" + DongleSection();
"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_);
} }
else 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_);
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.");
// ---- Toggles: same set as the remote's touchscreen ---- // ---- Toggles: same set as the remote's touchscreen ----
GUILayout.BeginHorizontal(); GUILayout.BeginHorizontal();
@@ -116,11 +113,10 @@ namespace HexapodSim
// ---- Live values ---- // ---- Live values ----
GUILayout.Space(6); GUILayout.Space(6);
var st = remote_.sticks; var st = remote_.sticks;
var raw = remote_.rawAxes;
var p = remote_.LastPacket; var p = remote_.LastPacket;
GUILayout.Label( 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" + (remote_.source == SimRemote.InputSource.Keyboard
$"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" + ? $"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" + $"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" + $" 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')}", $" 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(); 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) static string StanceString(int mask)
{ {
var sb = new StringBuilder(6); var sb = new StringBuilder(6);
@@ -1,10 +0,0 @@
fileFormatVersion: 2
guid: 5f676c461da1c254990d7719d77e6250
folderAsset: yes
timeCreated: 1641405303
licenseType: Free
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:
@@ -1,72 +0,0 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using UnityEngine;
namespace Assets.JoyStick
{
class HandleHoystick : MonoBehaviour
{
public RotationControl bot;
public Walk walk;
public void Update()
{
float a2 = Remap(Input.GetAxis("1"), 1, -127, -1, 127);
float a1 = Remap(Input.GetAxis("2"), 1, -127, -1, 127);
float a3 = Remap(Input.GetAxis("3"), -1, -127, 1, 127);
float b2 = Remap(Input.GetAxis("4"), -1, -127, 1, 127);
float b3 = Remap(Input.GetAxis("5"), -1, -127, 1, 127);
float b1 = Remap(Input.GetAxis("6"), -1, -127, 1, 127);
//Debug.Log(Input.GetAxis("4"));
//Debug.Log(Remap(Input.GetAxis("4"), -1, -127, 1, 127));
bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
1);
float speed = (float)(Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
walk.walkspeed = speed < 0.5f ? 0 : speed;
float rotation = -1 * limit(b3 / 4, -5, 5);
walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
walk.walkDirection = (-1 * ConvertRadiansToDegrees((float)Math.Atan2(b2, b1)));
float rotationabs = Math.Abs(walk.centerRotation);
float speed2 = ((6 / 3) * walk.walkspeed) + ((6 / 5) * rotationabs);
speed2 = speed2 < 6 ? speed2 : 6;
walk.speed2 = (int)(10 - speed2);
}
public float limit(float input, float min, float max)
{
input = input / 6;
return (input < max & input > min) ? input : input < min ? min : max;
}
public float ConvertRadiansToDegrees(float radians)
{
float degrees = (float)((180 / Math.PI) * radians);
return (degrees);
}
public float Remap(float from, float fromMin, float toMin, float fromMax, float toMax)
{
var fromAbs = from - fromMin;
var fromMaxAbs = fromMax - fromMin;
var normal = fromAbs / fromMaxAbs;
var toMaxAbs = toMax - toMin;
var toAbs = toMaxAbs * normal;
var to = toAbs + toMin;
return to;
}
}
}
@@ -1,13 +0,0 @@
fileFormatVersion: 2
guid: c5ce8a2605d301e44a4a210aa345ed62
timeCreated: 1641405303
licenseType: Free
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:
@@ -79,11 +79,8 @@ public class Leg : MonoBehaviour {
// (int)Remap((float)a1, -90, 90, 2500, 500), // (int)Remap((float)a1, -90, 90, 2500, 500),
// (int)Remap((float)a2, -90, 90, 2500, 500), // 0 is middel // (int)Remap((float)a2, -90, 90, 2500, 500), // 0 is middel
// (int)Remap((float)a3, 90, -90, 2500, 500)); // 0 is middel // (int)Remap((float)a3, 90, -90, 2500, 500)); // 0 is middel
if(false) // (servo angles used to go out over USB serial here; dropped 2026-11-17,
Serial.Instance.send(startServo, // the 2026 robot runs its own firmware and takes commands over the radio)
(int)Remap((float)a1, -90, 90, 0, 180),
(int)Remap((float)a2, 90, -90, 0, 180), // 0 is middel
(int)Remap((float)a3, -90, 90, 0, 180)); // 0 is middel
} }
else if (invertForLeftLeg && startServo != -1) else if (invertForLeftLeg && startServo != -1)
{ {
@@ -95,11 +92,6 @@ public class Leg : MonoBehaviour {
// (int)Remap((float)a1, -90, 90, 500, 2500), // (int)Remap((float)a1, -90, 90, 500, 2500),
// (int)Remap((float)a2, -90, 90, 500, 2500), // 0 is middel // (int)Remap((float)a2, -90, 90, 500, 2500), // 0 is middel
// (int)Remap((float)a3, 90, -90, 500, 2500)); // 0 is middel // (int)Remap((float)a3, 90, -90, 500, 2500)); // 0 is middel
if (false)
Serial.Instance.send(startServo,
(int)Remap((float)a1, 90, -90, 0, 180),
(int)Remap((float)a2, -90, 90, 0, 180), // 0 is middel
(int)Remap((float)a3, 90, -90, 0, 180)); // 0 is middel
} }
Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0); Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0);
@@ -12875,7 +12875,6 @@ GameObject:
serializedVersion: 6 serializedVersion: 6
m_Component: m_Component:
- component: {fileID: 73925799} - component: {fileID: 73925799}
- component: {fileID: 73925800}
m_Layer: 0 m_Layer: 0
m_Name: SerialIn m_Name: SerialIn
m_TagString: Untagged m_TagString: Untagged
@@ -12898,28 +12897,6 @@ Transform:
m_Children: [] m_Children: []
m_Father: {fileID: 0} m_Father: {fileID: 0}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!114 &73925800
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 73925798}
m_Enabled: 0
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 7fa025fa8dcb3a249b67fc0e2fe67b44, type: 3}
m_Name:
m_EditorClassIdentifier:
bot: {fileID: 311513962}
walk: {fileID: 311513965}
a1: 0
a2: 0
a3: 0
b1: 0
b2: 0
b3: 0
enabled: 0
enabled_send: 0
--- !u!1 &74230589 --- !u!1 &74230589
GameObject: GameObject:
m_ObjectHideFlags: 0 m_ObjectHideFlags: 0
@@ -325049,7 +325026,6 @@ GameObject:
serializedVersion: 6 serializedVersion: 6
m_Component: m_Component:
- component: {fileID: 1645933599} - component: {fileID: 1645933599}
- component: {fileID: 1645933598}
m_Layer: 0 m_Layer: 0
m_Name: Input m_Name: Input
m_TagString: Untagged m_TagString: Untagged
@@ -325057,20 +325033,6 @@ GameObject:
m_NavMeshLayer: 0 m_NavMeshLayer: 0
m_StaticEditorFlags: 0 m_StaticEditorFlags: 0
m_IsActive: 1 m_IsActive: 1
--- !u!114 &1645933598
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 1645933597}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: c5ce8a2605d301e44a4a210aa345ed62, type: 3}
m_Name:
m_EditorClassIdentifier:
bot: {fileID: 311513962}
walk: {fileID: 311513965}
--- !u!4 &1645933599 --- !u!4 &1645933599
Transform: Transform:
m_ObjectHideFlags: 0 m_ObjectHideFlags: 0
@@ -1,10 +0,0 @@
fileFormatVersion: 2
guid: 942facdab943ceb4ca29453f4ee4d1b3
folderAsset: yes
timeCreated: 1641404792
licenseType: Free
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:
@@ -1,119 +0,0 @@
using System;
using UnityEngine;
using System.Collections;
using System.IO.Ports;
using Random = System.Random;
public class Serial
{
private static Serial instance;
private int rnd = 0;
private Serial()
{
}
~Serial()
{
serialPort.Close();
serialPort.Dispose();
}
public static Serial Instance
{
get
{
if (instance == null)
{
instance = new Serial();
}
return instance;
}
}
public bool enabled = false;
public SerialPort serialPort;
public int[] values = new int[18];
public static string ByteArrayToString(byte[] ba)
{
return BitConverter.ToString(ba).Replace("-", " ");
}
public void send(int start, int a, int b, int c)
{
if (serialPort == null)
{
serialPort = new SerialPort(@"\\.\" + "COM4", 250000);
try
{
serialPort.Parity = Parity.Even;
serialPort.StopBits = StopBits.One;
serialPort.DataBits = 8;
serialPort.Open();
serialPort.DiscardOutBuffer();
serialPort.DiscardInBuffer();
System.Threading.Thread.Sleep(1000);
enabled = true;
}
catch
{
enabled = false;
}
}
if (enabled)
{
values[start] = a;
values[++start] = b;
values[++start] = c;
if (serialPort is SerialPort && start == 17)
{
// string send = "s";
byte[] send = new byte[18+1+2];
send[18] = 0x00;
send[19] = 0xfe;
send[20] = 0xff;
// send[21] = 0xff;
// if (rnd == 0)
// {
//rnd = 1;
for (int i = 0; i < 18; i++)
{
//string crc = ((i) * (values[i])) + "";
//send = send + (i).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
byte[] intBytes = BitConverter.GetBytes(values[i]);
if (BitConverter.IsLittleEndian)
Array.Reverse(intBytes);
send[i] = intBytes[3];
send[18] += send[i];
}
// Debug.Log(ByteArrayToString(send));
serialPort.Write(send, 0, 18+1+2);
serialPort.BaseStream.Flush();
// }
// else
// {
// rnd = 0;
// for (int i = 10; i < 18; i++)
// {
// string crc = ((i + 3) * (values[i])) + "";
//
// send = send + (i + 2).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
// }
//
// Debug.Log(send);
// serialPort.Write(send);
// }
}
}
}
}
@@ -1,13 +0,0 @@
fileFormatVersion: 2
guid: c99b6d9374ff6d3419c7692fdf3c1791
timeCreated: 1542057315
licenseType: Free
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:
@@ -1,70 +0,0 @@
/*using System;
using UnityEngine;
using System.Collections;
using System.IO.Ports;
using System.Linq;
public class Serial2 : MonoBehaviour{
public float x;
public float y;
public RotationControl bot;
~Serial2() {
serialPort.Close();
}
public bool enabled = false;
SerialPort serialPort;
public void Update()
{
if (serialPort == null)
{
serialPort = new SerialPort(@"\\.\" + "COM7", 115200);
try
{
serialPort.NewLine = "\n";
serialPort.Open();
serialPort.DiscardOutBuffer();
serialPort.DiscardInBuffer();
enabled = true;
}
catch
{
enabled = false;
}
}
if (enabled)
{
if (serialPort is SerialPort)
{
string received = serialPort.ReadLine();
string[] data = received.Split(',');
x = ConvertRadiansToDegrees(float.Parse(data.First()));
y = ConvertRadiansToDegrees(float.Parse(data.Last()));
bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
}
}
}
public float limit(float input, float min, float max)
{
return (input < max & input > min) ? input : input < min ? min : max;
}
public float ConvertRadiansToDegrees(float radians)
{
float degrees = (float) ((180 / Math.PI) * radians);
return (degrees);
}
}
*/
@@ -1,13 +0,0 @@
fileFormatVersion: 2
guid: ac6468bb4d2734947a923ea42d6d8192
timeCreated: 1557262171
licenseType: Free
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:
@@ -1,170 +0,0 @@
//using System;
//using UnityEngine;
//using System.Collections;
//using System.IO.Ports;
//using System.Linq;
//using UnityEngine.UI;
//public class Serial3 : MonoBehaviour{
// public RotationControl bot;
// public Walk walk;
// public float a1, a2, a3, b1, b2, b3;
// // ~Serial3()
// // {
// // serialPort.Close();
// // serialPort.Dispose();
// // }
// public bool enabled = false;
// public bool enabled_send = false;
// SerialPort serialPort;
// public void Update()
// {
// if (serialPort == null)
// {
// serialPort = new SerialPort(@"\\.\" + "COM7", 230400);
// try
// {
// serialPort.NewLine = "\r\n";
//// serialPort.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
// serialPort.Open();
// serialPort.DiscardOutBuffer();
// serialPort.DiscardInBuffer();
// enabled = true;
// }
// catch
// {
// enabled = false;
// }
// }
// if (enabled)
// {
// serialPort = Serial.Instance.serialPort;
// Serial.Instance.enabled = enabled_send;
// if (serialPort is SerialPort)
// {
// string received = serialPort.ReadLine();
// string[] data = received.Split(',');
// foreach (string d in data)
// {
// string[] ch = d.Split('_');
// int channel = int.Parse(ch.First());
// float value = float.Parse(ch.Last());
// switch (channel)
// {
// case 0:
// a1 = value - 127;
// break;
// case 1:
// a2 = value - 127;
// break;
// case 2:
// a3 = value - 127;
// break;
// case 3:
// b1 = value - 127;
// break;
// case 4:
// b2 = value - 127;
// break;
// case 5:
// b3 = value - 127;
// break;
// }
// bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
// 1);
// float speed = (float) (Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
// walk.walkspeed = speed < 0.5f ? 0 : speed;
// float rotation = -1 * limit(b3/4, -5, 5);
// walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
// walk.walkDirection = (-1 * ConvertRadiansToDegrees((float) Math.Atan2(b2, b1)));
// float rotationabs = Math.Abs(walk.centerRotation);
// float speed2 = ((6 / 3) * walk.walkspeed) + ((6/5) * rotationabs);
// speed2 = speed2 < 6 ? speed2 : 6;
// walk.speed2 = (int)(10 - speed2);
// }
// }
// }
// }
// private void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
// {
// SerialPort serialPort = (SerialPort)sender;
// if (enabled)
// {
// if (serialPort is SerialPort)
// {
// string received = serialPort.ReadLine();
// string[] data = received.Split(',');
// foreach (string d in data)
// {
// // if (data == "\r")
// // continue;
// string[] ch = d.Split('_');
// int channel = int.Parse(ch.First());
// float value = float.Parse(ch.Last());
// switch (channel)
// {
// case 0:
// a1 = value;
// break;
// case 1:
// a2 = value;
// break;
// case 2:
// a3 = value;
// break;
// case 3:
// b1 = value;
// break;
// case 4:
// b2 = value;
// break;
// case 5:
// b3 = value;
// break;
// }
// }
// // bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
// }
// }
// }
// public float limit(float input, float min, float max)
// {
// input = input / 6;
// return (input < max & input > min) ? input : input < min ? min : max;
// }
// public float ConvertRadiansToDegrees(float radians)
// {
// float degrees = (float)((180 / Math.PI) * radians);
// return (degrees);
// }
//}
@@ -1,13 +0,0 @@
fileFormatVersion: 2
guid: 7fa025fa8dcb3a249b67fc0e2fe67b44
timeCreated: 1558207463
licenseType: Free
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:
+36 -4
View File
@@ -155,7 +155,9 @@ python tools/safeboot_flash.py firmware/build/app.bin
``` ```
This resets the running app into safeboot and rewrites only the app region. If This resets the running app into safeboot and rewrites only the app region. If
the app is dead, replug the stick and re-run — it will catch the knock window. the app is dead or doesn't answer, start it with `--knock` and then replug the
stick: it keeps knocking (reopening the port as it comes back) until it lands in
safeboot's 1.5 s cold-boot window.
## Air settings ## Air settings
@@ -168,6 +170,36 @@ the Si24R1 over SPI (same register layout as the nRF24L01+).
- [x] identify the hardware, map the radio + serial pins - [x] identify the hardware, map the radio + serial pins
- [x] permanent two-stage firmware, wire-free USB flashing - [x] permanent two-stage firmware, wire-free USB flashing
- [ ] identify the LED and AT2401C TX/RX-enable pins (app toggles the 5 free pins) - [ ] identify the LED and AT2401C TX/RX-enable pins (app toggles the 5 free pins)
- [ ] radio hub app: carry `CmdPacket` both ways, per-node addressing, telemetry - [x] bridge mode: the dongle stands in for the robot's radio, Unity runs the
- [ ] Unity serial bridge (PC as hub when online, robot as hub when not) — see robot firmware (sim) behind it — see "Bridge mode" below
`../README.md` "Planned" - [ ] the other direction: Unity as the remote, driving the real robot
- [ ] AT2401C pins: at desk range and even a few rooms away with the remote at
minimum power, every on/off combination of PA1/PB1/PF0/PF1 received
every packet, so they couldn't be told apart; the link works without
driving them
## Bridge mode (Unity ↔ real remote)
The app listens on the hexapod's air settings as if it were the robot
(ch 96, 250 kbps, `HXPD1`, auto-ack, dynamic payload lengths, ack payloads)
and passes everything to the PC. Framing both ways over the USB serial
(115200 8N1):
```
A5 type len payload[len] sum sum = type + len + payload bytes (mod 256)
```
| Direction | Type | Payload |
|-----------|------|---------|
| PC → dongle | `L` | — : start bridging (LED on) |
| PC → dongle | `X` | — : stop |
| PC → dongle | `A` | ≤ 32 bytes: the robot's `Telemetry`, sent back on the auto-ack of the remote's next packet (reloaded after every packet, like `robot/Comms.cpp`). Dropped if not refreshed for ~0.5 s, so the remote never shows stale telemetry as live |
| dongle → PC | `R` | a packet the remote sent (its `CmdPacket`, any length; the firmware checks it) |
| dongle → PC | `S` | once a "second": packets received (u16 LE), ack payloads loaded (u16 LE). Its second runs ~30% long (blocking UART sends miss SysTick ticks); it's only for display |
Plain text never contains `A5`, so the PC skips anything between frames;
while bridging the app prints no text and only takes frames (and `b` for a
reflash). USB serial input goes through DMA into a ring buffer (no
interrupts in this app), and the USART's overrun stall is disabled, so a
burst can't deafen it. Unity's side is
`Hexapod_Simulation_Unity/Assets/HexapodSim/DongleLink.cs`.
+163 -14
View File
@@ -52,11 +52,14 @@ static void nrf_write_buf(uint8_t reg, const uint8_t *d, int n)
} }
static void nrf_cmd(uint8_t c) { CSN_LO(); spi_xfer(c); CSN_HI(); } static void nrf_cmd(uint8_t c) { CSN_LO(); spi_xfer(c); CSN_HI(); }
/* Receive on the hexapod protocol (ch 96, 250 kbps, addr HXPD1, 22-byte). Lets /* Receive on the hexapod protocol (ch 96, 250 kbps, addr HXPD1). Protocol v4
* us find the AT2401C RX-enable pin: packets only arrive when the LNA/antenna * uses dynamic payload lengths (the remote's CmdPacket is 26 bytes, with ack
* path is on. */ * payloads), so this does too and accepts any length. Auto-ack is on, so the
* remote's link % also shows whether our acks get back out (TX path). Lets us
* find the AT2401C enable pins: packets only arrive when the LNA path is on. */
static uint8_t rxmode; static uint8_t rxmode;
static uint16_t rx_count; static uint16_t rx_count;
static uint8_t rx_len;
static void rx_setup(void) static void rx_setup(void)
{ {
static const uint8_t addr[5] = { 0x48, 0x58, 0x50, 0x44, 0x31 }; /* "HXPD1" */ static const uint8_t addr[5] = { 0x48, 0x58, 0x50, 0x44, 0x31 }; /* "HXPD1" */
@@ -68,18 +71,22 @@ static void rx_setup(void)
nrf_write(0x05, 96); /* RF_CH */ nrf_write(0x05, 96); /* RF_CH */
nrf_write(0x06, 0x26); /* RF_SETUP: 250 kbps, max power */ nrf_write(0x06, 0x26); /* RF_SETUP: 250 kbps, max power */
nrf_write_buf(0x0A, addr, 5); /* RX_ADDR_P0 */ nrf_write_buf(0x0A, addr, 5); /* RX_ADDR_P0 */
nrf_write(0x11, 22); /* RX_PW_P0 */ nrf_write(0x1D, 0x04); /* FEATURE: EN_DPL */
nrf_write(0x1C, 0x01); /* DYNPD: pipe0 */
nrf_write(0x07, 0x70); /* clear status flags */ nrf_write(0x07, 0x70); /* clear status flags */
nrf_cmd(0xE2); /* FLUSH_RX */ nrf_cmd(0xE2); /* FLUSH_RX */
rx_count = 0; rx_count = 0;
rx_len = 0;
CE_HI(); CE_HI();
} }
static void rx_poll(void) static void rx_poll(void)
{ {
if (nrf_read(0x07) & 0x40) { /* RX_DR */ while (nrf_read(0x07) & 0x40) { /* RX_DR */
uint8_t buf[22]; CSN_LO(); spi_xfer(0x60); uint8_t n = spi_xfer(0xFF); CSN_HI(); /* R_RX_PL_WID */
CSN_LO(); spi_xfer(0x61); for (int i = 0; i < 22; i++) buf[i] = spi_xfer(0xFF); CSN_HI(); if (n > 32) { nrf_cmd(0xE2); nrf_write(0x07, 0x40); break; } /* corrupt: flush */
CSN_LO(); spi_xfer(0x61); for (int i = 0; i < n; i++) spi_xfer(0xFF); CSN_HI();
nrf_write(0x07, 0x40); /* clear RX_DR */ nrf_write(0x07, 0x40); /* clear RX_DR */
rx_len = n;
rx_count++; rx_count++;
} }
} }
@@ -132,7 +139,7 @@ static void banner(void)
uart_puts("\r\nhexapod dongle app v0 (@0x08001000)\r\n"); uart_puts("\r\nhexapod dongle app v0 (@0x08001000)\r\n");
uart_puts("nRF CONFIG=0x"); hex2(nrf_read(0)); uart_puts(" STATUS=0x"); hex2(nrf_read(7)); uart_puts("nRF CONFIG=0x"); hex2(nrf_read(0)); uart_puts(" STATUS=0x"); hex2(nrf_read(7));
uart_puts(" RF_CH=0x"); hex2(nrf_read(5)); uart_puts(" RF_SETUP=0x"); hex2(nrf_read(6)); uart_puts(" RF_CH=0x"); hex2(nrf_read(5)); uart_puts(" RF_SETUP=0x"); hex2(nrf_read(6));
uart_puts("\r\ncmds: v r 0-4(PA0/PA1/PB1/PF0/PF1) b=reflash\r\n"); uart_puts("\r\ncmds: v r 0-4(PA0/PA1/PB1/PF0/PF1) x=rx test b=reflash; bridge: frame A5 'L'\r\n");
} }
static void regdump(void) static void regdump(void)
{ {
@@ -156,6 +163,134 @@ static void finder_rx(void)
delay_ms(2); delay_ms(2);
} }
/* ---- USART1 RX through circular DMA ------------------------------------
* The bridge moves ~100 frames/s each way at 115200 baud. Polled single-byte
* RX loses bytes while we block on a TX frame or an SPI read, and this M0 app
* runs without interrupts (no vector relocation), so DMA channel 3 (USART1_RX)
* writes every byte into a ring we drain at leisure. */
#define DMA_CCR3 REG(0x40020030)
#define DMA_CNDTR3 REG(0x40020034)
#define DMA_CPAR3 REG(0x40020038)
#define DMA_CMAR3 REG(0x4002003C)
#define USART1_CR3 REG(0x40013808)
#define RXRING 256u
static volatile uint8_t rxring[RXRING];
static uint16_t rxtail;
static void uart_dma_init(void)
{
RCC_AHBENR |= 1u; /* DMA1 */
while (!(USART1_ISR & (1u << 6))) {} /* let any TX finish */
USART1_CR1 &= ~1u; /* UE off: CR3 OVRDIS needs it */
USART1_CR3 = (1u << 6) | (1u << 12); /* DMAR, OVRDIS (never stall on overrun) */
USART1_CR1 |= 1u;
DMA_CCR3 = 0;
DMA_CPAR3 = 0x40013824u; /* USART1_RDR */
DMA_CMAR3 = (uint32_t)rxring;
DMA_CNDTR3 = RXRING;
DMA_CCR3 = (1u << 7) | (1u << 5) | 1u; /* MINC, CIRC, EN; 8-bit, periph -> mem */
}
static int rx_getc(void)
{
uint16_t head = (uint16_t)(RXRING - DMA_CNDTR3) % RXRING;
if (head == rxtail) return -1;
uint8_t c = rxring[rxtail];
rxtail = (uint16_t)((rxtail + 1) % RXRING);
return c;
}
/* ---- Bridge: the dongle stands in for the robot's radio ------------------
* Framing both ways: A5 type len payload[len] sum, sum = type+len+payload (mod 256).
* host -> dongle 'L' listen (enter bridge) 'X' leave
* 'A' ack payload (<= 32 B): goes back on the auto-ack of the
* next packet, like the robot's Comms::loadAckPayload.
* Dropped if the host stops refreshing it for 500 ms, so
* the remote doesn't see stale telemetry as live.
* dongle -> host 'R' a received packet (the remote's CmdPacket)
* 'S' once a second: packets received (u16 LE), acks loaded (u16 LE)
* Plain text never contains A5, so the host can skip anything between frames. */
static uint8_t bridge;
static uint8_t ack_buf[32], ack_len;
static uint32_t ack_ms;
static uint16_t br_rx, br_acks;
static void frame_send(uint8_t type, const uint8_t *d, uint8_t n)
{
uint8_t sum = (uint8_t)(type + n);
uart_putc((char)0xA5); uart_putc((char)type); uart_putc((char)n);
for (uint8_t i = 0; i < n; i++) { uart_putc((char)d[i]); sum = (uint8_t)(sum + d[i]); }
uart_putc((char)sum);
}
static void ack_load(void)
{
nrf_cmd(0xE1); /* FLUSH_TX: only the newest one */
if (!ack_len) return;
CSN_LO(); spi_xfer(0xA8); for (uint8_t i = 0; i < ack_len; i++) spi_xfer(ack_buf[i]); CSN_HI(); /* W_ACK_PAYLOAD pipe 0 */
br_acks++;
}
static void bridge_start(void)
{
rx_setup(); /* ch96 / 250k / HXPD1 / auto-ack / dynamic length */
CE_LO();
nrf_write(0x1D, 0x06); /* FEATURE: EN_DPL | EN_ACK_PAY */
nrf_write(0x1C, 0x01); /* DYNPD pipe 0 */
nrf_cmd(0xE1);
ack_len = 0; br_rx = br_acks = 0;
rxmode = 0; finder = 0; bridge = 1;
BSRR(GPIOA) = 1u; /* LED on while bridging */
CE_HI();
}
static void bridge_stop(void)
{
bridge = 0; ack_len = 0;
CE_LO(); nrf_write(0, 0x0E); nrf_cmd(0xE1);
BSRR(GPIOA) = 1u << 16;
}
static void bridge_poll(uint32_t now)
{
if (ack_len && now - ack_ms > 500) { ack_len = 0; nrf_cmd(0xE1); } /* host went quiet */
while (nrf_read(0x07) & 0x40) { /* RX_DR */
uint8_t buf[32];
CSN_LO(); spi_xfer(0x60); uint8_t n = spi_xfer(0xFF); CSN_HI(); /* R_RX_PL_WID */
if (n > 32) { nrf_cmd(0xE2); nrf_write(0x07, 0x40); break; }
CSN_LO(); spi_xfer(0x61); for (uint8_t i = 0; i < n; i++) buf[i] = spi_xfer(0xFF); CSN_HI();
nrf_write(0x07, 0x40);
ack_load(); /* the ack for the NEXT packet */
frame_send('R', buf, n);
br_rx++;
if ((br_rx & 31) == 0) BSRR(GPIOA) = (IDR(GPIOA) & 1u) ? (1u << 16) : 1u; /* LED flicker */
}
}
/* Host frame parser; returns 1 while inside a frame (so its bytes aren't
* taken as single-letter commands). */
static uint8_t fr_state, fr_type, fr_len, fr_got, fr_sum, fr_buf[32];
static int frame_rx(int c, uint32_t now)
{
/* if-chain, not switch: a switch becomes a jump table needing libgcc */
if (fr_state == 0) {
if (c != 0xA5) return 0;
fr_state = 1;
} else if (fr_state == 1) {
fr_type = (uint8_t)c;
fr_state = 2;
} else if (fr_state == 2) {
fr_len = (uint8_t)c; fr_got = 0; fr_sum = (uint8_t)(fr_type + fr_len);
fr_state = (fr_len > 32) ? 0 : (fr_len ? 3 : 4);
} else if (fr_state == 3) {
fr_buf[fr_got++] = (uint8_t)c; fr_sum = (uint8_t)(fr_sum + c);
if (fr_got == fr_len) fr_state = 4;
} else {
fr_state = 0;
if ((uint8_t)c != fr_sum) return 1; /* corrupt: drop */
if (fr_type == 'L') bridge_start();
else if (fr_type == 'X') bridge_stop();
else if (fr_type == 'A' && bridge) {
for (uint8_t i = 0; i < fr_len; i++) ack_buf[i] = fr_buf[i];
ack_len = fr_len; ack_ms = now;
ack_load();
}
}
return 1;
}
static void reflash(void) /* ask safeboot to take over, no wire needed */ static void reflash(void) /* ask safeboot to take over, no wire needed */
{ {
uart_puts("resetting into safeboot flash mode...\r\n"); uart_puts("resetting into safeboot flash mode...\r\n");
@@ -168,16 +303,25 @@ static void reflash(void) /* ask safeboot to take over, no wire needed */
int main(void) int main(void)
{ {
uart_init8n1(); uart_init8n1();
uart_dma_init();
spi_init(); spi_init();
free_init(); free_init();
nrf_write(0, 0x0E); nrf_write(0, 0x0E);
delay_ms(5); delay_ms(5);
banner(); banner();
uint32_t t = 0; uint32_t now = 0, t = 0;
systick_setup();
for (;;) { for (;;) {
int c = uart_getc(); uint32_t ctrl = STK_CTRL; /* one read: reading clears COUNTFLAG */
if (c == 'v' || c == 'V') banner(); if (ctrl == 0) systick_setup(); /* delay_ms() (sweep, finder) stops it */
else if (ctrl & 0x10000) { now++; t++; } /* 1 ms ticks, polled */
if (bridge) bridge_poll(now);
int c = rx_getc();
if (c >= 0 && frame_rx(c, now)) c = -1; /* consumed by a host frame */
if (c < 0) {}
else if (bridge && c != 'b' && c != 'B') {} /* bridging: only frames (and reflash) */
else if (c == 'v' || c == 'V') banner();
else if (c == 'r' || c == 'R') regdump(); else if (c == 'r' || c == 'R') regdump();
else if (c == 'b' || c == 'B') reflash(); else if (c == 'b' || c == 'B') reflash();
else if (c == 'x' || c == 'X') { rxmode = !rxmode; finder = 0; if (rxmode) rx_setup(); else { CE_LO(); nrf_write(0, 0x0E); } uart_puts(rxmode ? "RX mode on (ch96/250k/HXPD1)\r\n" : "RX off\r\n"); } else if (c == 'x' || c == 'X') { rxmode = !rxmode; finder = 0; if (rxmode) rx_setup(); else { CE_LO(); nrf_write(0, 0x0E); } uart_puts(rxmode ? "RX mode on (ch96/250k/HXPD1)\r\n" : "RX off\r\n"); }
@@ -193,10 +337,15 @@ int main(void)
uart_puts("\r\n"); uart_puts("\r\n");
} }
if (rxmode) rx_poll(); if (rxmode) rx_poll();
delay_ms(1); if (t >= 1000) {
if (++t >= 1000) {
t = 0; t = 0;
if (rxmode) { uart_puts("RX pkts="); hex2(rx_count >> 8); hex2(rx_count & 0xFF); if (bridge) {
uint8_t s[4] = { (uint8_t)br_rx, (uint8_t)(br_rx >> 8), (uint8_t)br_acks, (uint8_t)(br_acks >> 8) };
frame_send('S', s, 4);
br_rx = br_acks = 0;
}
else if (rxmode) { uart_puts("RX pkts="); hex2(rx_count >> 8); hex2(rx_count & 0xFF);
uart_puts(" len="); hex2(rx_len);
uart_puts(" pins="); for (int i = 0; i < NFREE; i++) uart_putc(fst[i] ? '1' : '0'); uart_puts(" pins="); for (int i = 0; i < NFREE; i++) uart_putc(fst[i] ? '1' : '0');
uart_puts(" (0=PA0LED,1=PA1,2=PB1,3=PF0,4=PF1)\r\n"); rx_count = 0; } uart_puts(" (0=PA0LED,1=PA1,2=PB1,3=PF0,4=PF1)\r\n"); rx_count = 0; }
else if (finder) { uart_puts("finder ch=0x"); hex2(finder_ch); uart_puts(" RPD="); hex2(nrf_read(9)); else if (finder) { uart_puts("finder ch=0x"); hex2(finder_ch); uart_puts(" RPD="); hex2(nrf_read(9));
+10 -1
View File
@@ -36,6 +36,7 @@
#define USART1_CR1 REG(0x40013800) #define USART1_CR1 REG(0x40013800)
#define USART1_BRR REG(0x4001380C) #define USART1_BRR REG(0x4001380C)
#define USART1_ISR REG(0x4001381C) #define USART1_ISR REG(0x4001381C)
#define USART1_ICR REG(0x40013820)
#define USART1_RDR REG(0x40013824) #define USART1_RDR REG(0x40013824)
#define USART1_TDR REG(0x40013828) #define USART1_TDR REG(0x40013828)
/* SysTick */ /* SysTick */
@@ -65,14 +66,22 @@ static inline void uart_putc(char c)
USART1_TDR = (uint8_t)c; USART1_TDR = (uint8_t)c;
} }
static inline void uart_puts(const char *s) { while (*s) uart_putc(*s++); } static inline void uart_puts(const char *s) { while (*s) uart_putc(*s++); }
/* Clear overrun / noise / framing / parity errors. While ORE is set the USART
* stops moving new bytes into RDR, so one burst the app didn't read in time
* (e.g. SignalRGB probing the COM port) used to deafen it until a replug. */
static inline void uart_clear_errors(void)
{
if (USART1_ISR & 0x0Fu) USART1_ICR = 0x0Fu; /* ORECF NCF FECF PECF */
}
static inline int uart_getc(void) /* non-blocking, -1 if none */ static inline int uart_getc(void) /* non-blocking, -1 if none */
{ {
uart_clear_errors();
if (USART1_ISR & (1u << 5)) return (uint8_t)USART1_RDR; if (USART1_ISR & (1u << 5)) return (uint8_t)USART1_RDR;
return -1; return -1;
} }
static inline uint8_t uart_getc_block(void) static inline uint8_t uart_getc_block(void)
{ {
while (!(USART1_ISR & (1u << 5))) {} while (!(USART1_ISR & (1u << 5))) uart_clear_errors();
return (uint8_t)USART1_RDR; return (uint8_t)USART1_RDR;
} }
static inline void systick_setup(void) { STK_LOAD = 8000 - 1; STK_VAL = 0; STK_CTRL = 5; } static inline void systick_setup(void) { STK_LOAD = 8000 - 1; STK_VAL = 0; STK_CTRL = 5; }
+45 -6
View File
@@ -2,12 +2,14 @@
No BOOT0 wire, no ST-Link, no ROM bootloader. Writes only the app region No BOOT0 wire, no ST-Link, no ROM bootloader. Writes only the app region
(pages 4-15); safeboot itself is never touched. (pages 4-15); safeboot itself is never touched.
usage: safeboot_flash.py app.bin [--port COM16] usage: safeboot_flash.py app.bin [--port COM16] [--knock]
How it enters flash mode: How it enters flash mode:
1) if the app is running, 'b' makes it soft-reset into safeboot flash mode; 1) if the app is running, 'b' makes it soft-reset into safeboot flash mode;
2) otherwise (dead/absent app) replug the dongle and this spams 'S' to catch 2) otherwise (dead/absent app) replug the dongle and this spams 'S' to catch
safeboot's cold-boot knock window. safeboot's cold-boot knock window.
--knock skips 1) and keeps knocking for 2 minutes, reopening the port as it
comes and goes: start it, then replug. Use it when the app doesn't answer.
""" """
import sys, time, serial import sys, time, serial
@@ -54,6 +56,36 @@ def enter_flash(s):
return False return False
def knock_until_caught(port, timeout=120.0):
"""--knock: for an app that doesn't answer 'b' (dead or deaf). Keeps the
port open and spams 'S' for up to `timeout` s, reopening whenever the port
vanishes, so replugging the dongle at any moment lands in safeboot's 1.5 s
cold-boot knock window. Returns the open port in flash mode, or None."""
t = time.time()
s = None
while time.time() - t < timeout:
try:
if s is None:
s = serial.Serial(port, 115200, timeout=0.02)
s.write(b"S")
r = s.read(16)
if b"K" in r or b"SB1" in r or b"SBR" in r:
time.sleep(0.1); s.reset_input_buffer(); s.timeout = 0.3
s.write(b"V"); time.sleep(0.1)
if b"SBV" in s.read(16):
return s
s.timeout = 0.02
except (serial.SerialException, OSError):
try:
if s is not None:
s.close()
except Exception:
pass
s = None
time.sleep(0.02)
return None
def expect(s, ch, what): def expect(s, ch, what):
r = s.read(1) r = s.read(1)
if r != ch: if r != ch:
@@ -68,11 +100,18 @@ def main():
data += b"\xFF" data += b"\xFF"
print("app %s: %d bytes -> 0x%08X" % (args[0], len(data), APP_BASE)) print("app %s: %d bytes -> 0x%08X" % (args[0], len(data), APP_BASE))
s = open_port(port) if "--knock" in sys.argv:
print("entering safeboot flash mode...") print("knock mode: replug the dongle now (waiting up to 2 min)...")
if not enter_flash(s): s = knock_until_caught(port)
print("could not enter flash mode (app not running? replug and retry)") if s is None:
return 1 print("no knock window caught")
return 1
else:
s = open_port(port)
print("entering safeboot flash mode...")
if not enter_flash(s):
print("could not enter flash mode (app not running? replug and retry with --knock)")
return 1
s.reset_input_buffer() s.reset_input_buffer()
s.write(b"V"); time.sleep(0.1) s.write(b"V"); time.sleep(0.1)
print("safeboot:", s.read(16).decode("ascii", "replace").strip()) print("safeboot:", s.read(16).decode("ascii", "replace").strip())
+23 -16
View File
@@ -71,25 +71,31 @@ It drives the firmware the way the remote would and checks:
Open `Hexapod_Simulation_Unity` and press Play in `Main.unity`. No scene Open `Hexapod_Simulation_Unity` and press Play in `Main.unity`. No scene
edits are needed: `HexapodSimBootstrap` adds a `HexapodSimulation` object, edits are needed: `HexapodSimBootstrap` adds a `HexapodSimulation` object,
disables the old movement scripts (`Walk`, `RotationControl`, `Leg`, disables the old movement scripts (`Walk`, `RotationControl`, `Leg`), and
`HandleHoystick`), and binds the existing leg rig by name (`Leg` → `hip1` binds the existing leg rig by name (`Leg` → `hip1` / `Hip2` / `Knie`). The
/ `Hip2` / `Knie`). The on-screen panel (F1 toggles it) offers three input on-screen panel (F1 toggles it) offers two input sources:
sources:
- **Joystick:** the real remote as a USB controller, on input axes
`"1"`–`"6"` like the old `HandleHoystick`. Axis assignment and inversion
are set on the `SimRemote` component. The panel shows the raw axes so you
can fix them.
- **Keyboard:** test without a remote. WASD walks, Q/E turns, IJKL + U/O - **Keyboard:** test without a remote. WASD walks, Q/E turns, IJKL + U/O
drive the right stick, 1–7 flip the toggles, Backspace disarms, +/- drive the right stick, 1–7 flip the toggles, Backspace disarms, +/-
change speed. change speed. The panel has every remote toggle, the speed slider and
- **Robot serial:** no radio at all. Keystrokes go to `robot.ino`'s own simulated handset tilt (remote gyro).
`TEST_SERIAL_INPUT` harness. - **Real remote (radio):** the real handheld remote, over the air. The USB
nRF24 dongle (`../dongle/`, bridge mode) receives its packets, they go
into the firmware byte for byte, and the robot's `Telemetry` goes back as
the radio's ack payload, exactly like the real robot's `Comms`. The
remote's display then shows the simulated robot (state, IMU, joint
angles, servo calibration). `DongleLink.cs` finds the dongle's CH340 port
by itself (or set `SimRemote.donglePort`, e.g. `COM7`); quit SignalRGB
first, it grabs CH340 ports. Don't run the real robot at the same time:
both would answer the remote.
The panel also has every remote toggle, the speed slider, simulated There's no USB link to the remote or the robot any more (the old joystick
handset tilt (remote gyro), a **ground tilt** slider (to test the robot and serial-harness inputs and the `Serial*` / `HandleHoystick` scripts are
gyro), and live firmware state, joint angles, foot contact, the packet on gone): everything goes over the radio.
air and robot serial output.
The panel also has a **ground tilt** slider (to test the robot gyro), and
live firmware state, joint angles, foot contact, the packet on air and the
robot's debug prints.
The body moves **kinematically**, computed in the DLL (`SimBody.cpp`), so The body moves **kinematically**, computed in the DLL (`SimBody.cpp`), so
Unity shows what the smoke test checks: Unity shows what the smoke test checks:
@@ -119,4 +125,5 @@ component to the scene yourself.
firmware *intends*, including pulse-limit clamping (which is how it firmware *intends*, including pulse-limit clamping (which is how it
caught the old ±90° coxa offsets). Whether a real servo turns that way is caught the old ±90° coxa offsets). Whether a real servo turns that way is
a bench check. a bench check.
- The remote firmware. `SimRemote.cs` mirrors its packet rules instead. - The remote firmware, in Keyboard mode: `SimRemote.cs` mirrors its packet
rules instead. With the real remote over the radio, its own firmware runs.