대한민국 공군 중고도 정찰용 무인항공기(MUAV, KUS-FS) Ku/Ka-밴드 위성통신(SATCOM) 능동 위상배열 2축 짐벌 시선 안정화 & 전리층 페이딩 보상 1,000Hz 0-Alloc C# 항전 통신 제어 엔진

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;

namespace ZeliDesk.Defense.AerospaceSatcom
{
    /// <summary>
    /// 3차원 위치 및 방향 벡터 구조체 (16바이트 정렬)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 16)]
    public readonly struct Vector3D
    {
        public readonly float X;
        public readonly float Y;
        public readonly float Z;
        public readonly float W; // 패딩

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector3D(float x, float y, float z)
        {
            X = x; Y = y; Z = z; W = 0f;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public float LengthSquared() => X * X + Y * Y + Z * Z;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public float Length() => MathF.Sqrt(LengthSquared());
    }

    /// <summary>
    /// 자세 쿼터니언 (w, x, y, z)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 16)]
    public readonly struct Quaternion4D
    {
        public readonly float W;
        public readonly float X;
        public readonly float Y;
        public readonly float Z;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Quaternion4D(float w, float x, float y, float z)
        {
            W = w; X = x; Y = y; Z = z;
        }
    }

    /// <summary>
    /// SATCOM 짐벌 서보 및 AESA 제어 명령 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct SatcomControlOutput
    {
        public float AzimuthAngleRad;       // 방위각 짐벌 지령 (rad)
        public float ElevationAngleRad;     // 고각 짐벌 지령 (rad)
        public float AzimuthRateRps;        // 방위각 모터 각속도 지령 (rad/s)
        public float ElevationRateRps;      // 고각 모터 각속도 지령 (rad/s)
        public float AesaBeamOffsetAz;      // AESA 정밀 미세 빔 조향 방위각 (rad)
        public float AesaBeamOffsetEl;      // AESA 정밀 미세 빔 조향 고각 (rad)
        public float TxPowerCompensationDb; // 전리층/강우 보상 송신 이득 (dB)
        public bool IsLinkLocked;           // 시선 추적 잠금 여부 (True < 0.05 deg)
    }

    /// <summary>
    /// MUAV Ku/Ka-밴드 위성통신 2축 짐벌 시선 안정화 1,000Hz 0-Alloc C# 제어 엔진
    /// </summary>
    public sealed class MuavSatcomGimbalEngine
    {
        private const float DegToRad = MathF.PI / 180f;
        private const float RadToDeg = 180f / MathF.PI;
        private const float TrackingLockThresholdRad = 0.05f * DegToRad; // 0.05도 오차 임계치

        // 짐벌 서보 PID 게인
        private readonly float _kpAz = 45.0f;
        private readonly float _kdAz = 2.8f;
        private readonly float _kpEl = 50.0f;
        private readonly float _kdEl = 3.2f;

        // 짐벌 관성 모멘트 (kg*m^2)
        private readonly float _jAz = 0.085f;
        private readonly float _jEl = 0.062f;

        // 내부 상태 보관 (0-Alloc)
        private float _currentAzRad;
        private float _currentElRad;
        private float _prevAzError;
        private float _prevElError;

        public MuavSatcomGimbalEngine()
        {
            _currentAzRad = 0f;
            _currentElRad = 45f * DegToRad; // 초기 고각 45도
            _prevAzError = 0f;
            _prevElError = 0f;
        }

        /// <summary>
        /// 1,000Hz 실시간 틱 연산: 기체 6자유도 자세와 위성 위치로부터 시선 안정화 지령 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateTick(
            in Vector3D uavPositionNed,         // NED 기준 무인기 위치 (m)
            in Quaternion4D uavAttitude,        // 기체 자세 쿼터니언 (Navigation -> Body)
            in Vector3D uavAngularRateBodyRps,  // 자이로 각속도 (p, q, r)
            in Vector3D geoSatellitePosNed,     // 정지궤도 위성 NED 위치 (m)
            float atmosphericRainRateMmHr,      // 지상/고도별 강우율 (mm/h)
            float ionosphereTec,                // 전리층 TEC (10^16 e/m^2)
            float dt,                           // 루프 주기 (0.001초)
            ref SatcomControlOutput output)
        {
            // 1. 위성 상대 시선 벡터 산출 (NED 프레임)
            float dx = geoSatellitePosNed.X - uavPositionNed.X;
            float dy = geoSatellitePosNed.Y - uavPositionNed.Y;
            float dz = geoSatellitePosNed.Z - uavPositionNed.Z;
            float rangeInv = 1.0f / MathF.Sqrt(dx * dx + dy * dy + dz * dz);

            float losNedX = dx * rangeInv;
            float losNedY = dy * rangeInv;
            float losNedZ = dz * rangeInv;

            // 2. 쿼터니언을 이용하여 시선 벡터를 동체 좌표계(Body Frame)로 변환
            // v_b = q* * v_n * q
            RotateVectorByQuatConj(losNedX, losNedY, losNedZ, in uavAttitude, out float losBodyX, out float losBodyY, out float losBodyZ);

            // 3. 동체 기준 목표 짐벌 각도 계산 (Azimuth / Elevation)
            float targetAz = MathF.Atan2(losBodyY, losBodyX);
            float xyDist = MathF.Sqrt(losBodyX * losBodyX + losBodyY * losBodyY);
            float targetEl = MathF.Atan2(-losBodyZ, xyDist); // NED에서 Down이 양수이므로 -Z가 상향

            // 4. 추적 오차 산출
            float azError = targetAz - _currentAzRad;
            float elError = targetEl - _currentElRad;

            // 5. 기체 회전 각속도 피드포워드 보상 (외란 제거)
            // Roll(p), Pitch(q), Yaw(r) 각속도 반영
            float azFeedForwardRate = -uavAngularRateBodyRps.Z; // Yaw 역방향 보상
            float elFeedForwardRate = -(uavAngularRateBodyRps.Y * MathF.Cos(_currentAzRad) - uavAngularRateBodyRps.X * MathF.Sin(_currentAzRad));

            // 6. PID 피드백 및 모터 제어 지령
            float azRateCmd = azFeedForwardRate + (_kpAz * azError) + (_kdAz * (azError - _prevAzError) / dt);
            float elRateCmd = elFeedForwardRate + (_kpEl * elError) + (_kdEl * (elError - _prevElError) / dt);

            _prevAzError = azError;
            _prevElError = elError;

            // 7. 기계식 짐벌 위치 갱신
            _currentAzRad += azRateCmd * dt;
            _currentElRad += elRateCmd * dt;

            // 8. 잔여 미세 각도 오차를 AESA 위상배열 전자식 빔포밍으로 신속 흡수
            float residualAz = targetAz - _currentAzRad;
            float residualEl = targetEl - _currentElRad;

            // 9. 대기 강우 및 전리층 TEC에 따른 송신 이득 보상 연산
            // ITU-R P.838 모델 약산: gamma_R = 0.18 * R^1.05 at 30GHz
            float rainLossDb = 0.187f * MathF.Pow(MathF.Max(0f, atmosphericRainRateMmHr), 1.05f) * 4.5f; // 유효 강우 경로 4.5km
            float tecPhaseDelayRad = (40.3e16f * ionosphereTec) / (9e20f) * (2f * MathF.PI / 0.01f); // Ka-밴드 30GHz
            float totalCompDb = MathF.Min(12.0f, rainLossDb + (tecPhaseDelayRad * 0.15f));

            // 10. 출력 구조체 기록
            output.AzimuthAngleRad = _currentAzRad;
            output.ElevationAngleRad = _currentElRad;
            output.AzimuthRateRps = azRateCmd;
            output.ElevationRateRps = elRateCmd;
            output.AesaBeamOffsetAz = residualAz;
            output.AesaBeamOffsetEl = residualEl;
            output.TxPowerCompensationDb = totalCompDb;
            output.IsLinkLocked = (MathF.Abs(residualAz) < TrackingLockThresholdRad) && (MathF.Abs(residualEl) < TrackingLockThresholdRad);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void RotateVectorByQuatConj(
            float vx, float vy, float vz,
            in Quaternion4D q,
            out float rx, out float ry, out float rz)
        {
            // q conjugate: (w, -x, -y, -z)
            float qw = q.W, qx = -q.X, qy = -q.Y, qz = -q.Z;

            // t = 2 * cross(q.xyz, v)
            float tx = 2f * (qy * vz - qz * vy);
            float ty = 2f * (qz * vx - qx * vz);
            float tz = 2f * (qx * vy - qy * vx);

            // v' = v + q.w * t + cross(q.xyz, t)
            rx = vx + qw * tx + (qy * tz - qz * ty);
            ry = vy + qw * ty + (qz * tx - qx * tz);
            rz = vz + qw * tz + (qx * ty - qy * tx);
        }
    }
}