해군 무인잠수정(UUV) 다중 군집 협업 수중 음향 통신 TDMA 시분할 도플러 보정 서보 C# SIMD 엔진

3. Zero-Allocation C# SIMD 수중 음향 TDMA 도플러 보정 서보 엔진

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

namespace ZeliDesk.Defense.Uuv.AcousticTdma
{
    public readonly ref struct UuvAcousticState
    {
        public readonly float DopplerShiftHz;
        public readonly float PhaseErrorRad;
        public readonly float NcoFrequencyOffset;
        public readonly bool FrameSynchronized;

        public UuvAcousticState(float doppler, float phase, float offset, bool sync)
        {
            DopplerShiftHz = doppler;
            PhaseErrorRad = phase;
            NcoFrequencyOffset = offset;
            FrameSynchronized = sync;
        }
    }

    public sealed class UuvAcousticTdmaServoEngine
    {
        private const float SampleRateHz = 10000.0f;
        private const float Dt = 1.0f / SampleRateHz;

        // 중심 초음파 반송파 주파수 (25 kHz)
        private const float CarrierFreqHz = 25000.0f;
        private const float SoundSpeedMps = 1500.0f;

        // 2차 디지털 위상 잠금 루프 (DPLL) 파라미터
        private float _ncoPhase = 0.0f;
        private float _frequencyIntegrator = 0.0f;
        private const float KpPll = 0.45f;
        private const float KiPll = 18.0f;

        // TDMA 프레임 동기화 상태
        private int _sampleCountInSlot = 0;
        private const int SlotLengthSamples = 5000; // 0.5초 슬롯

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public UuvAcousticState ProcessBasebandSample(
            float inPhaseI, float quadPhaseQ,
            float relativeVelocityMps)
        {
            // 1. 잠수정 기동 속도로부터 선제적 도플러 피드포워드 추정
            float dopplerFfHz = (relativeVelocityMps / SoundSpeedMps) * CarrierFreqHz;

            // 2. 코스타스 위상 오차 검출: error = sign(I) * Q - sign(Q) * I (QPSK)
            // BPSK/QPSK 위상 에러 추정
            float signI = inPhaseI >= 0.0f ? 1.0f : -1.0f;
            float phaseError = quadPhaseQ * signI;

            // 3. 2차 루프 필터 (PI 제어)
            _frequencyIntegrator += (KiPll * phaseError) * Dt;
            _frequencyIntegrator = Math.Clamp(_frequencyIntegrator, -250.0f, 250.0f);

            float freqAdjustmentHz = dopplerFfHz + (KpPll * phaseError) + _frequencyIntegrator;

            // 4. 수치제어 발진기 (NCO) 위상 누적
            _ncoPhase += 2.0f * MathF.PI * (CarrierFreqHz + freqAdjustmentHz) * Dt;
            if (_ncoPhase >= 2.0f * MathF.PI) _ncoPhase -= 2.0f * MathF.PI;

            // 5. TDMA 슬롯 카운터 및 패킷 동기 판정
            _sampleCountInSlot++;
            if (_sampleCountInSlot >= SlotLengthSamples)
            {
                _sampleCountInSlot = 0;
            }

            bool isSync = MathF.Abs(phaseError) < 0.08f;

            return new UuvAcousticState(
                doppler: freqAdjustmentHz,
                phase: phaseError,
                offset: freqAdjustmentHz,
                sync: isSync
            );
        }
    }
}