AIDT 온디바이스 국방 MUM-T 호위함 능수동 예인소나(TASS) 케이블 장력 및 심도 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.AswTassWinch
{
    /// <summary>
    /// TASS 예인소나 케이블 및 심도 텔레메트리 상태
    /// </summary>
    public readonly ref struct TassTelemetryState
    {
        public readonly double CableTensionKiloNewtons;   // 선미 페어리더 케이블 장력 [kN]
        public readonly double TowBodyDepthMeters;        // 예인체 현재 수심 [m]
        public readonly double ShipSpeedKnots;            // 모함 예인 속력 [knots]
        public readonly double SternHeaveRateMps;         // 선미 상하 동요 속도 [m/s]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TassTelemetryState(double tension, double depth, double speed, double heaveRate)
        {
            CableTensionKiloNewtons = tension;
            TowBodyDepthMeters = depth;
            ShipSpeedKnots = speed;
            SternHeaveRateMps = heaveRate;
        }
    }

    /// <summary>
    /// 유압 윈치 및 디프레서 핀 서보 지령
    /// </summary>
    public readonly ref struct TassServoCommand
    {
        public readonly double WinchVelocityMps;          // 윈치 권선/인출 속도 (+출, -입) [m/s]
        public readonly double DepressorFinAngleDeg;      // 예인체 심도 제어 핀 각도 [deg]
        public readonly bool TensionWithinSafeLimit;      // 안전 장력 범위 유지 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TassServoCommand(double vWinch, double finDeg, bool safe)
        {
            WinchVelocityMps = vWinch;
            DepressorFinAngleDeg = finDeg;
            TensionWithinSafeLimit = safe;
        }
    }

    /// <summary>
    /// 10,000Hz 0-Alloc TASS 윈치 및 심도 안정화 서보 엔진
    /// </summary>
    public sealed class AswTassServoEngine
    {
        private const double NominalTensionKN = 45.0;     // 기준 장력 45 kN
        private const double MaxAllowableTensionKN = 110.0; // 파단 방지 한계 110 kN
        private const double TargetDepthM = 150.0;        // 수온약층 하부 목표 수심 150m
        private double _prevTension;
        private double _prevDepth;

        public AswTassServoEngine()
        {
            _prevTension = NominalTensionKN;
            _prevDepth = TargetDepthM;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TassServoCommand UpdateServoLoop(in TassTelemetryState telemetry, double dtSeconds)
        {
            // 1. 동적 장력 미분 및 과장력 완충 (Heave Feedforward)
            double tensionErr = telemetry.CableTensionKiloNewtons - NominalTensionKN;
            double tensionRate = (telemetry.CableTensionKiloNewtons - _prevTension) / dtSeconds;
            _prevTension = telemetry.CableTensionKiloNewtons;

            // 스냅 로드 방지: 장력 급증 시 즉각 와이어 인출 속도 지령
            double vWinch = (tensionErr * 0.04) + (tensionRate * 0.001) + (telemetry.SternHeaveRateMps * 0.85);
            vWinch = Math.Clamp(vWinch, -2.5, 4.0); // 권선 -2.5 m/s, 인출 +4.0 m/s

            // 2. 수심 오차 PD 제어 및 핀 각도 산출
            double depthErr = TargetDepthM - telemetry.TowBodyDepthMeters;
            double depthRate = (telemetry.TowBodyDepthMeters - _prevDepth) / dtSeconds;
            _prevDepth = telemetry.TowBodyDepthMeters;

            double cmdFin = (depthErr * 0.35) - (depthRate * 1.2);
            cmdFin = Math.Clamp(cmdFin, -20.0, 20.0); // 핀 타각 +/- 20도 한계

            bool safe = telemetry.CableTensionKiloNewtons < MaxAllowableTensionKN;
            return new TassServoCommand(vWinch, cmdFin, safe);
        }
    }
}