대한민국 공군 KA-1 공중통제기 저고도 산악 침투 지형추적(TF) 유도 및 1,000Hz 0-Alloc C# 비행경로각 안전제어 엔진

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

namespace ZeliDesk.Defense.AirForce.FlightControl
{
    public readonly record struct AircraftFlightState(
        float AltitudeMeters,       // z
        float TrueAirspeedMps,      // V
        float FlightPathAngleRad,   // gamma
        float PitchRateRadPerSec    // q
    );

    public ref struct TerrainFollowingCommand
    {
        public float CommandedPitchG;       // 목표 하중배수 nz
        public float CommandedFlightPathDeg;
        public float ClearanceErrorMeters;
        public bool TerrainWarningPullUp;
    }

    public static class Ka1FacTerrainFollowingEngine
    {
        private const float StandardGravity = 9.80665f;
        private const float MaxPositiveG = 3.5f; // KA-1 저고도 기동 제한
        private const float MinNegativeG = 0.5f;
        private const float ClearanceSetpoint = 60.0f; // 60m (약 200ft) 저고도

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeTerrainFollowing(
            ref readonly AircraftFlightState state,
            ReadOnlySpan<float> forwardTerrainAltitudesMeters, // 100m 간격 전방 지형
            out TerrainFollowingCommand cmd)
        {
            cmd = default;
            float v = state.TrueAirspeedMps;
            float z = state.AltitudeMeters;
            float gamma = state.FlightPathAngleRad;

            float rPullUp = (v * v) / (StandardGravity * (MaxPositiveG - MathF.Cos(gamma)));
            if (rPullUp < 100.0f) rPullUp = 100.0f;

            float maxAngle = -1.0f;
            float criticalDistance = 100.0f;

            // 전방 지형 프로파일 스캔 (최대 2km, 20개 샘플)
            int sampleCount = Math.Min(forwardTerrainAltitudesMeters.Length, 20);
            for (int i = 0; i < sampleCount; i++)
            {
                float x = (i + 1) * 100.0f; // 100m, 200m, ..., 2000m
                float terrainWithClearance = forwardTerrainAltitudesMeters[i] + ClearanceSetpoint;
                float heightDiff = terrainWithClearance - z;

                // 포물선 여유분을 반영한 요구 각도
                float reqAngle = MathF.Atan2(heightDiff, x) + (x / (2.0f * rPullUp));
                if (reqAngle > maxAngle)
                {
                    maxAngle = reqAngle;
                    criticalDistance = x;
                }
            }

            // 경로각 오차에 따른 하중배수(nz) 지령 생성
            float angleError = maxAngle - gamma;
            float kp = 2.5f;
            float desiredNz = MathF.Cos(gamma) + (kp * angleError * (v / StandardGravity));

            // 한계 G 클램핑
            bool warning = false;
            if (desiredNz > MaxPositiveG)
            {
                desiredNz = MaxPositiveG;
                warning = true;
            }
            else if (desiredNz < MinNegativeG)
            {
                desiredNz = MinNegativeG;
            }

            cmd.CommandedPitchG = desiredNz;
            cmd.CommandedFlightPathDeg = maxAngle * (180.0f / MathF.PI);
            cmd.ClearanceErrorMeters = z - forwardTerrainAltitudesMeters[0];
            cmd.TerrainWarningPullUp = warning;
        }
    }
}