대한민국 공군 KF-21 보라매 유무인 복합체계(MUM-T) 초광대역 mmWave 데이터링크 지향 짐벌 제어 및 1,000Hz 0-Alloc C# 공간 편대 슬롯 관리 엔진

3. 1,000Hz 0-Alloc C# MUM-T mmWave 짐벌 지향 및 편대 슬롯 관리 구현

편대 비행 중 4대의 UCAV 상대 좌표를 실시간 추적하고 안테나 짐벌을 제어하는 제로 가비지 C# 코드입니다.

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

public ref struct MumtFormationGimbalController
{
    private readonly float _maxGimbalRateRad;
    private readonly float _kpGimbal;
    private float _prevAzimuth;
    private float _prevElevation;

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public MumtFormationGimbalController(float maxGimbalRateDegPerSec, float kp)
    {
        _maxGimbalRateRad = maxGimbalRateDegPerSec * (MathF.PI / 180.0f);
        _kpGimbal = kp;
        _prevAzimuth = 0.0f;
        _prevElevation = 0.0f;
    }

    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public void UpdateAntennaPointing(
        Vector3 kf21Pos,
        Quaternion kf21Attitude,
        Vector3 ucavPos,
        out float targetAzimuthRad,
        out float targetElevationRad,
        out float azRateCmd,
        out float elRateCmd)
    {
        Vector3 relPosWorld = ucavPos - kf21Pos;
        float distance = relPosWorld.Length();

        // 월드 좌표계 상대 벡터를 동체 좌표계로 회전 변환 (역쿼터니언)
        Quaternion invAttitude = Quaternion.Conjugate(kf21Attitude);
        Vector3 relPosBody = Vector3.Transform(relPosWorld, invAttitude);

        targetAzimuthRad = MathF.Atan2(relPosBody.Y, relPosBody.X);
        targetElevationRad = (distance > 0.01f) ? MathF.Asin(Math.Clamp(relPosBody.Z / distance, -1.0f, 1.0f)) : 0.0f;

        // 1,000Hz 각속도 명령 계산
        float azError = targetAzimuthRad - _prevAzimuth;
        float elError = targetElevationRad - _prevElevation;

        azRateCmd = Math.Clamp(_kpGimbal * azError * 1000.0f, -_maxGimbalRateRad, _maxGimbalRateRad);
        elRateCmd = Math.Clamp(_kpGimbal * elError * 1000.0f, -_maxGimbalRateRad, _maxGimbalRateRad);

        _prevAzimuth = targetAzimuthRad;
        _prevElevation = targetElevationRad;
    }
}