%-------------------------------------------------------------------------- 
% The goal of this simulation is to simulate carrier tracking loop with the 
% way of costas loop.  
% Author:   Meng, xianwei, copyright of sun-create 
% Data:     2006.9.12 
%-------------------------------------------------------------------------- 
clear all; 
clc; 
%------------simulate costas loop------------- 
 
% disp(' '); 
 
fdel = 30;          %input('Enter the size of frequency step in Hertz>');% 
 
fn = 50.7143;       %input('Enter  loop filter natural frequency in Hertz>');%10 
lambda = 0.01;      %input('Enter lambda, the relative pole offset>');%0.1 
zeta = 1/sqrt(2); 
 
%------采樣頻率------------------ 
fs = 4000*1.023; 
tstop = 18;          %----結束時間---- 
 
npts = fs * tstop + 1; 
t = (0:(npts-1))/fs; 
 
nsettle = fix(npts/10); 
tsettle = nsettle/fs; 
 
freq = fs; 
fin  = [zeros(1, nsettle), fdel * ones(1,npts-nsettle)];    %--輸入的頻率 
 
phin = [zeros(1, nsettle), 2*pi*fdel*t(1:npts - nsettle)];  %--輸入的相位 
 
len = length(phin); 
Acostx = zeros(1,len); 
carrier_freq = 2000; 
% nsettle = 0; 
nsettle = 64; 
randn('state',0); 
for k=1:len 
    if ( k<=nsettle ) 
        Acostx(k) = cos(2*pi*carrier_freq*t(k) + 0); 
    else 
%         if(k / 100 =0) 
             
            Acostx(k) = cos(2*pi*carrier_freq*t(k) + 0.008*randn(1)*fdel);%*(-1); 
%         end; 
    end 
end 
 
% Generate original signal xref and a noisy version x adding  
% a standard Gaussian white noise.  
% sqrt_snr = 8; init = 2055615866; 
%-----信號加入噪聲--------------------------------------------- 
 
Acost = Acostx + 0.2*randn(size(t));;% 
dumptime =90;          %積分清零時間頻率計數(shù)門限 
Carrier_Tracking_loop(Acost,carrier_freq,fs,t,dumptime); 
 
