Files
hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/HexapodSimNative.cs
T
mangavalkandClaude Opus 5.5 2b4750f8f4 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>
2026-11-17 22:48:41 +01:00

252 lines
12 KiB
C#

using System;
using System.IO;
using System.Runtime.InteropServices;
using UnityEngine;
namespace HexapodSim
{
// Wire format, byte-for-byte identical to shared/HexapodProtocol/HexapodProtocol.h.
// Checked against the DLL's sizeof(CmdPacket) at load.
[StructLayout(LayoutKind.Sequential, Pack = 2)]
public struct CmdPacket
{
public short vx, vy, yaw_rate;
public short rx, ry, rz;
public short remote_roll, remote_pitch;
public short body_height; // -1000 lowest .. 0 default .. 1000 highest
public short cal_value; // servo centre calibration (remote only; 0 here)
public byte speed_scale, flags, seq;
public byte cal_cmd; // CAL_NONE (0): the sim's remote never calibrates servos
public byte cal_servo;
public const byte FLAG_ARMED = 1 << 0;
public const byte FLAG_STAND = 1 << 1;
public const byte FLAG_WALK = 1 << 2;
public const byte FLAG_MODE_B = 1 << 3;
public const byte FLAG_RSTICK_XLATE = 1 << 4;
public const byte FLAG_GAIT_WAVE = 1 << 5;
public const byte FLAG_REMOTE_GYRO = 1 << 6;
public const byte FLAG_ROBOT_GYRO = 1 << 7;
}
// Mirrors HxGeometry in sim/HexapodSimApi.h.
[StructLayout(LayoutKind.Sequential)]
public struct HxGeometry
{
public int numLegs;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountXmm;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountYmm;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountAngleDeg;
public float coxaMm, femurMm, tibiaMm;
public float stanceRadiusMm, stanceHeightMm;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] coxaMinDeg;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] coxaMaxDeg;
public float femurMinDeg, femurMaxDeg, tibiaMinDeg, tibiaMaxDeg;
public float controlHz;
public float legClearanceMm;
public float stanceHeightRangeMm;
}
// Mirrors HxStatus in sim/HexapodSimApi.h.
[StructLayout(LayoutKind.Sequential)]
public struct HxStatus
{
public int robotState; // 0 DISARMED, 1 SIT, 2 STAND, 3 WALKING
public float sitStandBlend;
public int linkOk;
public float headingDeg;
public float imuRollDeg, imuPitchDeg;
public int imuEnabled;
public uint simTimeMs;
public uint controlTicks;
public int stanceMask;
public float poseReachScale; // 1 = full commanded pose; less = scaled to what the legs can reach
public float twistScale; // 1 = walk command as sent; less = slowed to a stride the legs can take
public float stanceHeightMm; // current standing height (feet below hips)
public string StateName => robotState switch
{
0 => "DISARMED", 1 => "SIT", 2 => "STAND", 3 => "WALKING", _ => "?"
};
}
// Loads sim/build/hexapod_sim.dll - the unchanged robot firmware compiled for
// the PC. The DLL is copied to a temp file and loaded by hand (not DllImport)
// so Unity never locks the build output: rebuild with CMake, press Play or
// "Reload firmware" and the new code runs. Each load is a fresh power-on.
public sealed class HexapodSimNative : IDisposable
{
public const int ExpectedApiVersion = 8; // HXSIM_API_VERSION
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int IntFn();
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void VoidFn();
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void IntArgFn(int v);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void UIntArgFn(uint v);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void GeometryFn(out HxGeometry g);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void StatusFn(out HxStatus s);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void PacketFn(ref CmdPacket p, int len);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ImuFn(float ax, float ay, float az, float gx, float gy, float gz);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BytesInFn(byte[] bytes, int len);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int BytesOutFn(byte[] bytes, int cap);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ServosFn(int[] us, int[] attached, uint[] writes);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void FloatsFn(float[] values);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void TwoFloatsFn(float a, float b);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int AckFn(byte[] bytes, int cap);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BodyFn(float[] pos3, float[] quatWxyz4, out float yawDeg, out int contactMask);
IntPtr handle_;
string tempCopy_;
IntFn apiVersion_, packetSize_;
GeometryFn getGeometry_;
IntArgFn setRadioPresent_, setImuPresent_;
VoidFn start_;
UIntArgFn step_;
PacketFn radioSend_;
BytesInFn radioSendBytes_;
ImuFn setImu_;
BytesOutFn serialOutput_;
ServosFn getServos_;
FloatsFn getJoints_, getFeet_;
StatusFn getStatus_;
TwoFloatsFn setGroundTilt_;
BodyFn getBody_;
AckFn getAckPayload_;
public string LoadedFrom { get; private set; }
public static HexapodSimNative Load(string dllPath)
{
if (!File.Exists(dllPath))
throw new FileNotFoundException(
"Simulation DLL not found. Build it first: run .\\sim\\build.ps1 from the 2026-current folder in PowerShell.",
dllPath);
var n = new HexapodSimNative();
n.tempCopy_ = Path.Combine(Path.GetTempPath(), $"hexapod_sim_{Guid.NewGuid():N}.dll");
File.Copy(dllPath, n.tempCopy_, true);
n.handle_ = LoadLibraryW(n.tempCopy_);
if (n.handle_ == IntPtr.Zero)
{
int err = Marshal.GetLastWin32Error();
n.Dispose();
throw new Exception($"LoadLibrary failed for {dllPath} (Win32 error {err}). Is it a 64-bit build?");
}
n.LoadedFrom = dllPath;
n.apiVersion_ = n.Fn<IntFn>("hxsim_api_version");
n.packetSize_ = n.Fn<IntFn>("hxsim_packet_size");
n.getGeometry_ = n.Fn<GeometryFn>("hxsim_get_geometry");
n.setRadioPresent_ = n.Fn<IntArgFn>("hxsim_set_radio_present");
n.setImuPresent_ = n.Fn<IntArgFn>("hxsim_set_imu_present");
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.serialOutput_ = n.Fn<BytesOutFn>("hxsim_serial_output");
n.getServos_ = n.Fn<ServosFn>("hxsim_get_servos");
n.getJoints_ = n.Fn<FloatsFn>("hxsim_get_joints_deg");
n.getFeet_ = n.Fn<FloatsFn>("hxsim_get_feet_mm");
n.getStatus_ = n.Fn<StatusFn>("hxsim_get_status");
n.setGroundTilt_ = n.Fn<TwoFloatsFn>("hxsim_set_ground_tilt");
n.getBody_ = n.Fn<BodyFn>("hxsim_get_body");
n.getAckPayload_ = n.Fn<AckFn>("hxsim_get_ack_payload");
int version = n.apiVersion_();
if (version != ExpectedApiVersion)
{
n.Dispose();
throw new Exception($"hexapod_sim.dll API version {version}, Unity scripts expect {ExpectedApiVersion}. Rebuild the DLL or update the scripts.");
}
int size = n.packetSize_();
if (size != Marshal.SizeOf<CmdPacket>())
{
n.Dispose();
throw new Exception($"CmdPacket is {size} bytes in the firmware but {Marshal.SizeOf<CmdPacket>()} in C#. Update HexapodSimNative.CmdPacket to match HexapodProtocol.h.");
}
return n;
}
T Fn<T>(string name) where T : Delegate
{
IntPtr p = GetProcAddress(handle_, name);
if (p == IntPtr.Zero) throw new Exception($"hexapod_sim.dll is missing export '{name}' - rebuild it.");
return Marshal.GetDelegateForFunctionPointer<T>(p);
}
public int PacketSize => packetSize_();
public HxGeometry Geometry { get { getGeometry_(out var g); return g; } }
public HxStatus Status { get { getStatus_(out var s); return s; } }
public void SetRadioPresent(bool present) => setRadioPresent_(present ? 1 : 0);
public void SetImuPresent(bool present) => setImuPresent_(present ? 1 : 0);
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);
readonly byte[] serialBuf_ = new byte[4096];
public string SerialOutput()
{
int n = serialOutput_(serialBuf_, serialBuf_.Length);
return n > 0 ? System.Text.Encoding.ASCII.GetString(serialBuf_, 0, n) : "";
}
public void GetServos(int[] us, int[] attached, uint[] writes) => getServos_(us, attached, writes);
public void GetJointsDeg(float[] deg18) => getJoints_(deg18);
public void GetFeetMm(float[] xyz18) => getFeet_(xyz18);
public void SetGroundTilt(float rollDeg, float pitchDeg) => setGroundTilt_(rollDeg, pitchDeg);
readonly float[] bodyPos_ = new float[3], bodyQuat_ = new float[4];
// Kinematic body from the DLL, robot-world frame. The rotation is a
// robot-frame quaternion held in a Unity Quaternion for its math only.
// The robot's telemetry (HexapodProtocol.h Telemetry, 30 bytes) from the
// last ack payload; returns its length, 0 before the first packet.
public int GetAckPayload(byte[] buf) => getAckPayload_(buf, buf.Length);
public void GetBody(out Vector3 posMm, out Quaternion rotRobot, out float yawDeg, out int contactMask)
{
getBody_(bodyPos_, bodyQuat_, out yawDeg, out contactMask);
posMm = new Vector3(bodyPos_[0], bodyPos_[1], bodyPos_[2]);
rotRobot = new Quaternion(bodyQuat_[1], bodyQuat_[2], bodyQuat_[3], bodyQuat_[0]);
}
public void Dispose()
{
if (handle_ != IntPtr.Zero) { FreeLibrary(handle_); handle_ = IntPtr.Zero; }
if (tempCopy_ != null)
{
try { File.Delete(tempCopy_); } catch { /* still mapped on some systems; temp dir cleans up */ }
tempCopy_ = null;
}
}
[DllImport("kernel32", SetLastError = true, CharSet = CharSet.Unicode)]
static extern IntPtr LoadLibraryW(string path);
[DllImport("kernel32", SetLastError = true)]
[return: MarshalAs(UnmanagedType.Bool)]
static extern bool FreeLibrary(IntPtr handle);
[DllImport("kernel32", SetLastError = true, CharSet = CharSet.Ansi, ExactSpelling = true)]
static extern IntPtr GetProcAddress(IntPtr handle, string name);
}
}