AIDT 온디바이스 국방 MUM-T 해군 중어뢰 초공동 핀 제어기 수중 캐비티 경계면 추종 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.SupercavitatingFinTracking
{
    /// <summary>
    /// 초공동 어뢰 핀 침수 및 동역학 텔레메트리 상태
    /// </summary>
    public readonly ref struct SupercavFinTelemetryState
    {
        public readonly double TorpedoSpeedMps;           // 어뢰 전진 속도 [m/s]
        public readonly double FinWettedImmersionMm;      // 핀 수중 침수 깊이 [mm]
        public readonly double MeasuredFinLiftNewtons;    // 핀 발생 실측 양력 [N]
        public readonly double TailHullCavityDistanceMm;  // 선미 동체-캐비티 여유 간격 [mm]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavFinTelemetryState(double vMps, double hWetMm, double liftN, double distMm)
        {
            TorpedoSpeedMps = vMps;
            FinWettedImmersionMm = hWetMm;
            MeasuredFinLiftNewtons = liftN;
            TailHullCavityDistanceMm = distMm;
        }
    }

    /// <summary>
    /// 초공동 제어 핀 서보 지령
    /// </summary>
    public readonly ref struct FinActuatorServoCommand
    {
        public readonly double CommandedFinAngleDeg;      // 핀 타각 조향 지령 [deg]
        public readonly double TargetImmersionMm;         // 최적 목표 침수 깊이 [mm]
        public readonly bool PlaningSafeMaintained;       // 플래닝 충돌 회피 안전 상태

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public FinActuatorServoCommand(double deg, double targetH, bool safe)
        {
            CommandedFinAngleDeg = deg;
            TargetImmersionMm = targetH;
            PlaningSafeMaintained = safe;
        }
    }

    /// <summary>
    /// 10,000Hz 0-Alloc 초공동 핀 계면 추종 서보 엔진
    /// </summary>
    public sealed class SupercavitatingFinServoEngine
    {
        private const double OptimalImmersionMm = 14.0;    // 최적 14mm 침수선
        private const double DangerClearanceMm = 4.5;      // 플래닝 충돌 위험 한계 간격 4.5mm
        private double _currentFinAngleDeg;
        private double _prevImmersionMm;

        public SupercavitatingFinServoEngine()
        {
            _currentFinAngleDeg = 0.0;
            _prevImmersionMm = OptimalImmersionMm;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public FinActuatorServoCommand UpdateServoLoop(in SupercavFinTelemetryState telemetry, double dtSeconds)
        {
            // 1. 침수 깊이 변화 속도 추정
            double hRate = (telemetry.FinWettedImmersionMm - _prevImmersionMm) / dtSeconds;
            _prevImmersionMm = telemetry.FinWettedImmersionMm;

            // 2. 침수 깊이 오차 및 PD 서보 산출
            double hErr = OptimalImmersionMm - telemetry.FinWettedImmersionMm;
            double deltaAngle = (hErr * 0.18) - (hRate * 0.002);

            // 3. 선미 플래닝 충돌 방지 비상 역조향 바이패스
            if (telemetry.TailHullCavityDistanceMm < DangerClearanceMm)
            {
                // 선미가 캐비티 벽면으로 근접 시 즉각 동체를 중심선으로 밀어내는 긴급 타각 인가
                deltaAngle += (DangerClearanceMm - telemetry.TailHullCavityDistanceMm) * 1.5;
            }

            _currentFinAngleDeg = Math.Clamp(_currentFinAngleDeg + (deltaAngle * dtSeconds * 50.0), -16.0, 16.0);

            bool isSafe = (telemetry.TailHullCavityDistanceMm >= DangerClearanceMm) &&
                          (telemetry.FinWettedImmersionMm > 2.0);

            return new FinActuatorServoCommand(_currentFinAngleDeg, OptimalImmersionMm, isSafe);
        }
    }
}