AIDT 온디바이스 STEM 방사광 X선 극저온 qPlus 석영 튜닝포크 AFM 위상 잠금(PLL) 서보 제어기

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.CryoQPlusAfm
{
    /// <summary>
    /// qPlus 석영 튜닝포크 공진 텔레메트리 상태
    /// </summary>
    public readonly ref struct QPlusAfmTelemetryState
    {
        public readonly double FrequencyShiftHz;          // 주파수 편이 Delta f [Hz]
        public readonly double OscillationAmplitudePicoM; // 발진 진폭 A [pm]
        public readonly double PhaseErrorRad;             // PLL 위상 오차 [rad]
        public readonly double TunnelingCurrentPicoAmps;  // 동시 측정 터널링 전류 [pA]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public QPlusAfmTelemetryState(double df, double amp, double phaseErr, double iTunnel)
        {
            FrequencyShiftHz = df;
            OscillationAmplitudePicoM = amp;
            PhaseErrorRad = phaseErr;
            TunnelingCurrentPicoAmps = iTunnel;
        }
    }

    /// <summary>
    /// Z축 PZT 및 AGC 구동 전압 지령
    /// </summary>
    public readonly ref struct QPlusAfmServoCommand
    {
        public readonly double ZPiezoDisplacementPicoM;   // Z 피에조 변위 지령 [pm]
        public readonly double ExcitationDriveMilliVolts; // 자려 발진 AGC 구동 전압 [mV]
        public readonly bool FeedbackLocked;              // 원자간력 록킹 안정화 상태

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public QPlusAfmServoCommand(double zPicoM, double vDrive, bool locked)
        {
            ZPiezoDisplacementPicoM = zPicoM;
            ExcitationDriveMilliVolts = vDrive;
            FeedbackLocked = locked;
        }
    }

    /// <summary>
    /// 10,000Hz 0-Alloc 극저온 qPlus AFM 서보 엔진
    /// </summary>
    public sealed class CryoQPlusAfmServoEngine
    {
        private const double TargetFrequencyShift = -8.5;  // 설정 주파수 편이 Delta f_set [Hz]
        private const double TargetAmplitudePicoM = 50.0;  // 설정 진폭 50 pm
        private const double KpZ = 120.0;                  // Z-PZT 비례 게인 [pm/Hz]
        private const double KiZ = 450.0;                  // Z-PZT 적분 게인
        private double _zIntegral;
        private double _currentZPicoM;
        private double _driveAmplitude;

        public CryoQPlusAfmServoEngine()
        {
            _zIntegral = 0.0;
            _currentZPicoM = 0.0;
            _driveAmplitude = 15.0; // 15 mV
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public QPlusAfmServoCommand UpdateServoLoop(in QPlusAfmTelemetryState telemetry, double dtSeconds)
        {
            // 1. 주파수 편이 에러 계산 (Delta f - Delta f_set)
            double dfError = telemetry.FrequencyShiftHz - TargetFrequencyShift;

            // 2. 안티-와인드업 적분 연산
            _zIntegral = Math.Clamp(_zIntegral + (dfError * dtSeconds), -500.0, 500.0);

            // 3. Z축 피에조 변위 지령 (시료 충돌 방지 클램핑)
            double deltaZ = (KpZ * dfError) + (KiZ * _zIntegral);
            _currentZPicoM = Math.Clamp(_currentZPicoM + (deltaZ * dtSeconds), -25000.0, 25000.0); // +/- 25 nm

            // 4. AGC 진폭 유지 서보 (Quality Factor Q ~ 100,000 진폭 록킹)
            double ampError = TargetAmplitudePicoM - telemetry.OscillationAmplitudePicoM;
            _driveAmplitude = Math.Clamp(_driveAmplitude + (ampError * 0.01), 0.5, 100.0);

            bool isLocked = Math.Abs(dfError) < 0.2 && Math.Abs(telemetry.PhaseErrorRad) < 0.05;
            return new QPlusAfmServoCommand(_currentZPicoM, _driveAmplitude, isLocked);
        }
    }
}