Knco= 1/4096;       %   NCO gain (разрядность NCO)
KP= 2;              % 1/cycles  phase detector gain
wn = 2*pi*400;      % rad/s   loop natural frequency  (очевидно несущая круговая частота)
fs = 25e6;         % Hz sample rate (частота выборок или семплирования)
zeta = 1;           % damping factor
Ts= 1/fs;           % s  sample time (время семплирования)

KL= 2*zeta*wn*Ts/(KP*Knco);      % loop filter proportional gain
KI= wn^2*Ts^2/(KP*Knco);         % loop filter integral gain
fprintf('Пропорц. коэфф KL %d\n', KL);
fprintf('Интегр. коэфф KI %d\n', KI);

b0= KP*KL*Knco;
b1= KP*Knco*(KI - KL);
a1= KP*KL*Knco - 2;
a2= 1 + KP*Knco*(KI - KL);
b= [b0 b1];         % numerator coeffs
a= [1 a1 a2];       % denominator coeffs

N= 100000;
n= 1:N;
t= n*Ts;
x= ones(1,N);       % step function
y= filter(b,a,x);   % step response
pe = y - 1;         % phase error response
plot(t*1e3,pe),grid
xlabel('ms'),ylabel('Phase Error = u/ref-phase -1'),figure  %plot phase error

%% Модель цифровой ФАПЧ

% количество выборок на модель
N= 300000;
% Гц частота сигнала fref
fref = 8e6;
% Гц частота выборок
fs= 96e6;
% начальная ошибка NCO Гц fnco = fref+ferr
ferr = -800;
% натуральная частота Гц
fn = 5000;
% период выборки сек
Ts = 1/fs;
% индексы времени модели
n= 0:N-1;
% матрица времени модели мсек
t= n*Ts*1000;
% циклы начальной фазы опорного сигнала
init_phase = 0.7;
% циклы фазы опорного сигнала
ref_phase = fref*n*Ts + init_phase;
% циклы фазы опорн. сигн. по mod 1
ref_phase = mod(ref_phase,1);
% коэффициент усиления NCO (по факту минимальный шаг соотв. разр. сетке)
Knco= 1/4096;
% коэфф. усил. фазового детектора 1/cycles
KP = 2;
% демпинг фактор
zeta = 1.0;
% круговая натуральная частота rad/s fn = ?n/(2?)
wn = 2*pi*fn;
% пропорциональный коэффициент петлевого фильтра
KL= 2*zeta*wn*Ts/(KP*Knco);
% интегральный коэффициент петлевого фильтра
KI= wn^2*Ts^2/(KP*Knco);
% распечатка
fprintf('Пропорц. коэфф KL %d\n', KL);
fprintf('Интегр. коэфф KI %d\n', KI);

% Расчет коэффициентов передаточной функции с замкнутого контура u / ref_phase
% CL(z) = (b0 + b1z^-1)/(a2Z^-2 + a1z^-1 + 1)
b0= KP*KL*Knco;
b1= KP*Knco*(KI - KL);
a1= KP*KL*Knco - 2;
a2= 1 + KP*Knco*(KI - KL);
% коэффициенты петлевого фильтра (numerator denominator)
b= [b0 b1];
a= [1 a1 a2];

% начальная частота NCO Гц
fnco = fref+ferr;
% распечатка
fprintf('начальная частота NCO Гц %d\n', fnco);

u(1) = 0;
ur(1) = 0;
int(1)= 0;
% начальная фазовая ошибка
phase_error(1) = -init_phase;
% начальная значение на входе NCO
vtune(1) = -init_phase*KL;
% решение разностных уравнений
for n= 2:N;
% NCO
x = fnco*Ts + u(n-1) + vtune(n-1)*Knco; % циклы NCO фазы
u(n) = mod(x,1); % циклы NCO фазы по mod 1
s = sin(2*pi*u(n-1)); % NCO sin выход
y(n)= round(2^15*s)/2^15; % квантованный выход синуса
% фазовый детектор
pe= ref_phase(n-1) - u(n-1); % ошибка фазы
pe= 2*(mod(pe+1/2,1) - 1/2); % обертка, если пересечение фаз +/- 1/2 цикла
phase_error(n) = pe;
% Петлевой фильтр
int(n) = KI*pe + int(n-1); % интегратор
vtune(n) = int(n) + KL*pe; % выход петлевого фильтра

xr = fref*Ts + ur(n-1); % циклы fref
ur(n) = mod(xr,1); % циклы fref фазы по mod 1
sr = sin(2*pi*ur(n-1)); % sin выход fref
yr(n)= round(2^15*sr)/2^15; % квантованный выход fref

end

%% Графика

% фазовая ошибка с фазового детектора
figure
plot(t,phase_error),grid
axis([0 1 -1 1])
xlabel('t (ms)'),ylabel('phase_error'),title('фазовая ошибка с фаз. детектора')

% выход VCO
figure
plot(t,vtune),grid
axis([0 1 -3.5 3])
xlabel('t (ms)'),ylabel('vtune'),title('выходной сигнал петлевого фильтра')

figure
psd(y(11000:end),2^14,fs/1e6)
axis([7.95 8.05 -80 40]),xlabel('MHz'),title('Спектр выхода NCO')

% % Частотная характеристика замкнутого контура
% figure
% u = 0:.1:.9;
% f= 10* 10 .^u;
% f = [f 10*f 100*f 1000*f];
% z = exp(j*2*pi*f/fs);
% CL= (b0 + b1*z.^-1)./(1 + a1*z.^-1 + a2*z.^-2);
% CL_dB= 20*log10(abs(CL));
% semilogx(f,CL_dB),grid
% xlabel('Hz'),ylabel('CL(z) dB'),title('Частотная характеристика замкнутого контура')

% sin NCO и fref
figure;
plot(t, y, 'b-', 'LineWidth', 2);
hold on;
grid on;
plot(t, yr, 'r-', 'LineWidth', 2);
hold on
hold off;