대한민국 해군 충무공이순신급 구축함 가변심도소나(VDS) 예인 케이블 카테너리 인장력 및 1,000Hz 0-Alloc C# 수온약층 관통 능동 심도 제어 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.Navy.VdsAcoustics
{
    public readonly record struct VdsSensorTelemetry(
        float ShipSpeedKnots,
        float SternHeaveAccelMps2,
        float CablePayoutLengthMeters,
        float CableTensionKiloNewtons,
        float BodyMeasuredDepthMeters
    );

    public ref struct WinchServoCommand
    {
        public float WinchMotorRpm;
        public float CommandedTensionKn;
        public float DepthErrorMeters;
        public bool ActiveCompensationEngaged;
    }

    public static class YisunshinVdsCore
    {
        private const float DrumRadiusMeters = 0.65f;
        private const float MaxTensionLimitKn = 120.0f;
        private const float Kp = 1.8f;
        private const float Kd = 2.4f;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void UpdateWinchCompensation(
            ref readonly VdsSensorTelemetry telemetry,
            float targetDepthMeters,
            float dtSeconds,
            ref WinchServoCommand command)
        {
            float depthError = targetDepthMeters - telemetry.BodyMeasuredDepthMeters;
            command.DepthErrorMeters = depthError;

            // 1. 함미 상하 운동(Heave) 속도 적분 선행 보상
            float heaveVelocity = telemetry.SternHeaveAccelMps2 * dtSeconds;

            // 2. 심도 오차 PD 제어
            float reqLineVelocityMps = (Kp * depthError) - (heaveVelocity * 1.2f);

            // 3. 윈치 드럼 RPM 환산
            float drumCircumference = 2.0f * MathF.PI * DrumRadiusMeters;
            float targetRpm = (reqLineVelocityMps / drumCircumference) * 60.0f;

            // 윈치 모터 속도 리미터 (-150 rpm ~ +150 rpm)
            targetRpm = MathF.Min(MathF.Max(targetRpm, -150.0f), 150.0f);

            // 4. 케이블 장력 한계 보호
            if (telemetry.CableTensionKiloNewtons > MaxTensionLimitKn)
            {
                // 장력 과다 시 강제 풀림(Payout) 모드로 전환
                targetRpm = 60.0f;
                command.ActiveCompensationEngaged = false;
            }
            else
            {
                command.ActiveCompensationEngaged = true;
            }

            command.WinchMotorRpm = targetRpm;
            command.CommandedTensionKn = MathF.Min(telemetry.CableTensionKiloNewtons, MaxTensionLimitKn);
        }
    }
}