lock.cpp

锁相环程序

// lock.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <stdio.h>
#include <math.h>

#define N 102400
#define M 128
#define LATCH 0.01
#define pi 3.1416
#define FS 36000		//sample fre
#define FC 9000			//mid fre
#define FO 3000			//F omig
#define FA 1000			//tiao fu

#define K0	(pow(2,-8))	//LF parameters
#define K1	(pow(2,-6))

#define SIN_LENGTH 4096
#define PHASE_DIFF 1024

#define GATE_NCO (1/M)
#define JUMP 280
#define RATE_RAW (SIN_LENGTH*FC/FS)

#define DELTA_FC (50)

static int Rate=RATE_RAW;//每隔几个点采正弦表
static float LF_out[2]={0,0};
static float Carrier_storage[SIN_LENGTH];
static int first_phase_Q=0;
static int last_phase_Q = 0;
static float I[2]={0,0};
static float Q[2]={0,0};
static float NCO_I[M];
static float NCO_Q[M];
static float delta_phase[2]={0,0};
float s_raw[N];
static long n;//原始数据缓冲区指针
float *s;

//do_LF
//static float LF_out[2];

void do_LF(float delta_phase[],float LF_out[])
{
	float temp;
	temp = (float)( (K0+K1)*delta_phase[1] - K0*delta_phase[0] + LF_out[0] );
	LF_out[0] = LF_out[1];
	LF_out[1] = 2*temp;
	
	//LF_out[1] = 16*temp/pi;

	if(LF_out[1]>5)
		LF_out[1]=5;
	if(LF_out[1]<-5)
		LF_out[1]=-5;
}
//init sin table
//static float Carrier_storage[TABLE_LENGTH];
void Carrier_create(float Carrier_storage[],int sin_length)
{
	int i;
	for(i=0;i<sin_length;i++)
	{
		Carrier_storage[i] = (float)sin(2*pi*i/sin_length);
	}
}
//carrier home
//static first_phase_I;
//static first_phase_Q;
void Carrier_home(float Carrier_storage[],float NCO_I[],float NCO_Q[],int m,int rate)
{
	//从保存的向量波形中，作为本地载波。
	int P_sample_Q;
	int P_sample_I;
	int i;
	for(i=0;i<m;i++)
	{	
		P_sample_Q = first_phase_Q + i*rate;
		P_sample_I = first_phase_Q + PHASE_DIFF + i*rate;
		while(P_sample_I >= SIN_LENGTH || P_sample_Q >= SIN_LENGTH)
		{
			if(P_sample_I >= SIN_LENGTH)
				P_sample_I-=SIN_LENGTH;					//回退
			if(P_sample_Q >= SIN_LENGTH)
				P_sample_Q-=SIN_LENGTH;					//回退
		}
		NCO_I[i] = Carrier_storage[P_sample_I];
		NCO_Q[i] = Carrier_storage[P_sample_Q];
	}
}
//update_NCO, unknow
//static int last_phase_Q = 0;
int update_NCO(float Carrier_storage[],float NCO_I[],float NCO_Q[],int m,int rate,float LF_out,float gate_NCO)
{
	//从保存的向量波形中，按照一定的规则提取，作为本地载波。
	int Count_Ad=0;
	int P_sample_Q=last_phase_Q;
	int P_sample_I=last_phase_Q+PHASE_DIFF;
	int i=0;
	float temp;
	if(LF_out<0)
		temp=-LF_out;

	if ( temp < gate_NCO )                //太小，不能调整？？？？？？？？？？？？？？
	{
		printf("too small!\n");
		for (i=0;i<m;i++)
        {
			P_sample_I=P_sample_I+rate;
			P_sample_Q=P_sample_Q+rate;
			while(P_sample_I >= SIN_LENGTH || P_sample_Q >= SIN_LENGTH)
			{
				if(P_sample_I >= SIN_LENGTH)
					P_sample_I-=SIN_LENGTH;					//回退
				if(P_sample_Q >= SIN_LENGTH)
					P_sample_Q-=SIN_LENGTH;					//回退
			}
			NCO_I[i]=Carrier_storage[P_sample_I];  //得到本地载波
			NCO_Q[i]=Carrier_storage[P_sample_Q];  //得到本地载波
		}
		//保存最后I路的相位，使相位连续
		last_phase_Q = P_sample_Q;
		return 0;
	}
	else if (LF_out>0)
	{
		Count_Ad = (int)(1/LF_out);//下取整
		printf("abs(1/LF_out)=%d\n",Count_Ad);
		if(Count_Ad==0)
			return 0;
		for (i=0;i<m;i++)
		{           
            P_sample_I=P_sample_I+rate;
			P_sample_Q=P_sample_Q+rate;
 
			if ( /*(i>=Count_Ad) && */((i%Count_Ad)==1) )                    //根据环路滤波器的输出，进行调整, 每隔Count_Ad
			{
				P_sample_I=P_sample_I-1;    //采前一点
				P_sample_Q=P_sample_Q-1;
			}
			while(P_sample_I >= SIN_LENGTH || P_sample_Q >= SIN_LENGTH)
			{
				if(P_sample_I >= SIN_LENGTH)
					P_sample_I-=SIN_LENGTH;					//回退
				if(P_sample_Q >= SIN_LENGTH)
					P_sample_Q-=SIN_LENGTH;					//回退
			}
			NCO_I[i]=Carrier_storage[P_sample_I];  //得到本地载波
			NCO_Q[i]=Carrier_storage[P_sample_Q];  //得到本地载波
		}
	}
	else if (LF_out<0)
	{
		Count_Ad = (int)(-(1/LF_out));			//负数取整，向0靠拢 
		printf("abs(1/LF_out)=%d\n",Count_Ad);
		if(Count_Ad==0)
			return 0;
		for (i=0;i<m;i++)
        {
			P_sample_I=P_sample_I+rate;
			P_sample_Q=P_sample_Q+rate;
 
			if ( /*(i>=Count_Ad) && */((i%Count_Ad)==1) )                    //根据环路滤波器的输出，进行调整, 每隔Count_Ad
			{
				P_sample_I=P_sample_I+1;    //采后一点
				P_sample_Q=P_sample_Q+1;
			}
			while(P_sample_I >= SIN_LENGTH || P_sample_Q >= SIN_LENGTH)
			{
				if(P_sample_I >= SIN_LENGTH)
					P_sample_I-=SIN_LENGTH;					//回退
				if(P_sample_Q >= SIN_LENGTH)
					P_sample_Q-=SIN_LENGTH;					//回退
			}
			NCO_I[i]=Carrier_storage[P_sample_I];  //得到本地载波
			NCO_Q[i]=Carrier_storage[P_sample_Q];  //得到本地载波
		}
	}
	//保存最后I路的相位，使相位连续
	last_phase_Q = P_sample_Q;
	return 1;
}
//adjust NCO fre
static int ad_table[4]={0,0,0,0};
static int p_table=0;
int NCO_fre_ad(float Carrier_storage[],float NCO_I[],float NCO_Q[],int m,float delta_fre,float gate_NCO)
{
	//从保存的向量波形中，按照一定的规则提取，作为本地载波。
	
	int P_sample_Q=last_phase_Q;
	int P_sample_I=last_phase_Q+PHASE_DIFF;
	int i=0;
	float temp;
	int inter;
	float floater;
	if(delta_fre>0)
	{
		temp=delta_fre;
		inter=(int)temp;
		floater=temp-inter;
		Rate+=inter;
		if(Rate>2024)
			Rate=2024;
		if(0<floater && floater<=0.25)
		{
			ad_table[0]=1;
			ad_table[1]=0;
			ad_table[2]=0;
			ad_table[3]=0;
		}
		if(0.25<floater && floater<=0.5)
		{
			ad_table[0]=1;
			ad_table[1]=0;
			ad_table[2]=1;
			ad_table[3]=0;
		}
		if(0.5<floater && floater<=0.75)
		{
			ad_table[0]=1;
			ad_table[1]=1;
			ad_table[2]=1;
			ad_table[3]=0;
		}
		if(0.75<floater)
		{
			ad_table[0]=1;
			ad_table[1]=1;
			ad_table[2]=1;
			ad_table[3]=1;
		}
	}
	if(delta_fre<0)
	{
		temp=-delta_fre;
		inter=(int)temp;
		floater=temp-inter;
		Rate-=inter;
		if(Rate<24)
			Rate=24;
		if(0<floater && floater<=0.25)
		{
			ad_table[0]=-1;
			ad_table[1]=0;
			ad_table[2]=0;
			ad_table[3]=0;
		}
		if(0.25<floater && floater<=0.5)
		{
			ad_table[0]=-1;
			ad_table[1]=0;
			ad_table[2]=-1;
			ad_table[3]=0;
		}
		if(0.5<floater && floater<=0.75)
		{
			ad_table[0]=-1;
			ad_table[1]=-1;
			ad_table[2]=-1;
			ad_table[3]=0;
		}
		if(0.75<floater)
		{
			ad_table[0]=-1;
			ad_table[1]=-1;
			ad_table[2]=-1;
			ad_table[3]=-1;
		}
	}
	if(delta_fre==0)
	{
		ad_table[0]=0;
		ad_table[1]=0;
		ad_table[2]=0;
		ad_table[3]=0;
	}
	for (i=0;i<m;i++)
    {
		P_sample_I=P_sample_I+Rate+ad_table[p_table];
		P_sample_Q=P_sample_Q+Rate+ad_table[p_table];
		p_table=(p_table+1)%3;
		while(P_sample_I >= SIN_LENGTH || P_sample_Q >= SIN_LENGTH)
		{
			if(P_sample_I >= SIN_LENGTH)
				P_sample_I-=SIN_LENGTH;					//回退
			if(P_sample_Q >= SIN_LENGTH)
				P_sample_Q-=SIN_LENGTH;					//回退
		}
		NCO_I[i]=Carrier_storage[P_sample_I];  //得到本地载波
		NCO_Q[i]=Carrier_storage[P_sample_Q];  //得到本地载波
	}
	//保存最后I路的相位，使相位连续
	last_phase_Q = P_sample_Q;
	
	return 1;
}
/*static float last_phase;
void update_NCO(float jump,float NCO_I[M],float NCO_Q[M]);
{
	float temp_phase;
	for(i=0;i<M;i++)
	{
		temp_phase = 2*pi*i*(FC+jump)/FS + last_phase;
		NCO_I[i]=cos(temp_ph);
		NCO_Q[i]=sin(temp_ph);
	}
	//updata last_phase
	int temp = temp_phase/(2*pi);//get the integer
	last_phase = temp_phase - temp*2*pi;
}*/
//grab
/*static float I[2];
static float Q[2];
static float NCO_I[M];
static float NCO_Q[M];
static float delta_phase[2];
*/
int grab(float s[])
{
	float jump;
	int i=0;
	float temp;
	I[0]=I[1];
	Q[0]=Q[1];
	delta_phase[0]=delta_phase[1];
	
	for(i=0;i<M;i++)
	{
		I[1]+=s[i]*NCO_I[i];
		Q[1]+=s[i]*NCO_Q[i];
	}

	temp = I[0]*I[1]+Q[0]*Q[1];
	if(temp==0)
		return 0;
	delta_phase[1] = (I[0]*Q[1]-I[1]*Q[0])/temp;
	//捕获
	temp=I[1]*I[1]+Q[1]*Q[1];
	if(temp > LATCH)
		return 1;
	else if (temp < 0)/////////////////////////??????
		jump=JUMP;
	else
		jump=-JUMP;

	//scan_NCO(jump,NCO_I[M],NCO_Q[M]);////////////////?????????????

	return 0;
}
//calculate arctan
static float arctan(float x)
{
	float ret;
	ret = (float)(x - pow(x,3)/3 + pow(x,5)/5);
	return ret;
}
//trace
int trace(float s[])
{
	int i=0;
	float temp_phase[2];
	float temp;
	I[0]=I[1];
	Q[0]=Q[1];
	delta_phase[0]=delta_phase[1];
	
	for(i=0;i<M;i++)
	{					
		I[1]+=s[i]*NCO_I[i];
		Q[1]+=s[i]*NCO_Q[i];
	}
	temp = (I[0]*I[1]+Q[0]*Q[1]);
	if(temp==0)
	{
		NCO_fre_ad(Carrier_storage,NCO_I,NCO_Q,M,0,GATE_NCO);
		printf("return\n");
		return 0;
	}
	delta_phase[1] = atan( (I[0]*Q[1]-I[1]*Q[0]) / temp );
	if(delta_phase[1]>=0)
	{
		if(delta_phase[1]>pi/2)
			delta_phase[1]=pi/2;
		if(temp<0)
			delta_phase[1]-=pi;
	}
	else//delta_phase[1]<0
	{
		if(delta_phase[1]<-pi/2)
			delta_phase[1]=-pi/2;
		if(temp<0)
			delta_phase[1]+=pi;
	}


//for test below
	//delta_phase[1] = (float)( Q[1]/pow(I[1]*I[1]+Q[1]*Q[1],0.5) );
	//for(i=0;i<2;i++)
	{
		temp_phase[1] = atan(Q[1]/I[1]);
		
		if(temp_phase[1]>=0)
		{
			if(temp_phase[1]>pi/2)
				temp_phase[1]=pi/2;
			if(I[1]<0)
				temp_phase[1]-=pi;
		}
		else//temp_phase[i]<0
		{
		/*	if(temp_phase[1]<-pi/2)
				temp_phase[1]=-pi/2;*/
			if(Q[1]>0)
				temp_phase[1]+=pi;
		}
		temp = arctan(Q[1]/I[1]);
			if(temp>=0)
		{
			if(temp>pi/2)
				temp=pi/2;
			if(I[1]<0)
				temp-=pi;
		}
		else//temp<0
		{
			if(temp<-pi/2)
				temp=-pi/2;
			if(Q[1]>0)
				temp+=pi;
		}
	}//  -pi/2<temp_phase<pi/2
	//delta_phase[1] = temp_phase[1] - temp_phase[0];
//end add


	if(delta_phase[1]>=pi)
		delta_phase[1]-=pi;
	if(delta_phase[1]<-pi)
		delta_phase[1]+=pi;
	printf("I[1]=%f,Q[1]=%f,real_phase=%f,emu_phase=%f\n",I[1],Q[1],temp_phase[1]/pi,temp/pi );
	printf("delta_phase[1]=%f\n,",delta_phase[1]/pi);
	do_LF(delta_phase,LF_out);
	printf("LF_out[1]=%f\n",LF_out[1]);
	//更新NCO
	//update_NCO(Carrier_storage,NCO_I,NCO_Q,M,RATE,LF_out[1],GATE_NCO);
	//NCO_fre_ad(Carrier_storage,NCO_I,NCO_Q,M,-delta_phase[1],GATE_NCO);
	NCO_fre_ad(Carrier_storage,NCO_I,NCO_Q,M,LF_out[1],GATE_NCO);
	return 1;
}
void init_global()
{
	n=0;
	s=&s_raw[n];
}
//generate data for test

