고고도 장기체공(HALE) 태양광 무인기 초유연 날개 광섬유 FBG 형상 감지 및 능동 캠버 제어 C# SIMD 서보 엔진

3. C# .NET 9.0 Zero-Allocation 2,000Hz 날개 형상 감지 및 캠버 서보 엔진

SIMD 수치 적분기를 사용하여 8개 구간의 FBG 변형률 데이터를 0.5ms마다 즉각 3차원 변위로 변환하고 서보 모터 지령을 생성합니다.

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

namespace ZeliDesk.Defense.Aeroelasticity
{
    /// <summary>
    /// 초유연 날개 8개 스팬 스테이션 FBG 광섬유 변형률 텔레메트리 (2,000Hz 샘플링)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 4)]
    public readonly struct WingFbgStrainTelemetry
    {
        public readonly float TimestampSec;
        public readonly float TrueAirspeedMs; // e.g. 24.5 m/s
        public readonly float AmbientAirDensityKgM3;
        public readonly Vector256<float> TopStrainsMicrostrain;
        public readonly Vector256<float> BottomStrainsMicrostrain;

        public WingFbgStrainTelemetry(
            float time, float speed, float density,
            Vector256<float> top, Vector256<float> bot)
        {
            TimestampSec = time;
            TrueAirspeedMs = speed;
            AmbientAirDensityKgM3 = density;
            TopStrainsMicrostrain = top;
            BottomStrainsMicrostrain = bot;
        }
    }

    /// <summary>
    /// 2,000Hz FBG 형상 실시간 재구성 및 능동 캠버 플랩 서보 엔진
    /// </summary>
    public sealed class HaleWingAeroelasticServoEngine
    {
        private const float SparHeightMeters = 0.18f;
        private const float StationSegmentLengthMeters = 2.5f; // 8개 구간 = 20m 반날개
        private const float CamberGainKp = 0.85f;
        private const float CamberGainKd = 0.042f;

        private float _prevTipDeflection;

        public HaleWingAeroelasticServoEngine()
        {
            _prevTipDeflection = 0.0f;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public void ComputeWingShapeAndCamber(
            in WingFbgStrainTelemetry telemetry,
            out float currentTipDeflectionMeters,
            out float currentTipTwistAngleDeg,
            out float commandedOutboardCamberDeg)
        {
            // 1. SIMD 곡률 연산: kappa = (e_top - e_bot) / h_spar (1e-6 단위 변환)
            Vector256<float> diff = Avx.Subtract(telemetry.TopStrainsMicrostrain, telemetry.BottomStrainsMicrostrain);
            Vector256<float> scale = Vector256.Create(1e-6f / SparHeightMeters);
            Vector256<float> curvatures = Avx.Multiply(diff, scale);

            Span<float> curvSpan = stackalloc float[8];
            MemoryMarshal.Write(MemoryMarshal.AsBytes(curvSpan), in curvatures);

            // 2. 사다리꼴 수치 적분을 통한 3차원 변위 및 각도 복원
            float slope = 0.0f;
            float deflection = 0.0f;

            for (int i = 0; i < 8; i++)
            {
                slope += curvSpan[i] * StationSegmentLengthMeters;
                deflection += slope * StationSegmentLengthMeters;
            }

            currentTipDeflectionMeters = deflection;
            currentTipTwistAngleDeg = slope * 57.2958f;

            // 3. 변위 속도 계산 (dt = 0.0005s, 2,000Hz)
            float tipVelocity = (deflection - _prevTipDeflection) * 2000.0f;
            _prevTipDeflection = deflection;

            // 4. 외측 가변 캠버 플랩 지령 산출 (-12도 ~ +12도)
            // 상향 처짐 시 양력 감소(-camber) 유도하여 굽힘 모멘트 완화
            float deflectionError = deflection - 1.20f; // 정상 순항 1.2m 처짐 기준
            float targetCamber = -(CamberGainKp * deflectionError) - (CamberGainKd * tipVelocity);

            commandedOutboardCamberDeg = Math.Clamp(targetCamber, -12.0f, 12.0f);
        }
    }
}