대한민국 육군 K2 흑표 전차 헌터-킬러 연동 전차장조준경(KCPS) 2축 자이로 안정화 및 1,000Hz 0-Alloc C# 시선(LOS) 제어 엔진

3. 1,000Hz 0-Alloc C# K2 KCPS 시선 안정화 및 헌터-킬러 제어 엔진

자이로 각속도계와 앤코더 센서를 1ms 주기로 융합하여 짐벌 BLDC 모터 구동 토크를 직교 분배하는 고속 C# 코드입니다.

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

public ref struct K2KcpsLosStabilizationEngine
{
    private readonly float _azimuthKp;
    private readonly float _azimuthKd;
    private readonly float _elevationKp;
    private readonly float _elevationKd;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public K2KcpsLosStabilizationEngine(
        float azKp = 1200.0f,
        float azKd = 85.0f,
        float elKp = 1500.0f,
        float elKd = 95.0f)
    {
        _azimuthKp = azKp;
        _azimuthKd = azKd;
        _elevationKp = elKp;
        _elevationKd = elKd;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void RegulateLineOfSight(
        float hullYawRateDegPerSec,
        float hullPitchRateDegPerSec,
        float sightAzimuthDeg,
        float sightElevationDeg,
        float targetAzimuthDeg,
        float targetElevationDeg,
        bool hunterKillerTriggered,
        out float azMotorTorqueNm,
        out float elMotorTorqueNm,
        out bool gunAlignedReadyToFire)
    {
        // 1. 시선 각도 오차 계산
        float azErrorDeg = targetAzimuthDeg - sightAzimuthDeg;
        float elErrorDeg = targetElevationDeg - sightElevationDeg;

        // 2. 외란 역위상 피드포워드 + 오차 피드백 토크 합성
        float azHullRateRad = hullYawRateDegPerSec * (MathF.PI / 180.0f);
        float elHullRateRad = hullPitchRateDegPerSec * (MathF.PI / 180.0f);

        azMotorTorqueNm = (_azimuthKp * azErrorDeg) - (_azimuthKd * azHullRateRad);
        elMotorTorqueNm = (_elevationKp * elErrorDeg) - (_elevationKd * elHullRateRad);

        // 모터 최대 토크 리미터 적용 (50 Nm)
        azMotorTorqueNm = Math.Clamp(azMotorTorqueNm, -50.0f, 50.0f);
        elMotorTorqueNm = Math.Clamp(elMotorTorqueNm, -50.0f, 50.0f);

        // 3. 헌터-킬러 정합 완료 판정 (오차 0.05도 이내 및 트리거 활성화 상태)
        gunAlignedReadyToFire = hunterKillerTriggered && 
                                (MathF.Abs(azErrorDeg) < 0.05f) && 
                                (MathF.Abs(elErrorDeg) < 0.05f);
    }
}