수직이착륙 틸트로터 무인기 블레이드-와류 간섭(BVI) 저소음 능동 플랩 제어 서보 C# SIMD 엔진

3. Zero-Allocation C# SIMD 틸트로터 BVI 능동 플랩 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;

namespace ZeliDesk.Defense.Rotorcraft.BviAcoustics
{
    public readonly ref struct BviServoState
    {
        public readonly float AcousticNoiseDba;
        public readonly float FlapDeflectionAngleDeg;
        public readonly float PiezoDriveVoltage;
        public readonly bool BviNoiseSuppressed;

        public BviServoState(float noise, float flap, float volt, bool suppressed)
        {
            AcousticNoiseDba = noise;
            FlapDeflectionAngleDeg = flap;
            PiezoDriveVoltage = volt;
            BviNoiseSuppressed = suppressed;
        }
    }

    public sealed class TiltrotorBviServoEngine
    {
        private const float SampleRateHz = 20000.0f;
        private const float Dt = 1.0f / SampleRateHz;

        // 목표 사양 (BVI 충격 소음 75 dBA 이하 제압)
        private const float TargetMaxNoiseDba = 75.0f;
        private const float RotorRpm = 450.0f;
        private const float OmegaRadS = RotorRpm * (2.0f * MathF.PI / 60.0f);

        // PI 서보 제어 상태
        private float _noiseIntegral = 0.0f;
        private float _prevError = 0.0f;
        private const float KpFlap = 0.35f;
        private const float KiFlap = 18.0f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public BviServoState SuppressBviPulse(
            float bladeAzimuthDeg,
            float measuredAcousticPressurePa,
            float nacelleTiltAngleDeg,
            float descentRateMps)
        {
            // 1. 측정된 음압(Pa)으로부터 dBA 소음 레벨 환산
            float safePa = MathF.Max(measuredAcousticPressurePa, 1.0e-5f);
            float currentNoiseDba = 20.0f * MathF.Log10(safePa / 20.0e-6f);

            // 2. 오차 계산
            float noiseError = currentNoiseDba - TargetMaxNoiseDba;

            // 3. 적분 연산 및 윈드업 제한
            _noiseIntegral += noiseError * Dt;
            _noiseIntegral = Math.Clamp(_noiseIntegral, -1.0f, 1.0f);

            // 4. 로터 방위각에 따른 다중 고조파(HHC: 3/rev & 4/rev) 피드포워드 합성
            float psiRad = bladeAzimuthDeg * (MathF.PI / 180.0f);
            float hhcFeedforward = 3.5f * MathF.Sin(3.0f * psiRad - 0.4f) + 2.0f * MathF.Cos(4.0f * psiRad);

            // 나셀 경사각(천이 모드)에 따른 가중치
            float nacelleFactor = MathF.Sin(nacelleTiltAngleDeg * (MathF.PI / 180.0f));

            float controlDemand = (KpFlap * noiseError) + (KiFlap * _noiseIntegral) + (hhcFeedforward * nacelleFactor);
            float flapAngleDeg = Math.Clamp(controlDemand, -6.0f, 6.0f);

            // 압전 액추에이터 구동 전압 (0 ~ 150V)
            float piezoVoltage = Math.Clamp(75.0f + flapAngleDeg * 12.0f, 0.0f, 150.0f);

            bool isSuppressed = currentNoiseDba <= TargetMaxNoiseDba;

            return new BviServoState(
                noise: currentNoiseDba,
                flap: flapAngleDeg,
                volt: piezoVoltage,
                suppressed: isSuppressed
            );
        }
    }
}