성층권 태양광 고고도 무인기(HAPS) 최대 전력점 추적(MPPT) 및 주익 유연 공력 트림 서보 C# SIMD 엔진

3. Zero-Allocation C# SIMD HAPS MPPT 및 주익 트림 서보 엔진

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

namespace ZeliDesk.Defense.Haps.EnergyAero
{
    public readonly ref struct HapsServoState
    {
        public readonly float SolarPowerWatts;
        public readonly float MpptDutyCycle;
        public readonly float AileronTrimDeg;
        public readonly bool FlutterDamped;

        public HapsServoState(float power, float duty, float trim, bool damped)
        {
            SolarPowerWatts = power;
            MpptDutyCycle = duty;
            AileronTrimDeg = trim;
            FlutterDamped = damped;
        }
    }

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

        // MPPT 상태
        private float _prevVoltage = 0.0f;
        private float _prevPower = 0.0f;
        private float _currentDuty = 0.65f;

        // 공력 플러터 댐퍼 상태
        private float _prevBendingRate = 0.0f;
        private const float KpFlutter = 0.45f;
        private const float KdFlutter = 0.008f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public HapsServoState UpdateServoCycle(
            float vPv, float iPv,
            float wingTipZAccelG,
            float wingTorsionAngleDeg)
        {
            // 1. 태양광 출력 전력 계산 및 MPPT 섭동-관측(P&O) 루프
            float currentPower = vPv * iPv;
            float deltaV = vPv - _prevVoltage;
            float deltaP = currentPower - _prevPower;

            if (MathF.Abs(deltaV) > 0.01f)
            {
                float step = (deltaP / deltaV > 0.0f) ? 0.0005f : -0.0005f;
                _currentDuty = Math.Clamp(_currentDuty + step, 0.1f, 0.95f);
            }

            _prevVoltage = vPv;
            _prevPower = currentPower;

            // 2. 주익 굽힘-비틀림 진동 속도 적분
            float bendingVelocity = (wingTipZAccelG - 1.0f) * 9.80665f * Dt;
            float bendingAccel = (bendingVelocity - _prevBendingRate) * SampleRateHz;
            _prevBendingRate = bendingVelocity;

            // 3. 플러터 감쇠 보조익 트림 계산 (PD 제어)
            float trimDemandDeg = -(KpFlutter * wingTorsionAngleDeg) - (KdFlutter * bendingAccel);
            float finalTrimDeg = Math.Clamp(trimDemandDeg, -15.0f, 15.0f);

            bool isDamped = MathF.Abs(wingTorsionAngleDeg) < 0.5f;

            return new HapsServoState(
                power: currentPower,
                duty: _currentDuty,
                trim: finalTrimDeg,
                damped: isDamped
            );
        }
    }
}