//float s_raw[N];

void generate_data()
{
	int i;
	for (i=0;i<N;i++)
	{
		s_raw[i]=(float)( cos(2*pi*i*(FC+DELTA_FC)/FS) );
		//s_raw[i]=(float)( (1+0.5*cos(2*pi*i*FA/FS)) * (1+0.5) * cos(2*pi*i*(FC+20)/FS+pi/4+0.61*cos(2*pi*i*FO/FS+pi/4)) );
		//s2[i]=(1+0.5) * cos(2*pi*i*FC/FS+pi/4/*+PHK*/+1.25*cos(2*pi*FO/FS+pi/4));
	}
}
//main
//static long n;//原始数据缓冲区指针
//float *s;
void dummy()
{
	return;
}
void main()
{
	float fre;
	int i;
	float sum;
	printf("Hello World!\n");
	init_global();
	generate_data();
	Carrier_create(Carrier_storage,SIN_LENGTH);
	Carrier_home(Carrier_storage,NCO_I,NCO_Q,M,RATE_RAW);
	
	while(n<N)
	{

		/*while(1)
		{
			if( grab(s) == 1 )
				break;
			else
			{
				n+=M;
				s=&s_raw[n];
			}
		}*///产生NCO
		//while(1)
		{
			n+=M;
			s=&s_raw[n];
			trace(s);//更新NCO
			dummy();
			sum=0;
			for(i=0;i<M;i++)
			{
				sum+=NCO_I[i]*NCO_Q[i];
			}
			fre=FS*Rate/4096;
			printf("Rate=%d,fre_in=%d,fre_local=%f\n",Rate,FC+DELTA_FC,fre);
		}	
	}
}