AIDT 온디바이스 국방 MUM-T 틸트로터 무인기(Tiltrotor UAV) 동적 유입류(Dynamic Inflow) 및 천이 비행 나셀 틸트 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.TiltrotorUav
{
    /// <summary>
    /// 틸트로터 무인기 비행 상태 및 나셀 지향각
    /// </summary>
    public readonly ref struct TiltrotorFlightTelemetryState
    {
        public readonly double NacelleAngleDeg;       // 나셀 각도 (90도=수직, 0도=수평) [deg]
        public readonly double AirspeedMps;           // 대기속도 [m/s]
        public readonly double PitchRateRadPerSec;    // 피치 각속도 q [rad/s]
        public readonly double RollRateRadPerSec;     // 롤 각속도 p [rad/s]
        public readonly double RotorRpm;              // 로터 회전수 [RPM]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TiltrotorFlightTelemetryState(double nacelleDeg, double vAir, double q, double p, double rpm)
        {
            NacelleAngleDeg = nacelleDeg;
            AirspeedMps = vAir;
            PitchRateRadPerSec = q;
            RollRateRadPerSec = p;
            RotorRpm = rpm;
        }
    }

    /// <summary>
    /// 로터 스와시플레이트 피치 및 조타면 구동 출력
    /// </summary>
    public readonly ref struct TiltrotorActuatorOutput
    {
        public readonly double CollectivePitchDeg;     // 로터 콜렉티브 피치 [deg]
        public readonly double LongitudinalCyclicDeg;  // 종방향 사이클릭 피치 [deg]
        public readonly double WingFlaperonDeg;        // 주익 플래퍼론 타각 [deg]
        public readonly double CommandedNacelleRateDps;// 나셀 틸트 각속도 지령 [deg/s]
        public readonly bool IsWithinSafeCorridor;     // 천이 비행 안전 회랑 내 유지 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TiltrotorActuatorOutput(double col, double cyc, double flap, double nacRate, bool safe)
        {
            CollectivePitchDeg = col;
            LongitudinalCyclicDeg = cyc;
            WingFlaperonDeg = flap;
            CommandedNacelleRateDps = nacRate;
            IsWithinSafeCorridor = safe;
        }
    }

    /// <summary>
    /// 10,000Hz 틸트로터 동적 유입류 보상 및 천이 비행 서보 엔진
    /// </summary>
    public ref struct TiltrotorTransitionServoEngine
    {
        private readonly double _kpPitch;
        private readonly double _kdPitch;
        private readonly double _kpRoll;
        private readonly double _kdRoll;
        private readonly double _maxNacelleRateDps;

        private double _inflowLambda1c; // 종방향 유입류 상태 변수
        private double _prevPitchRate;

        public TiltrotorTransitionServoEngine(
            double kpPitch,
            double kdPitch,
            double kpRoll,
            double kdRoll,
            double maxNacelleRateDps)
        {
            _kpPitch = kpPitch;
            _kdPitch = kdPitch;
            _kpRoll = kpRoll;
            _kdRoll = kdRoll;
            _maxNacelleRateDps = maxNacelleRateDps;
            _inflowLambda1c = 0.0;
            _prevPitchRate = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TiltrotorActuatorOutput ComputeCycle(
            in TiltrotorFlightTelemetryState state,
            double targetNacelleDeg,
            double dtSeconds)
        {
            // 1. 피트-피터스 종방향 비대칭 동적 유입류 간이 추정 (전진비에 비례)
            double tipSpeed = (state.RotorRpm * 2.0 * Math.PI / 60.0) * 1.5; // 반경 1.5m 로터
            double advanceRatio = (state.AirspeedMps * Math.Cos(state.NacelleAngleDeg * Math.PI / 180.0)) / Math.Max(10.0, tipSpeed);
            double targetLambda1c = 0.15 * advanceRatio;
            _inflowLambda1c += (targetLambda1c - _inflowLambda1c) * (dtSeconds / 0.02); // 시정수 20ms 일차 필터

            // 2. 나셀 각도에 따른 천이 안전 속도 회랑(Conversion Corridor) 검증
            // 수직모드(90도)에서는 0~30m/s, 수평모드(0도)에서는 50~120m/s 허용
            double minCorridorSpeed = (90.0 - state.NacelleAngleDeg) * 0.55;
            double maxCorridorSpeed = 30.0 + (90.0 - state.NacelleAngleDeg) * 1.0;
            bool isCorridorSafe = state.AirspeedMps >= (minCorridorSpeed - 5.0) && state.AirspeedMps <= (maxCorridorSpeed + 10.0);

            // 3. 나셀 틸트 지령 슬루레이트 제어
            double nacelleErr = targetNacelleDeg - state.NacelleAngleDeg;
            double nacelleRateCmd = Math.Clamp(nacelleErr * 2.0, -_maxNacelleRateDps, _maxNacelleRateDps);

            // 4. 로터 콜렉티브 & 주익 플래퍼론 분배 (나셀 각도 가중 블렌딩)
            double nacelleRad = state.NacelleAngleDeg * Math.PI / 180.0;
            double rotorWeight = Math.Sin(nacelleRad); // 90도일 때 1.0
            double wingWeight = Math.Cos(nacelleRad);  // 0도일 때 1.0

            double baseCollective = 8.0 * rotorWeight + 16.0 * wingWeight; // 비행기 모드에서 큰 피치

            // 5. 종방향 사이클릭 피치 = 피치 댐핑 + 비대칭 유입류 보상
            double dPitchRate = (state.PitchRateRadPerSec - _prevPitchRate) / dtSeconds;
            _prevPitchRate = state.PitchRateRadPerSec;

            double cyclicComp = -(_kpPitch * state.PitchRateRadPerSec + _kdPitch * dPitchRate) + (_inflowLambda1c * 12.0);
            double longitudinalCyclic = Math.Clamp(cyclicComp, -10.0, 10.0);

            // 주익 플래퍼론은 고정익 모드 비율에 맞춰 증가
            double flaperon = Math.Clamp(wingWeight * 15.0 - (state.PitchRateRadPerSec * 8.0), 0.0, 25.0);

            return new TiltrotorActuatorOutput(
                baseCollective,
                longitudinalCyclic,
                flaperon,
                nacelleRateCmd,
                isCorridorSafe);
        }
    }
}