태양돛(Solar Sail) 광자 복사압(Radiation Pressure) 심우주 무연료 항법 & 1,000Hz 0-Alloc C# CoP-CoM 토크 상쇄 및 능동 반사 윙렛(Vane) 자세 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Numerics;

namespace ZeliDesk.Stem.SolarSail
{
    /// 

    /// 1,000Hz 태양돛 광압 복사력 및 CoP-CoM 교란 토크 상쇄 윙렛 틸트 서보 제어기
    /// GC 무할당 스택 메모리 0-Alloc 구현체
    /// 
    public ref struct SolarSailAttitudeServo
    {
        // 물리 상수
        private const float SolarFlux1AuWm2 = 1361.0f;
        private const float SpeedOfLightMps = 299792458.0f;
        private const float SailAreaM2 = 144.0f;             // 12m x 12m 사각형 돛
        private const float SailReflectivity = 0.92f;
        private const float VaneArmLengthM = 8.5f;           // 모서리 윙렛 모멘트 암
        private const float MaxVaneTiltAngleDeg = 45.0f;

        // 서보 제어 상태
        public float SunIncidenceAngleDeg;
        public float DisturbanceTorqueNmX;
        public float DisturbanceTorqueNmY;
        public float VaneTiltAngleDeg1;
        public float VaneTiltAngleDeg2;
        public float VaneTiltAngleDeg3;
        public float VaneTiltAngleDeg4;
        public float TotalNetThrustMn;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Initialize()
        {
            SunIncidenceAngleDeg = 35.0f;
            DisturbanceTorqueNmX = 0.0f;
            DisturbanceTorqueNmY = 0.0f;
            VaneTiltAngleDeg1 = 0.0f;
            VaneTiltAngleDeg2 = 0.0f;
            VaneTiltAngleDeg3 = 0.0f;
            VaneTiltAngleDeg4 = 0.0f;
            TotalNetThrustMn = 0.0f;
        }

        /// 

        /// 1ms 제어 주기: 태양 센서 앙각 및 자이로 각속도로부터 교란 토크 역산 및 4개 윙렛 각도 지령
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void StepAttitudeControl(
            float sunVectorAngleDeg, 
            float gyroRateDegPerSecX, 
            float gyroRateDegPerSecY, 
            float copOffsetMmX, 
            float copOffsetMmY,
            out float commandedThrustMn, 
            out float vane1Tilt, 
            out float vane2Tilt, 
            out float vane3Tilt, 
            out float vane4Tilt)
        {
            SunIncidenceAngleDeg = sunVectorAngleDeg;
            float alphaRad = SunIncidenceAngleDeg * (MathF.PI / 180.0f);
            float cosAlpha = MathF.Cos(alphaRad);

            // 1. 순수 태양 복사압 추력 계산: F = 2 * (I / c) * A * R * cos^2(alpha)
            float pRadNominal = (SolarFlux1AuWm2 / SpeedOfLightMps) * SailAreaM2;
            TotalNetThrustMn = (2.0f * pRadNominal * SailReflectivity * cosAlpha * cosAlpha) * 1000.0f; // mN 단위
            commandedThrustMn = TotalNetThrustMn;

            // 2. CoP-CoM 편차에 의한 태양광 외란 토크 산출 (tau = r x F)
            // 편차 mm -> m 변환
            float thrustNewtons = TotalNetThrustMn * 0.001f;
            DisturbanceTorqueNmX = (copOffsetMmY * 0.001f) * thrustNewtons;
            DisturbanceTorqueNmY = -(copOffsetMmX * 0.001f) * thrustNewtons;

            // 3. 자이로 댐핑 및 토크 상쇄 PID 피드포워드 제어기
            const float KpTorque = 120.0f; // deg / Nm
            const float KdRate = 15.0f;    // deg / (deg/s)

            float reqTiltX = DisturbanceTorqueNmX * KpTorque + gyroRateDegPerSecX * KdRate;
            float reqTiltY = DisturbanceTorqueNmY * KpTorque + gyroRateDegPerSecY * KdRate;

            // 4. 4개 윙렛 차동 각도 분배 (X/Y 축 대칭 분배)
            VaneTiltAngleDeg1 = MathF.Max(-MaxVaneTiltAngleDeg, MathF.Min(MaxVaneTiltAngleDeg, reqTiltX + reqTiltY));
            VaneTiltAngleDeg2 = MathF.Max(-MaxVaneTiltAngleDeg, MathF.Min(MaxVaneTiltAngleDeg, -reqTiltX + reqTiltY));
            VaneTiltAngleDeg3 = MathF.Max(-MaxVaneTiltAngleDeg, MathF.Min(MaxVaneTiltAngleDeg, -reqTiltX - reqTiltY));
            VaneTiltAngleDeg4 = MathF.Max(-MaxVaneTiltAngleDeg, MathF.Min(MaxVaneTiltAngleDeg, reqTiltX - reqTiltY));

            vane1Tilt = VaneTiltAngleDeg1;
            vane2Tilt = VaneTiltAngleDeg2;
            vane3Tilt = VaneTiltAngleDeg3;
            vane4Tilt = VaneTiltAngleDeg4;
        }
    }
}