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 byte speed_scale, flags, seq; 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; public float coxaMinDeg, coxaMaxDeg, femurMinDeg, femurMaxDeg, tibiaMinDeg, tibiaMaxDeg; public float controlHz; public int servoUsAt0Deg, servoUsPerDeg; 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 = 5; // 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 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_; ImuFn setImu_; BytesInFn serialInput_; BytesOutFn serialOutput_; ServosFn getServos_; FloatsFn getJoints_, getFeet_; StatusFn getStatus_; TwoFloatsFn setGroundTilt_; BodyFn getBody_; 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 repo root 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("hxsim_api_version"); n.packetSize_ = n.Fn("hxsim_packet_size"); n.getGeometry_ = n.Fn("hxsim_get_geometry"); n.setRadioPresent_ = n.Fn("hxsim_set_radio_present"); n.setImuPresent_ = n.Fn("hxsim_set_imu_present"); n.start_ = n.Fn("hxsim_start"); n.step_ = n.Fn("hxsim_step"); n.radioSend_ = n.Fn("hxsim_radio_send"); n.setImu_ = n.Fn("hxsim_set_imu"); n.serialInput_ = n.Fn("hxsim_serial_input"); n.serialOutput_ = n.Fn("hxsim_serial_output"); n.getServos_ = n.Fn("hxsim_get_servos"); n.getJoints_ = n.Fn("hxsim_get_joints_deg"); n.getFeet_ = n.Fn("hxsim_get_feet_mm"); n.getStatus_ = n.Fn("hxsim_get_status"); n.setGroundTilt_ = n.Fn("hxsim_set_ground_tilt"); n.getBody_ = n.Fn("hxsim_get_body"); 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()) { n.Dispose(); throw new Exception($"CmdPacket is {size} bytes in the firmware but {Marshal.SizeOf()} in C#. Update HexapodSimNative.CmdPacket to match HexapodProtocol.h."); } return n; } T Fn(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(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()); // 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() { 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. 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); } }