afc_detect.m

这是AFC(自适应频率控制)程序

% Project Name:	AFC
% FILE NAME :	AFC_detect.m
% ABSTRUCT:	This file is for AFC dection.
	
AFC_setup;
AFC_init;
AFC_gain_table_init;

for n=1:2:length(x)-1
    %clipping the input
    if (x(n) > 1) x(n)=1; end
    if (x(n) < -1) x(n)=-1; end

    %DC filtering    
    afc_dc_n=AFC_DC_ALPHA*x(n) + ONE_MINUS_AFC_DC_ALPHA*afc_dc_n_1;
    if (afc_dc_n > 1) afc_dc_n=1; end
    if (afc_dc_n < -1) afc_dc_n=-1; end
    afc_dc_n_1=afc_dc_n;
    
    %subtract from DC
    afc_ac_xn=x(n)-afc_dc_n;
    if (afc_ac_xn > 1) afc_ac_xn=1; end
    if (afc_ac_xn < -1) afc_ac_xn=-1; end
        
    %Ex estimate
    afc_Ex_n=AFC_EX_ALPHA*((afc_ac_xn./2).^2) + ONE_MINUS_AFC_EX_ALPHA*afc_Ex_n_1;
    if (afc_Ex_n > 1) afc_Ex_n=1; end
    if (afc_Ex_n < -1) afc_Ex_n=-1; end
    afc_Ex_n_1=afc_Ex_n;
    
    %IIR
    afc_iir_yn=afc_gain*afc_b0*afc_ac_xn + afc_a1*afc_iir_yn_1 + AFC_A2*afc_iir_yn_2;
%    if (afc_iir_yn > 1) afc_iir_yn=1; end
%    if (afc_iir_yn < -1) afc_iir_yn=-1; end
    
    %Pole adaptation
    pole_gain=(afc_iir_yn_2+afc_iir_yn)*AFC_R - afc_iir_yn_1*afc_a1;   
    step_gain=AFC_MSCALE*afc_iir_yn_1*afc_delta;
    
    if (afc_iir_yn_1*pole_gain > 0)
        if (afc_iir_yn_1*(step_gain-pole_gain) < 0)
            afc_delta=afc_delta*2;
        else
            afc_delta=afc_delta/2;
        end
        if (afc_delta > 1) afc_delta=1; end
        afc_a1=afc_a1+AFC_P_ALPHA*afc_delta;
    else
        if (afc_iir_yn_1*(step_gain+pole_gain) < 0)
            afc_delta=afc_delta*2;
        else
            afc_delta=afc_delta/2;
        end
        if (afc_delta > 1) afc_delta=1; end
        afc_a1=afc_a1-AFC_P_ALPHA*afc_delta;    
    end
    
    if (afc_a1 > 1) afc_a1=1; end
    if (afc_a1 < -1) afc_a1=-1; end

    afc_iir_yn_2=afc_iir_yn_1;
    afc_iir_yn_1=afc_iir_yn;

    %Ey estimate
    afc_Ey_n=AFC_EY_ALPHA*(afc_iir_yn.^2) + ONE_MINUS_AFC_EY_ALPHA*afc_Ey_n_1;
    if (afc_Ey_n > 1) afc_Ey_n=1; end
    if (afc_Ey_n < -1) afc_Ey_n=-1; end
    afc_Ey_n_1=afc_Ey_n;
    
    %Gain adaptation
    if (afc_Ex_n > afc_Ey_n*(afc_gain_table(gain_index).^2))
        gain_index=gain_index+3;
    else
        gain_index=gain_index-1;
    end
    if (gain_index > length(afc_gain_table))
        gain_index=length(afc_gain_table);
    else if (gain_index < 2)
        gain_index=2;
        end
    end
    afc_b0=afc_b0 + afc_gain_table(gain_index)*afc_b0;
    if (afc_b0 > 1) afc_b0=1; end
    if (afc_b0 < -1) afc_b0=-1; end
    afc_gain=afc_gain_table(gain_index-1);
    gain_index=gain_index-1;
    
    %Timer update
    if ((afc_gain_table(gain_index) < AFC_THG) & (afc_b0 < AFC_THB_ALPHA-(afc_a1.^2*AFC_THB_BETA)))
        if (afc_timer < 0)
            afc_timer=afc_timer-afc_k_2; 
        else
            afc_timer=afc_timer+afc_k_2;
        end
        if (afc_timer<-1 | afc_timer>1)
            afc_timer=-1;
            afc_k_1=11/50;
            afc_k_2=11/50;
        end
    else
        if (afc_timer < 0)
            afc_timer=afc_timer+afc_k_1;
            if (afc_timer >= 0)
                afc_timer=0;
                AFC_DETECT_BIT=1;
                fprintf(1,'Tone found!\n');
                detected_sample=n;
                break;
            end
        else
            afc_timer=afc_timer-afc_k_1;
            if (afc_timer < 0)
                afc_timer=0;
            end
        end 
    end
    gain(n)=afc_gain_table(gain_index);
    b0(n)=afc_b0; 
    timer(n)=afc_timer;
    thetan(n)=acos(afc_a1/2/AFC_R)*270833/(2*pi);
    deltan(n)=afc_delta;
end
n5=1; % ie pole adaption
if (AFC_DETECT_BIT~=1)
    fprintf(1,'Tone not present!\n');
end

m=1:1:n/2;
figure(1); plot(gain(2*m-1)); title('afc gain'); xlabel('sample'); ylabel('gain');
figure(2); plot(b0(2*m-1)); title('afc\_b0n'); xlabel('sample'); ylabel('afc\_b0n');
figure(3); plot(timer(2*m-1)); title('afc timer'); xlabel('sample'); ylabel('timer');
figure(4); plot(thetan(2*m-1)); title('estimated frequency'); xlabel('sample'); ylabel('frequency');
figure(5); plot(deltan(2*m-1)); title('delta'); xlabel('sample'); ylabel('delta');