hexapod-robot/2026-current/Hexapod_Simulation_Unity/Assets/HexapodSim/HexapodSimNative.cs

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 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 = 6;   // 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 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.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");
            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");

            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>());
        // 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);
    }
}