우주-항공 유무인복합체계 초고속 자유공간 광통신(FSO) 빔 포인팅-포착-추적(PAT) 지터 안정화 서보 C# SIMD 엔진

3. Zero-Allocation C# SIMD 레이저 광통신 PAT 서보 엔진

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

namespace ZeliDesk.Defense.Lasercom.Fso
{
    public readonly ref struct LasercomPatState
    {
        public readonly float JitterUradX;
        public readonly float JitterUradY;
        public readonly float FsmVoltageX;
        public readonly float FsmVoltageY;
        public readonly bool LinkLocked;

        public LasercomPatState(float jx, float jy, float vx, float vy, bool locked)
        {
            JitterUradX = jx;
            JitterUradY = jy;
            FsmVoltageX = vx;
            FsmVoltageY = vy;
            LinkLocked = locked;
        }
    }

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

        // FSM 제어 게인
        private const float Kp = 3.2f;
        private const float Ki = 180.0f;
        private const float Kd = 0.00045f;
        private const float Kff = 0.85f;

        // 적분기 및 이전 오차 상태
        private float _intErrX = 0.0f, _prevErrX = 0.0f;
        private float _intErrY = 0.0f, _prevErrY = 0.0f;

        // 빔 추적 락 판정 임계값 (0.8 마이크로라디안)
        private const float LockThresholdUrad = 0.8f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public LasercomPatState TrackOpticalBeam(
            float qpdA, float qpdB, float qpdC, float qpdD,
            float gyroJitterRateX, float gyroJitterRateY)
        {
            float totalPower = qpdA + qpdB + qpdC + qpdD;
            if (totalPower < 1.0e-6f)
            {
                // 광신호 미수신 상태
                return new LasercomPatState(999.0f, 999.0f, 0.0f, 0.0f, false);
            }

            float invTotal = 1.0f / totalPower;

            // 1. QPD 각도 편차 정규화 계산 (urad 단위 환산 계수 곱)
            // 비례 상수: 15.0 urad / 정규화 단위
            float normX = ((qpdA + qpdD) - (qpdB + qpdC)) * invTotal;
            float normY = ((qpdA + qpdB) - (qpdC + qpdD)) * invTotal;

            float uradX = normX * 15.0f;
            float uradY = normY * 15.0f;

            // 2. X축 PID + Gyro 피드포워드
            _intErrX += uradX * Dt;
            _intErrX = Math.Clamp(_intErrX, -2.0f, 2.0f);
            float derX = (uradX - _prevErrX) * SampleRateHz;
            _prevErrX = uradX;

            float voltX = (Kp * uradX) + (Ki * _intErrX) + (Kd * derX) - (Kff * gyroJitterRateX);
            voltX = Math.Clamp(voltX, -10.0f, 10.0f);

            // 3. Y축 PID + Gyro 피드포워드
            _intErrY += uradY * Dt;
            _intErrY = Math.Clamp(_intErrY, -2.0f, 2.0f);
            float derY = (uradY - _prevErrY) * SampleRateHz;
            _prevErrY = uradY;

            float voltY = (Kp * uradY) + (Ki * _intErrY) + (Kd * derY) - (Kff * gyroJitterRateY);
            voltY = Math.Clamp(voltY, -10.0f, 10.0f);

            // 4. 추적 락 상태 검증
            float totalJitter = MathF.Sqrt(uradX * uradX + uradY * uradY);
            bool isLocked = totalJitter <= LockThresholdUrad;

            return new LasercomPatState(
                jx: uradX,
                jy: uradY,
                vx: voltX,
                vy: voltY,
                locked: isLocked
            );
        }
    }
}