전술 계류 비행선(Tactical Aerostat) 고고도 돌풍 계류삭(Tether) 장력 완충 윈치 서보 C# SIMD 엔진

3. Zero-Allocation C# SIMD 계류삭 장력 완충 윈치 서보 엔진

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

namespace ZeliDesk.Defense.Aerostat.Tether
{
    public readonly ref struct WinchServoState
    {
        public readonly float CurrentTensionKn;
        public readonly float WinchDrumSpeedRpm;
        public readonly float MotorTorqueNm;
        public readonly bool EmergencyBrakeActive;

        public WinchServoState(float tension, float speed, float torque, bool brake)
        {
            CurrentTensionKn = tension;
            WinchDrumSpeedRpm = speed;
            MotorTorqueNm = torque;
            EmergencyBrakeActive = brake;
        }
    }

    public sealed class AerostatTetherWinchServoEngine
    {
        private const float SampleRateHz = 5000.0f;
        private const float Dt = 1.0f / SampleRateHz;

        // 물리 파라미터
        private const float TargetTensionKn = 25.0f;       // 정상 운용 장력 25 kN
        private const float MaxSafeTensionKn = 45.0f;      // 최대 허용 안전 장력 45 kN
        private const float DrumRadiusMeters = 0.45f;      // 드럼 반경 45 cm
        private const float JTotal = 85.0f;                // 윈치+모터 회전 관성 (kg*m^2)
        private const float BViscous = 1.2f;               // 점성 마찰 계수

        // 제어기 상태
        private float _drumVelocityRadS = 0.0f;
        private float _prevTensionError = 0.0f;
        private float _tensionIntegral = 0.0f;

        // 적응형 슬라이딩 모드 및 PID 게인
        private const float Kp = 120.0f;
        private const float Ki = 350.0f;
        private const float Kd = 0.08f;
        private const float SmcGain = 250.0f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public WinchServoState UpdateTensionControl(
            float measuredTensionKn,
            float aerostatPitchDeg,
            float aerostatZAccelG,
            float windSpeedMps)
        {
            // 1. 돌풍 급증에 따른 선제적 장력 예측 보정 (피드포워드)
            float gustDynamicForceKn = 0.5f * 1.225f * (windSpeedMps * windSpeedMps) * 45.0f * 0.001f;
            float verticalMotionInertiaKn = (aerostatZAccelG - 1.0f) * 12.0f; // 1200kg 유효 질량 가정
            float predictedTensionKn = measuredTensionKn + gustDynamicForceKn * 0.15f + verticalMotionInertiaKn;

            // 2. 장력 오차 계산 (장력이 타겟보다 높으면 드럼을 풀어주고, 낮으면 감음)
            float tensionErrorKn = predictedTensionKn - TargetTensionKn;

            // 3. 적응형 적분 연산 및 윈드업 방지
            _tensionIntegral += tensionErrorKn * Dt;
            _tensionIntegral = Math.Clamp(_tensionIntegral, -10.0f, 10.0f);

            float tensionDerivative = (tensionErrorKn - _prevTensionError) * SampleRateHz;
            _prevTensionError = tensionErrorKn;

            // 4. 슬라이딩 서피스 s = e + c * de
            float slidingSurface = tensionErrorKn + 0.02f * tensionDerivative;
            float smcTerm = SmcGain * MathF.Sign(slidingSurface);

            // 5. 요구 모터 토크 산출 (드럼을 회전시켜 케이블 송출 속도 제어)
            // 양의 오차(장력 과다) -> 양의 속도(케이블 풀기, Pay-out)
            float desiredAngularAccel = (Kp * tensionErrorKn + Ki * _tensionIntegral + Kd * tensionDerivative + smcTerm);

            // 동역학 역산: Torque = J * alpha + B * omega + R * Tension
            float cableLoadTorque = (measuredTensionKn * 1000.0f) * DrumRadiusMeters;
            float motorTorqueNm = (JTotal * desiredAngularAccel) + (BViscous * _drumVelocityRadS) - cableLoadTorque;

            // 토크 리미터 (-4500 Nm ~ +4500 Nm)
            motorTorqueNm = Math.Clamp(motorTorqueNm, -4500.0f, 4500.0f);

            // 6. 윈치 회전 속도 갱신
            float netAccel = (motorTorqueNm + cableLoadTorque - BViscous * _drumVelocityRadS) / JTotal;
            _drumVelocityRadS += netAccel * Dt;

            float drumRpm = _drumVelocityRadS * (60.0f / (2.0f * MathF.PI));
            bool emergencyBrake = measuredTensionKn >= MaxSafeTensionKn;

            return new WinchServoState(
                tension: measuredTensionKn,
                speed: drumRpm,
                torque: motorTorqueNm,
                brake: emergencyBrake
            );
        }
    }
}