HALE 태양광 무인기 플렉서블 윙 공탄성 플러터 & 1,000Hz 0-Alloc C# 테오도르센 비정상 공기력 제어 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.AeroelasticityControl
{
    /// <summary>
    /// HALE 유연 날개 센서 계측 상태 구조체
    /// </summary>
    public readonly ref struct FlexibleWingSensorState
    {
        public double AirspeedMps { get; }          // 대기속도 V (m/s)
        public double WingTipBendingDeflectionM { get; } // 날개 끝 굽힘 변위 h (m)
        public double WingTipTorsionAngleRad { get; }   // 날개 끝 비틀림 각도 alpha (rad)
        public double WingTipBendingAccelMps2 { get; }  // 날개 끝 수직 가속도 (m/s^2)
        public double AltitudeMeters { get; }       // 비행 고도 (m)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public FlexibleWingSensorState(double v, double h, double alpha, double accel, double alt)
        {
            AirspeedMps = v;
            WingTipBendingDeflectionM = h;
            WingTipTorsionAngleRad = alpha;
            WingTipBendingAccelMps2 = accel;
            AltitudeMeters = alt;
        }
    }

    /// <summary>
    /// 1,000Hz 0-Alloc C# 테오도르센 비정상 공기력 추정 및 능동 플러터 억제 엔진
    /// </summary>
    public ref struct ActiveFlutterSuppressionEngine
    {
        private const double SemiChordB = 0.75; // 날개 반시위 길이 0.75m
        private const double FlutterSpeedThreshold = 28.5; // 성층권 플러터 임계속도 28.5 m/s

        private double _previousBendingDeflectionM;
        private double _integratedTorsionRadSec;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ActiveFlutterSuppressionEngine(double initialDeflection)
        {
            _previousBendingDeflectionM = initialDeflection;
            _integratedTorsionRadSec = 0.0;
        }

        /// <summary>
        /// 1,000Hz 실시간 루프 (dt = 0.001s) 감쇠율 계산 및 제어 에일러론 조타각 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ProcessStep(
            in FlexibleWingSensorState state,
            out double commandedAileronDeg,
            out double aerodynamicDampingRatio,
            out bool isFlutterImminent)
        {
            const double dt = 0.001; // 1ms 타임스텝

            // 1. 날개 굽힘 속도 dh/dt 산출
            double bendingVelocityMps = (state.WingTipBendingDeflectionM - _previousBendingDeflectionM) / dt;
            _previousBendingDeflectionM = state.WingTipBendingDeflectionM;

            // 2. 환원 주파수 k 및 테오도르센 감쇠 근사 (F ~ 0.5 + 0.5/(1 + 2*k))
            double omegaEst = Math.Abs(bendingVelocityMps / Math.Max(0.01, state.WingTipBendingDeflectionM));
            double reducedFreqK = (omegaEst * SemiChordB) / Math.Max(1.0, state.AirspeedMps);
            double theodorsenF = 0.5 + (0.5 / (1.0 + 2.0 * reducedFreqK));

            // 3. 공탄성 감쇠비 추정
            double velocityMargin = (FlutterSpeedThreshold - state.AirspeedMps) / FlutterSpeedThreshold;
            aerodynamicDampingRatio = 0.05 * velocityMargin * theodorsenF;

            // 4. 플러터 임박 판정 (감쇠비 < 0.01 또는 속도 임계치 접근)
            isFlutterImminent = aerodynamicDampingRatio < 0.012;

            // 5. 능동 플러터 억제(Active Flutter Suppression) 서보 제어기
            // 가동 에일러론 조타: beta = - (K_h * dh/dt + K_alpha * alpha)
            const double kh = 1.25; // deg / (m/s)
            const double kalpha = 35.0; // deg / rad
            double aileronCmd = -(kh * bendingVelocityMps + kalpha * state.WingTipTorsionAngleRad);

            commandedAileronDeg = Math.Clamp(aileronCmd, -15.0, 15.0);
            _integratedTorsionRadSec += state.WingTipTorsionAngleRad * dt;
        }
    }
}