% % Acost(11001:19000) = Acost(11001:19000) .* ones(1,8000) * (-1); 
% % Acost(71001:79000) = Acost(71001:79000) .* ones(1,8000) * (-1); 
% % Acost(91001:99000+10000) = Acost(91001:99000+10000) .* ones(1,8000+10000) * (-1); 
% %Acost((91001+10000):99000+10000) = Acost((91001+10000):99000+10000) .* ones(1,8000+10000) * (-1); 
% %---------initialize--------- 
% w2b = 0; w2c = 0; s5 =0; phivco =0 ; 
% twopi = 2 * pi; 
% twofs = 2 * fs; 
%  
% %--define 2*fs; 
%  
% %---set loop gain-----------zeta = 1/sqrt(2); 
% G = pi*fn*(zeta + sqrt(zeta*zeta - lambda)); 
%  
% %---set filter parameter----Wn = 2*pi*fn 
% a = 2*pi*fn/(zeta + sqrt(zeta*zeta - lambda)); 
%  
% %---define constant--- 
% a1 = a*(1-lambda); a2 = a*lambda; 
%  
% %---初始化向量---- 
% % phierror = zeros(1,npts); 
% % fvco = zeros(1, npts); 
% % vco_sin = zeros(1, npts); 
% % vco_cos = zeros(1, npts); 
%  
% phivco = 0;lpw2c = 0; Qpw2c=0; 
%  
% dumptime = 90; 
% dumptime_counter = 0; 
% vertor_len = 0; 
%  
% %---------costas loop simulation--------- 
% for i =1:npts 
%     dumptime_counter = dumptime_counter + 1; 
%     if (dumptime_counter>dumptime) 
%         dumptime_counter = dumptime_counter - dumptime; 
%     end; 
%     vco_sin(i) = sin(2*pi*carrier_freq.*t(i) + phivco); 
%     vco_cos(i) = cos(2*pi*carrier_freq.*t(i) + phivco); 
%      
%     I_cos = vco_cos(i) * Acost(i); 
%     Q_sin = vco_sin(i) * Acost(i); 
%     s11 = Q_sin; 
%     Qs11= I_cos; 
%     %mul_tmp_Q(i) = Q_sin; 
%  
%     %------低通濾波器-------Integrate and dump------- 
%     Qps6  = Qs11;             %--V輸入 
%     Qpw1c = Qps6 + Qpw2c;   %-積分步驟1 
%     Qpw2c = Qps6 + Qpw1c;   %--積分步驟2 
%     Qs11 = Qpw1c;%/twofs;      %--V輸出 
%  
%     %------低通濾波器------------------------------- 
%     lps6  = s11;             %--V輸入 
%     lpw1c = lps6 + lpw2c;   %-積分步驟1 
%     lpw2c = lps6 + lpw1c;   %--積分步驟2 
%     s11 = lpw1c;%/twofs;      %--V輸出 
%      
%     %------積分清零，信號抽取---------------------- 
%     if (dumptime_counter == dumptime) 
%         vertor_len = vertor_len + 1;  
%         s11 = s11/dumptime; 
%         Qs11= Qs11/dumptime; 
%         Qft_tmp(vertor_len) = Qs11; 
%         ft_tmp(vertor_len) = s11;     
%         %------------------------------------------ 
%    %---------------鑒相器--------------------------------- 
%         s2 = ft_tmp(vertor_len) * Qft_tmp(vertor_len); 
%         s3 = G*s2;              %--濾波器增益乘信號 
%         s4 = a1*s3; 
%      
%         s4a = s4 - a2*s5;       %--環(huán)路濾波器積分輸出 
%         w1b = s4a + w2b;        %--濾波器積分步驟1 
%         w2b = s4a + w1b;        %--濾波器積分步驟2 
%         s5  = w1b/twofs;        %--產生濾波器輸出 
%    %----------VCO----------------------------------------  
%         s6  = s3 + s5;          %--VCO積分輸入 
%         w1c = s6 + w2c;         %--VCO積分步驟1 
%         w2c = s6 + w1c;         %--VCO積分步驟2 
%         phivco = w1c/twofs;     %--VCO積分輸出 
%      
%         phierror(i) = s11;       %--相位差矩陣 
%         fvco(vertor_len) = s6/twofs;     %--VCO輸入矩陣 
%          
%         %-------積分清零----------------------- 
%         Qs11 = 0; 
%         s11  = 0; 
%         Qpw2c =0; 
%         lpw2c = 0; 
%     end; 
%   
%      
% end; 
%  
% % freqerror = fin - fvco;     %--頻差矩陣/fs 
%  
% plot([1:vertor_len]*dumptime./fs, Qft_tmp); 
% hold on; 
% plot([1:vertor_len]*dumptime./fs, ft_tmp,'r') 
% grid on; 
% xlabel('時間（t）'); 
% ylabel('I和Q路幅值'); 
% title('載波跟蹤環(huán)（costas loop）模擬'); 
% set(gcf, 'Color', [1 1 1]) 
 
 
 
 
% %-----------------------Global variable defination------------------------- 
%  
%        PRN_Fs = 1023000; 
%        NavData_Fs = 50; 
%        sampleTimes = 18; 
%        Carrier_Frequency = 3000000; 
%        Carrier_doppleFre_Shift = 0;             %4000; 
%        Phase_Diff = pi/9; 
%        Pha_pos = 1023;                          % C/A Code Phase 
%        len = 120;                               %Signal plot length 
% %--------------------------- Create signal.-------------------------------- 
%  
%        PRN_num = 1023; 
%         
%        PRN_temp = double(0: (1/(sampleTimes*PRN_Fs)) :PRN_num*(1/(PRN_Fs))); 
%        PRN_t = PRN_temp(1:(length(PRN_temp)-1)); 
%        %--------------------------GPS PRN signal--------------------------- 
%   
%         
%        %------------------------carrier signal----------------------------- 
%        %Fs = 4000;   t = 0:1/Fs:1; 
%        Carrier_sample_time = PRN_t; 
%        Carrier = cos(2 * pi * (Carrier_Frequency + Carrier_doppleFre_Shift)... 
%                         * Carrier_sample_time + Phase_Diff);  
%        %-----plot-- 
%        figure; 
%        an=[1:21]; 
%        stem(an.*(1/(sampleTimes*PRN_Fs)),Carrier(1:21)); 
%        hold on;  
%        plot(an.*(1/(sampleTimes*PRN_Fs)),Carrier(1:21),'r'); 
%        grid on; 
%        if (sampleTimes*1000000/Carrier_Frequency>2) 
%             title('Carrier Signal Over Sampling Rate'); 
%        else 
%            title('Carrier Signal UnderSampling Rate'); 
%        end;