대한민국 해군 KSS-III Batch-II 잠수함 무반동 어뢰발사 밸러스트 수중 균형 및 1,000Hz 0-Alloc C# 트림 자세 복원 제어 엔진

3. 1,000Hz 0-Alloc C# KSS-III 잠수함 발사 밸러스트 트림 제어 구현

어뢰 사출 신호와 동시에 1ms 주기로 오리피스 밸브 서보 타각을 연산하고 잠수함 피치 각도를 0.1도 이내로 유지하는 제로 가비지 C# 코드입니다.

using System;
using System.Runtime.CompilerServices;

public ref struct SubmarineTrimBallastController
{
    private readonly float _torpedoMassKg;
    private readonly float _tubeDistFromCgM;
    private readonly float _tankDistFromCgM;
    private readonly float _kpPitch;
    private readonly float _kdPitch;
    private float _injectedWaterMassKg;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public SubmarineTrimBallastController(float torpMass, float tubeDist, float tankDist, float kp, float kd)
    {
        _torpedoMassKg = torpMass;
        _tubeDistFromCgM = tubeDist;
        _tankDistFromCgM = tankDist;
        _kpPitch = kp;
        _kdPitch = kd;
        _injectedWaterMassKg = 0.0f;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void UpdateTrimValve(
        bool isEjecting,
        float currentPitchRad,
        float currentPitchRateRad,
        float depthMeter,
        float dt,
        out float valveOpeningRatio,
        out float remainingDeficitKg)
    {
        if (isEjecting)
        {
            float targetWaterKg = _torpedoMassKg * (_tubeDistFromCgM / _tankDistFromCgM);
            remainingDeficitKg = targetWaterKg - _injectedWaterMassKg;

            // 수심에 따른 해수 유입 압력 보정 (h * rho * g)
            float differentialHead = MathF.Max(depthMeter, 5.0f);
            float maxFlowRateKgPerSec = 450.0f * MathF.Sqrt(differentialHead / 50.0f);

            // 피치 오차 피드백 제어
            float pitchCorrection = _kpPitch * currentPitchRad + _kdPitch * currentPitchRateRad;
            float desiredFlow = (remainingDeficitKg / 0.15f) + pitchCorrection * 200.0f;

            valveOpeningRatio = Math.Clamp(desiredFlow / maxFlowRateKgPerSec, 0.0f, 1.0f);
            float actualFlowKgPerSec = valveOpeningRatio * maxFlowRateKgPerSec;
            _injectedWaterMassKg += actualFlowKgPerSec * dt;
        }
        else
        {
            valveOpeningRatio = 0.0f;
            remainingDeficitKg = 0.0f;
        }
    }
}