대한민국 공군 KC-330 공중급유기 플라잉 붐 공력 서보 제어 및 1,000Hz 0-Alloc C# 자동 급유정렬(AAR) 엔진

using System;
using System.Numerics;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.AirForce.AirRefueling
{
    public readonly record struct BoomState(
        float AzimuthDeg,      // 요 각도
        float ElevationDeg,    // 피치 각도
        float ExtensionMeters, // 텔레스코프 전개 길이 (0 ~ 6m)
        float AirspeedMps
    );

    public readonly record struct ReceptacleTrackingData(
        Vector3 RelTargetPosMeters, // 붐 노즐 기준 리셉터클 상대좌표
        Vector3 RelVelocityMps,
        bool OpticalTrackLocked
    );

    public ref struct BoomActuatorCommand
    {
        public float CommandedElevonLeftDeg;
        public float CommandedElevonRightDeg;
        public float TelescopeRateMps;
        public float AlignmentErrorMm;
        public bool ReadyForContact;
    }

    public static class Kc330AarBoomAerodynamicServoEngine
    {
        private const float MaxControlSurfaceDeg = 20.0f;
        private const float ContactToleranceMm = 50.0f; // 50mm 도킹 허용 오차

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void UpdateBoomGuidance(
            ref readonly BoomState boom,
            ref readonly ReceptacleTrackingData target,
            out BoomActuatorCommand cmd)
        {
            cmd = default;
            float errorDistMm = target.RelTargetPosMeters.Length() * 1000.0f;
            cmd.AlignmentErrorMm = errorDistMm;

            if (!target.OpticalTrackLocked)
            {
                cmd.CommandedElevonLeftDeg = 0.0f;
                cmd.CommandedElevonRightDeg = 0.0f;
                cmd.TelescopeRateMps = 0.0f;
                cmd.ReadyForContact = false;
                return;
            }

            // 1. 수유기 바우웨이브 외란 보상
            float distZ = MathF.Abs(target.RelTargetPosMeters.Z);
            float bowWaveBias = Math.Clamp(1.5f / (distZ + 0.5f), 0.0f, 4.0f); // 하향 피치 요구

            // 2. 오차 기반 PD 제어 법칙
            float kpPitch = 3.2f;
            float kdPitch = 0.8f;
            float kpYaw = 2.8f;
            float kdYaw = 0.7f;

            float pitchCmd = (kpPitch * target.RelTargetPosMeters.Z) - (kdPitch * target.RelVelocityMps.Z) - bowWaveBias;
            float yawCmd = (kpYaw * target.RelTargetPosMeters.Y) - (kdYaw * target.RelVelocityMps.Y);

            // 3. V자형 러더베이터(Ruddervator) 믹싱: Left = Pitch + Yaw, Right = Pitch - Yaw
            float leftDeg = Math.Clamp(pitchCmd + yawCmd, -MaxControlSurfaceDeg, MaxControlSurfaceDeg);
            float rightDeg = Math.Clamp(pitchCmd - yawCmd, -MaxControlSurfaceDeg, MaxControlSurfaceDeg);

            // 4. 텔레스코프 전개 속도 제어
            float extRate = 0.0f;
            if (errorDistMm < 1500.0f && errorDistMm > ContactToleranceMm)
            {
                extRate = 0.45f; // 초당 45cm 전진
            }

            cmd.CommandedElevonLeftDeg = leftDeg;
            cmd.CommandedElevonRightDeg = rightDeg;
            cmd.TelescopeRateMps = extRate;
            cmd.ReadyForContact = errorDistMm <= ContactToleranceMm;
        }
    }
}