AIDT 온디바이스 국방 MUM-T 무인기뢰처리기(MDV) 측면주사소나(SSS) 저고도 해저면 추종(Altitude-Hold) 심도 제어 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.MdvNavigation
{
    /// <summary>
    /// MDV 음향 고도계 및 항행 동역학 계측 상태
    /// </summary>
    public readonly ref struct MdvAltitudeTelemetryState
    {
        public readonly double AltitudeMeters;        // 해저면 정하방 고도 [m]
        public readonly double ForwardSpeedMps;       // 전진 속도 u [m/s]
        public readonly double VerticalSpeedMps;      // 수직 속도 w [m/s]
        public readonly double PitchAngleRad;         // 피치각 theta [rad]
        public readonly double ForwardObstacleDistM;  // 전방 장애물 탐지 거리 [m]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public MdvAltitudeTelemetryState(double alt, double u, double w, double pitch, double obsDist)
        {
            AltitudeMeters = alt;
            ForwardSpeedMps = u;
            VerticalSpeedMps = w;
            PitchAngleRad = pitch;
            ForwardObstacleDistM = obsDist;
        }
    }

    /// <summary>
    /// 수직 트러스터 및 잠타 제어 출력
    /// </summary>
    public readonly ref struct MdvAltitudeControlOutput
    {
        public readonly double VerticalThrusterNewtons; // 수직 트러스터 추력 [N]
        public readonly double SternElevatorCommandRad; // 잠타 타각 [rad]
        public readonly double AltitudeErrorMeters;     // 고도 유지 오차 [m]
        public readonly bool IsGroundProximityAlarm;    // 해저 충돌 위험(<1.5m) 경보

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public MdvAltitudeControlOutput(double thrust, double elevator, double err, bool alarm)
        {
            VerticalThrusterNewtons = thrust;
            SternElevatorCommandRad = elevator;
            AltitudeErrorMeters = err;
            IsGroundProximityAlarm = alarm;
        }
    }

    /// <summary>
    /// 10,000Hz 무인기뢰처리기 슬라이딩 모드 해저면 추종 서보 엔진
    /// </summary>
    public ref struct MdvAltitudeHoldServoEngine
    {
        private readonly double _netBuoyancyNewtons; // 순 잔여 부력 (W - B)
        private readonly double _c1Slope;
        private readonly double _c2Integral;
        private readonly double _ksmcGain;
        private readonly double _boundaryLayer;
        private readonly double _maxThrustN;

        private double _altIntegral;

        public MdvAltitudeHoldServoEngine(
            double netBuoyancyNewtons,
            double c1Slope,
            double c2Integral,
            double ksmcGain,
            double boundaryLayer,
            double maxThrustN)
        {
            _netBuoyancyNewtons = netBuoyancyNewtons;
            _c1Slope = c1Slope;
            _c2Integral = c2Integral;
            _ksmcGain = ksmcGain;
            _boundaryLayer = boundaryLayer;
            _maxThrustN = maxThrustN;
            _altIntegral = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public MdvAltitudeControlOutput ProcessCycle(
            in MdvAltitudeTelemetryState state,
            double targetAltitudeM,
            double dtSeconds)
        {
            // 1. 고도 오차 및 적분
            double altErr = state.AltitudeMeters - targetAltitudeM;
            _altIntegral += altErr * dtSeconds;

            // 2. 수직 고도 변화율
            double hDot = state.ForwardSpeedMps * Math.Sin(state.PitchAngleRad) - state.VerticalSpeedMps * Math.Cos(state.PitchAngleRad);

            // 전방 급경사 장애물 회피 바이어스 (10m 이내 장애물 감지 시 상승 바이어스)
            if (state.ForwardObstacleDistM < 10.0)
            {
                altErr -= (10.0 - state.ForwardObstacleDistM) * 0.4;
            }

            // 3. 슬라이딩 표면 S 계산
            double sAlt = altErr + _c1Slope * hDot + _c2Integral * _altIntegral;

            // 4. 경계층 포화 함수 sat(S / Phi)
            double satVal = Math.Clamp(sAlt / _boundaryLayer, -1.0, 1.0);

            // 5. 제어 추력 연산 (부력 보상 + SMC)
            double thrustCmd = _netBuoyancyNewtons - (_ksmcGain * satVal);
            thrustCmd = Math.Clamp(thrustCmd, -_maxThrustN, _maxThrustN);

            // 6. 잠타 타각 지령 (피치 자세 안정화 결합)
            double elevatorCmd = Math.Clamp(-0.2 * satVal - 0.5 * state.PitchAngleRad, -0.35, 0.35);

            // 충돌 경보 플래그 (1.5m 미만 근접 시)
            bool isAlarm = state.AltitudeMeters < 1.5;

            return new MdvAltitudeControlOutput(
                thrustCmd,
                elevatorCmd,
                altErr,
                isAlarm);
        }
    }
}