위성 편대비행(Formation Flying) 클로헤시-윌트셔 상대 궤도 & 1,000Hz 0-Alloc C# 최소연료 랑데부 제어 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.SatelliteFormation
{
    /// <summary>
    /// LVLH 좌표계 상대 위치 및 속도 상태 구조체
    /// </summary>
    public readonly ref struct LvlhRelativeState
    {
        public double XRadialMeters { get; }        // 방사 방향 변위 x (m)
        public double YInTrackMeters { get; }       // 진행 방향 변위 y (m)
        public double ZCrossTrackMeters { get; }    // 면외 방향 변위 z (m)
        public double VxRadialMps { get; }          // 방사 방향 속도 vx (m/s)
        public double VyInTrackMps { get; }         // 진행 방향 속도 vy (m/s)
        public double VzCrossTrackMps { get; }      // 면외 방향 속도 vz (m/s)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public LvlhRelativeState(double x, double y, double z, double vx, double vy, double vz)
        {
            XRadialMeters = x;
            YInTrackMeters = y;
            ZCrossTrackMeters = z;
            VxRadialMps = vx;
            VyInTrackMps = vy;
            VzCrossTrackMps = vz;
        }
    }

    /// <summary>
    /// 1,000Hz 0-Alloc C# CW 상대 궤도 유도 및 랑데부 펄스 엔진
    /// </summary>
    public ref struct ClohessyWiltshireEngine
    {
        private const double MuEarth = 3.986004418e14; // m^3/s^2
        private const double OrbitRadiusMeters = 6878000.0; // 500km 고도 (R_e + 500km)
        private readonly double _meanMotionN;

        private double _accumulatedDeltaVMps;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ClohessyWiltshireEngine(double initialDeltaV)
        {
            _meanMotionN = Math.Sqrt(MuEarth / Math.Pow(OrbitRadiusMeters, 3.0)); // ~0.001107 rad/s
            _accumulatedDeltaVMps = initialDeltaV;
        }

        /// <summary>
        /// 1,000Hz 실시간 루프 (dt = 0.001s) 표류율 계산 및 보정 추력 펄스 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ProcessStep(
            in LvlhRelativeState state,
            out double secularDriftRateMps,
            out double commandedThrustForceN,
            out bool isPeriodicOrbitClosed,
            out double distanceToTargetMeters)
        {
            // 1. 장주기 표류율 drift = -(6*n*x + 3*vy)
            secularDriftRateMps = -(6.0 * _meanMotionN * state.XRadialMeters + 3.0 * state.VyInTrackMps);

            // 2. 표류 소거를 위한 목표 속도 vy_target = -2 * n * x
            double vyTarget = -2.0 * _meanMotionN * state.XRadialMeters;
            double vyError = state.VyInTrackMps - vyTarget;

            // 3. 편대 위성 질량 25kg 기준 제어 추력 산출 (PD 서보)
            const double satelliteMassKg = 25.0;
            const double kp = 0.85; // N / (m/s)
            double thrustY = -satelliteMassKg * kp * vyError;
            commandedThrustForceN = Math.Clamp(thrustY, -0.50, 0.50); // 500mN 추력기 한계

            // 4. 상대 거리 및 폐곡선 주기성 판정
            distanceToTargetMeters = Math.Sqrt((state.XRadialMeters * state.XRadialMeters) +
                                               (state.YInTrackMeters * state.YInTrackMeters) +
                                               (state.ZCrossTrackMeters * state.ZCrossTrackMeters));

            isPeriodicOrbitClosed = Math.Abs(secularDriftRateMps) < 0.0005; // 0.5 mm/s 이내 drift

            const double dt = 0.001;
            double impulseDeltaV = Math.Abs(commandedThrustForceN / satelliteMassKg) * dt;
            _accumulatedDeltaVMps += impulseDeltaV;
        }
    }
}