AIDT 온디바이스 국방 MUM-T 무인 자율 모함(Mothership) 군집 자폭 드론 자동 재장전 및 급속 발사 캐러셀(Carousel) 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.DroneCarrier
{
    /// <summary>
    /// 선체 해상 요동 및 캐러셀 회전 계측 상태
    /// </summary>
    public readonly ref struct CarrierLaunchTelemetryState
    {
        public readonly double CarouselAngleDeg;     // 현재 캐러셀 탄창 각도 [deg]
        public readonly double ShipRollAngleDeg;     // 선체 롤 각도 [deg]
        public readonly double ShipPitchAngleDeg;    // 선체 피치 각도 [deg]
        public readonly double PneumaticPressureBar; // 공압 사출 챔버 압력 [bar]
        public readonly int RemainingDroneCount;     // 잔여 드론 수량

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CarrierLaunchTelemetryState(double carouselDeg, double roll, double pitch, double press, int count)
        {
            CarouselAngleDeg = carouselDeg;
            ShipRollAngleDeg = roll;
            ShipPitchAngleDeg = pitch;
            PneumaticPressureBar = press;
            RemainingDroneCount = count;
        }
    }

    /// <summary>
    /// 캐러셀 모터 및 사출 밸브 제어 출력
    /// </summary>
    public readonly ref struct CarrierLaunchControlOutput
    {
        public readonly double CarouselMotorTorqueNm; // 캐러셀 인덱싱 모터 토크 [N*m]
        public readonly double PlatformPitchCompDeg;  // 스튜어트 플랫폼 피치 보정각 [deg]
        public readonly double PlatformRollCompDeg;   // 스튜어트 플랫폼 롤 보정각 [deg]
        public readonly bool FirePistonSolenoid;      // 공압 사출 솔레노이드 격발 플래그
        public readonly bool IsAlignedForLaunch;      // 튜브 정렬 오차 < 0.05도 달성 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CarrierLaunchControlOutput(double torque, double pPitch, double pRoll, bool fire, bool aligned)
        {
            CarouselMotorTorqueNm = torque;
            PlatformPitchCompDeg = pPitch;
            PlatformRollCompDeg = pRoll;
            FirePistonSolenoid = fire;
            IsAlignedForLaunch = aligned;
        }
    }

    /// <summary>
    /// 10,000Hz 무인 드론 모함 캐러셀 재장전 및 급속 사출 서보 엔진
    /// </summary>
    public ref struct DroneCarrierCarouselServoEngine
    {
        private const double SlotPitchDeg = 5.625; // 64발 기준 슬롯당 360 / 64 = 5.625도
        private readonly double _kpAngle;
        private readonly double _kdAngle;
        private readonly double _maxMotorTorqueNm;
        private readonly double _minPneumaticBar;

        private double _prevErrorDeg;
        private double _cycleTimerSec;

        public DroneCarrierCarouselServoEngine(
            double kpAngle,
            double kdAngle,
            double maxMotorTorqueNm,
            double minPneumaticBar)
        {
            _kpAngle = kpAngle;
            _kdAngle = kdAngle;
            _maxMotorTorqueNm = maxMotorTorqueNm;
            _minPneumaticBar = minPneumaticBar;
            _prevErrorDeg = 0.0;
            _cycleTimerSec = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CarrierLaunchControlOutput ProcessCycle(
            in CarrierLaunchTelemetryState state,
            int targetSlotIndex,
            bool launchTriggerArmed,
            double dtSeconds)
        {
            _cycleTimerSec += dtSeconds;

            // 1. 목표 슬롯 각도 계산 (64슬롯 캐러셀)
            double targetAngleDeg = (targetSlotIndex % 64) * SlotPitchDeg;
            double angleErr = targetAngleDeg - state.CarouselAngleDeg;

            // 2. PD 각도 서보 제어
            double dErr = (angleErr - _prevErrorDeg) / dtSeconds;
            _prevErrorDeg = angleErr;

            double torque = _kpAngle * angleErr + _kdAngle * dErr;
            torque = Math.Clamp(torque, -_maxMotorTorqueNm, _maxMotorTorqueNm);

            // 3. 선체 요동 상쇄 스튜어트 플랫폼 보정 각도
            double compPitch = -state.ShipPitchAngleDeg;
            double compRoll = -state.ShipRollAngleDeg;

            // 4. 발사관 정렬 판정 (0.05도 이내)
            bool isAligned = Math.Abs(angleErr) <= 0.05;

            // 5. 공압 사출 격발 조건 (정렬 완료 + 압력 충분 + 방아쇠 인가 + 3초 쿨다운)
            bool fireSolenoid = isAligned &&
                                (state.PneumaticPressureBar >= _minPneumaticBar) &&
                                launchTriggerArmed &&
                                (_cycleTimerSec >= 3.0);

            if (fireSolenoid)
            {
                _cycleTimerSec = 0.0; // 발사 후 타이머 리셋
            }

            return new CarrierLaunchControlOutput(
                torque,
                compPitch,
                compRoll,
                fireSolenoid,
                isAligned);
        }
    }
}