대한민국 공군 T-50 고등훈련기 3중 디지털 FBW 단주기 종동역학 제어 및 1,000Hz 0-Alloc C# AOA 실속 방지 리미터

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.AirForce.FlightControl
{
    public readonly record struct FbwSensorChannel(
        float AoaDeg,            // 받음각 센서
        float PitchRateDegPerSec,// 피치각속도 자이로 (q)
        float DynamicPressureKpa,// 동압 q_bar
        float PilotPitchStickInput // -1.0(기수내림) ~ +1.0(기수들림)
    );

    public ref struct FbwActuatorOutput
    {
        public float CommandedElevonDeg;
        public float FilteredAoaDeg;
        public bool AoaLimiterActive;
        public bool TripleRedundancyPassed;
    }

    public static class T50FlightControlCore
    {
        private const float MaxAllowedAoaDeg = 29.0f;
        private const float AoaMarginDeg = 25.0f;
        private const float KAlpha = 0.65f;
        private const float KQ = 0.42f;
        private const float KLimiter = 3.5f;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ProcessFlightControlTriple(
            ReadOnlySpan channels,
            ref FbwActuatorOutput output)
        {
            if (channels.Length < 3)
            {
                output.TripleRedundancyPassed = false;
                return;
            }

            // 1. 3채널 센서 중위수 투표 (Median Voting) - 0-Alloc
            float aoa = GetMedian(channels[0].AoaDeg, channels[1].AoaDeg, channels[2].AoaDeg);
            float q = GetMedian(channels[0].PitchRateDegPerSec, channels[1].PitchRateDegPerSec, channels[2].PitchRateDegPerSec);
            float stick = GetMedian(channels[0].PilotPitchStickInput, channels[1].PilotPitchStickInput, channels[2].PilotPitchStickInput);

            output.FilteredAoaDeg = aoa;
            output.TripleRedundancyPassed = true;

            // 2. 단주기 선행 예측 받음각: alpha_pred = alpha + 0.3 * q
            float predictedAoa = aoa + 0.3f * q;

            // 3. 파일럿 스틱 기본 승강타 타각 (최대 ±25°)
            float nominalElevon = stick * 20.0f;

            // 4. AOA 실속 방지 리미터 동작 검사
            bool limiterActive = false;
            if (predictedAoa > AoaMarginDeg)
            {
                float excessAoa = predictedAoa - AoaMarginDeg;
                float noseDownOverride = -excessAoa * KLimiter;
                nominalElevon = MathF.Min(nominalElevon, noseDownOverride);
                limiterActive = true;
            }

            // 5. 안정성 증대 피치 댐퍼 피드백: delta_fb = - K_alpha * alpha - K_q * q
            float feedbackElevon = -KAlpha * aoa - KQ * q;
            float totalElevon = nominalElevon + feedbackElevon;

            // 액추에이터 포화 리미터 (-25° ~ +25°)
            output.CommandedElevonDeg = MathF.Min(MathF.Max(totalElevon, -25.0f), 25.0f);
            output.AoaLimiterActive = limiterActive;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static float GetMedian(float a, float b, float c)
        {
            if (a > b)
            {
                if (b > c) return b;
                return (a > c) ? c : a;
            }
            else
            {
                if (a > c) return a;
                return (b > c) ? c : b;
            }
        }
    }
}