%Define Loop Filter parameters(Sets damping)
kp=0.15; %Proportional constant
ki=0.1; %Integrator constant
%PLL implementation
for n=2:length(Signal)
vco(n)=conj(exp(j*(2*pi*n*f/fs+phi_hat(n-1))));%Compute VCO
phd_output(n)=imag(Signal(n)*vco(n));%Complex multiply VCO x Signal input
e(n)=e(n-1)+(kp+ki)*phd_output(n)-ki*phd_output(n-1);%Filter integrator
phi_hat(n)=phi_hat(n-1)+e(n);%Update VCO
end;

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

% количество выборок на модель
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, 'b-', 'LineWidth', 2),grid
axis([0 3 -1 1])
xlabel('t (ms)'),ylabel('phase_error'),title('фазовая ошибка с фаз. детектора')

% выход VCO
figure
plot(t,vtune, 'r-', 'LineWidth', 2),grid
axis([0 3 -1 1])
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;

% digital PLL
% Copyright © 2015 Bernd Porr <mail@berndporr.me.uk>

% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.

% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.

% You should have received a copy of the GNU Lesser General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.


% PLL which locks on the carrier
% The osciallator is a two pole IIR standard oscillator
% used in comms
%
% phase comparison is by simple multiplication
%
% note that we need to run the system in proper causal
% C style because the oscillator, PLL and filter needs
% to react to the input signal

% variables for the osciallator
% cosine output
c = 1;
% delayed cosine by one timestep
c_delay = 0;
% sine output
s = 0;
% delayed sine by one timestep
s_delay = 0;

% vectors storing the results by adding the samples one by one
sine = [];
cosine = [];
vco = [];
input_signal = [];

% PLL loop filter
b = fir1(100, 0.1);
% filter state because we just feed one value at a time in it and
% then come back to it every timestep so we need to save the state
zf = zeros(numel(-1,1);

v = 0;

% main loop: reading data one by one
for i=1:1000

% test signal, will be replaced by the input signal
% the PLL needs to lock on it, let's assume our
% frequency is 1% off. Should be f=0.1 but is f=0.099
% and we have some random phase angle.
y = cos(2*pi*0.1*i+pi/2.345543);

%"voltage" controlled oscillator with f=0.1 +/- v input
w0 = 2*pi*(0.1+v*0.002);
c_delay = c;
s_delay = s;
c = c_delay * cos(w0) - s_delay * sin(w0);
s = s_delay * cos(w0) + c_delay * sin(w0);

% phase detector
p = y*(-s);

% filtering out the 2f term
[v,zf] = filter(b,1,p,zf);

% let's save everything in handy vectors for plotting
input_signal = [input_signal y];
sine = [sine s];
cosine = [cosine c];
vco = [vco v];
end

%plot(input_signal);
plot(vco);