대한민국 공군 C-130J 수송기 저고도 화물투하(LAPES) 지면효과 보정 및 1,000Hz 0-Alloc C# 트림 피치 제어 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.AirForce.Transport
{
    public readonly record struct LapesFlightState(
        float AltitudeMeters,       // 2 ~ 5m 지면고도
        float AirspeedMps,          // 약 67 m/s (130 kts)
        float PitchAngleDeg,
        float PitchRateDegPerSec,
        float CargoPositionMeters,  // 0m(기내 고정) ~ 12m(램프 이탈)
        float CargoWeightKg         // 최대 15,000kg
    );

    public ref struct LapesPitchControlOutput
    {
        public float CommandedElevatorDeg; // 승강타 조타각
        public float PitchTrimOffsetDeg;
        public float CurrentGroundEffectFactor;
        public bool CargoSeparationComplete;
    }

    public static class C130jLapesGroundEffectPitchEngine
    {
        private const float WingSpanMeters = 40.41f; // C-130J 날개폭
        private const float MaxElevatorDownDeg = -25.0f; // 최대 기수 내림

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void UpdateLapesPitchTrim(
            ref readonly LapesFlightState state,
            out LapesPitchControlOutput output)
        {
            output = default;

            // 1. 지면효과 계수 산출: sigma = 16(h/b)^2 / (1 + 16(h/b)^2)
            float hb = state.AltitudeMeters / WingSpanMeters;
            float hbSq16 = 16.0f * hb * hb;
            float sigma = hbSq16 / (1.0f + hbSq16);

            // 2. 화물 후방 이동에 따른 무게중심 편차 환산 (피치업 모멘트)
            // CargoPosition: 0m(정상 CG) -> 12m(램프 끝, 극단적 후방 CG)
            float cgShiftRatio = Math.Clamp(state.CargoPositionMeters / 12.0f, 0.0f, 1.0f);
            float inducedPitchUpMoment = cgShiftRatio * 18.0f; // 최대 18도 상당의 피치업 모멘트

            // 3. 지면효과에 의한 추가 양력 피치 모멘트 보정
            float groundEffectPitchBias = (1.0f - sigma) * 3.5f;

            // 4. 승강타 제어 법칙: PD 피치 각속도 감쇠 + 선행 보상
            float pitchError = state.PitchAngleDeg - 2.5f; // 목표 착륙자세 2.5도
            float kp = 1.8f;
            float kd = 0.65f;

            // 선행 역토크 = 화물 후방 모멘트 + 지면효과 보정
            float feedforward = -(inducedPitchUpMoment + groundEffectPitchBias);
            float feedback = -(kp * pitchError + kd * state.PitchRateDegPerSec);

            float commandedElevator = feedforward + feedback;
            commandedElevator = Math.Clamp(commandedElevator, MaxElevatorDownDeg, 15.0f);

            output.CommandedElevatorDeg = commandedElevator;
            output.PitchTrimOffsetDeg = feedforward;
            output.CurrentGroundEffectFactor = sigma;
            output.CargoSeparationComplete = state.CargoPositionMeters >= 12.0f;
        }
    }
}