대한민국 육군 KTSSM-II 전술지대지유도탄 마하 5+ 대기권 열차폐 탄두 콘 공력 가열 해석 및 1,000Hz 0-Alloc C# 3차원 핀-롤 유도 제어 엔진

3. 1,000Hz 0-Alloc C# KTSSM-II 공력 핀-롤 유도 제어 엔진 구현

마하 5의 극초음속 급강하 중 1ms 제어 주기 내에서 쿼터니언 자세 오차를 보정하고 4개 공력 핀 서보 각도를 분배하는 무할당 C# 코드입니다.

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

public ref struct Ktssm2FinGuidanceEngine
{
    private readonly float _maxFinDeflectionRad;
    private readonly float _kpRoll;
    private readonly float _kdRoll;
    private float _prevRollError;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public Ktssm2FinGuidanceEngine(float maxFinDeg, float kpRoll, float kdRoll)
    {
        _maxFinDeflectionRad = maxFinDeg * (MathF.PI / 180.0f);
        _kpRoll = kpRoll;
        _kdRoll = kdRoll;
        _prevRollError = 0.0f;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void ComputeFinDeflections(
        Vector3 accelCmd,
        float currentRollRad,
        float currentRollRateRad,
        float dynamicPressureQ,
        out float fin1, out float fin2, out float fin3, out float fin4)
    {
        // BTT 롤 지향 각도 연산
        float targetRoll = MathF.Atan2(accelCmd.Y, accelCmd.Z);
        float rollError = targetRoll - currentRollRad;

        // 각도 언래핑 [-PI, PI]
        while (rollError > MathF.PI) rollError -= 2.0f * MathF.PI;
        while (rollError < -MathF.PI) rollError += 2.0f * MathF.PI;

        float rollControl = _kpRoll * rollError - _kdRoll * currentRollRateRad;
        _prevRollError = rollError;

        // 동압(q) 기반 공력 타각 게인 적응
        float qFactor = Math.Clamp(dynamicPressureQ / 120000.0f, 0.4f, 1.5f);
        float pitchCmd = accelCmd.Z / (50.0f * qFactor);
        float yawCmd = accelCmd.Y / (50.0f * qFactor);

        // 4개 조종 핀 믹싱
        fin1 = Math.Clamp(rollControl + pitchCmd + yawCmd, -_maxFinDeflectionRad, _maxFinDeflectionRad);
        fin2 = Math.Clamp(rollControl - pitchCmd + yawCmd, -_maxFinDeflectionRad, _maxFinDeflectionRad);
        fin3 = Math.Clamp(rollControl - pitchCmd - yawCmd, -_maxFinDeflectionRad, _maxFinDeflectionRad);
        fin4 = Math.Clamp(rollControl + pitchCmd - yawCmd, -_maxFinDeflectionRad, _maxFinDeflectionRad);
    }
}