한국형 발사체 누리호(KSLV-II) 3단부 롤 제어용 하이드라진 추력기(RCS) 펄스 폭 변조 및 1,000Hz 0-Alloc C# 궤도 투입 자세 제어 엔진

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

namespace ZeliDesk.Education.StemSpace.RocketRcs
{
    public readonly record struct RocketAttitudeTelemetry(
        Quaternion CurrentOrientation,
        Quaternion TargetOrientation,
        Vector3 AngularVelocityRadPerSec,
        float PropellantTankPressureBar
    );

    public ref struct RcsValveCommand
    {
        public bool PositiveRollValveOpen;
        public bool NegativeRollValveOpen;
        public float DutyCyclePercent;
        public bool AttitudeLocked;
    }

    public static class NuriRcsControlCore
    {
        private const float PwpfThresholdOn = 0.35f;
        private const float PwpfThresholdOff = 0.12f;
        private const float RollGainKp = 6.5f;
        private const float RollGainKd = 3.2f;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ModulateRcsThrusters(
            ref readonly RocketAttitudeTelemetry tel,
            ref float pwpfFilterState, // 1차 필터 상태 변수 (스택 관리)
            float dtSec,
            ref RcsValveCommand cmd)
        {
            // 1. 쿼터니언 오차로부터 롤 각도 오차 역산
            Quaternion qErr = Quaternion.Inverse(tel.CurrentOrientation) * tel.TargetOrientation;
            float rollErrorRad = 2.0f * (float)Math.Atan2(qErr.X, qErr.W);

            // 2. PD 제어 입력 산출
            float controlDemand = (rollErrorRad * RollGainKp) - (tel.AngularVelocityRadPerSec.X * RollGainKd);

            // 3. PWPF 1차 지연 필터 적분
            float filterTimeConstant = 0.05f;
            pwpfFilterState += (dtSec / filterTimeConstant) * (controlDemand - pwpfFilterState);

            // 4. 슈미트 트리거 이중 임계값 판정
            bool firePos = false;
            bool fireNeg = false;

            if (pwpfFilterState > PwpfThresholdOn)
            {
                firePos = true;
            }
            else if (pwpfFilterState < -PwpfThresholdOn)
            {
                fireNeg = true;
            }

            cmd.PositiveRollValveOpen = firePos;
            cmd.NegativeRollValveOpen = fireNeg;
            cmd.DutyCyclePercent = Math.Clamp(Math.Abs(pwpfFilterState) * 100.0f, 0.0f, 100.0f);
            cmd.AttitudeLocked = Math.Abs(rollErrorRad) < 0.005f; // 0.3도 이내 안정
        }
    }
}