gensamples.c

GPS卫星导航接收机的仿真程序。用C语言实现

#include	<stdio.h>
#include	<math.h>
#include	<stdlib.h>
#include	<string.h>

#include	"simulator.h"
#include	"Matrix_Tools.h"
#include	"GenNavDataBit.h"
#include	"nrutil.h"
#include	"GenSamples.h"
#include	"Sim_Util.h"

//*********************************************
//		Some external global variables
//*********************************************
extern	double		StartTime;
extern	char		CA_Table[SAT_NUM][1025];	
extern	double		DataStartTime, DataEndTime;
extern	double		fs;
extern	double		f_IF;
extern	double		f_BF;
extern	double		Waveform[WAVEFORM_LENGTH];
extern  double      cosTable[TableLength];
extern  double		sinTable[TableLength];

RAW_EPH  eph;
RAW_EPH  ephsv[SAT_NUM+1];

double *Tp  =	NULL;
double *Tc	=	NULL;
double *Carrier_I	=	NULL;
double *Carrier_Q =  NULL;
double *Nav_sample	=	NULL;
char   *Code_sample	=	NULL;
double *sv_signal_I = NULL;
double *sv_signal_Q = NULL;

double *wave_sample	= NULL;


double *time_Rx	=	NULL;
double *Tp_tmp  =	NULL;
double *Tc_tmp =	NULL;
double *y2_Tp	=	NULL;
double *y2_Tc	=	NULL;


//******************************************************************************
//**	Function:	Allocate_Mem												
//**	Input:		The required memory capacity 													  
//**	Tasks:		Allocate reqired memory before simulation to accelerate 	
//**						the simulation speed												  
//******************************************************************************
void	Allocate_Mem ( int sample_num, int pos_num)
{
	Tp = (double *)malloc(sample_num*sizeof(double));   // delay on code
	Tc = (double *)malloc(sample_num*sizeof(double));	// delay on carrier
	Carrier_I = (double *)malloc(sample_num*sizeof(double));	// carrier phase wave form on one sv
	Carrier_Q = (double *)malloc(sample_num*sizeof(double));
	Nav_sample = (double *)malloc(sample_num*sizeof(double));
	Tp = (double *)malloc(sample_num*sizeof(double));	// ?
	sv_signal_I = (double *)malloc(sample_num*sizeof(double));
	sv_signal_Q = (double *)malloc(sample_num*sizeof(double));

	Code_sample = (char *)malloc(sample_num*sizeof(char));
	wave_sample = (double *)malloc(sample_num*sizeof(double));

	time_Rx = (double *)malloc(pos_num*sizeof(double));
	Tp_tmp = (double *)malloc(pos_num*sizeof(double));
	Tc_tmp = (double *)malloc(pos_num*sizeof(double));
	y2_Tp = (double *)malloc(pos_num*sizeof(double));
	y2_Tc = (double *)malloc(pos_num*sizeof(double));

}

//******************************************************************************
//**	Function:	FREE_Mem												
//**	Input:		None 													  
//**	Tasks:		Release the memory allocated after the simulation
//******************************************************************************
void	FREE_Mem( )
{
	free (Carrier_I);
	free (Carrier_Q);
	free (Nav_sample);
	free (Code_sample);
	free (wave_sample);
	free (Tp);
	free (Tc);
	free (sv_signal_I);
	free (sv_signal_Q);


	free (time_Rx);
	free (Tp_tmp);
	free (Tc_tmp);
	free (y2_Tp);
	free (y2_Tc);
}


//******************************************************************************
//**	Function:	GenComSig												
//**	Tasks:		Determine if a SV can be seen directly, if so generate the 
//**						 signal for this SV, and combine signals from all available 
//**						 SV's to create the final signal without background noise.
//******************************************************************************
void	GenComSig( double * t_vect,  int t_length, double **RxPos, int Num_RxPos,
					double **EphData, int Num_eph, double *ComSig_I, double *ComSig_Q, OPTION *pOption )
{
	int icount, jcount, prn;
	int		Obs_cap;
	double Upos[3];

	jcount	=	(int) (t_vect[0]/TrajTD);
	Upos[0]	=	RxPos[jcount][1];	// X
	Upos[1]	=	RxPos[jcount][2];	// Y
	Upos[2]	=	RxPos[jcount][3];	// Z
	// First, clear the buffer.
	for ( icount = 0; icount < t_length ; icount ++ )
	{
	    ComSig_I[icount]	=	0.0;
	    ComSig_Q[icount]	=	0.0;
	}

	for (prn = 1; prn <= SAT_NUM;  prn ++ )
	{        
		// Here the availability of the SV is checked		
		Obs_cap = SV_LOS_detect(prn, Upos, t_vect[0]+StartTime);
		
		if (Obs_cap == 1 )
		{
			printf("\r\nSV:  %d", prn);

			// The current SV can be seen at this time, simulate the data.
			Signal_prn(sv_signal_I, sv_signal_Q, prn,  t_vect, t_length, RxPos, Num_RxPos, EphData, Num_eph, pOption); 
		
			for ( icount = 0; icount < t_length ; icount ++ )
				ComSig_I[icount] +=	sv_signal_I[icount];

			if (pOption->IQ_Switch)
			{
				for ( icount = 0; icount < t_length ; icount ++ )
					ComSig_Q[icount] +=	sv_signal_Q[icount];
			}
		}
	}
}


//******************************************************************************
//**	Function:	Signal_prn												
//**	Tasks:		generate the signal for one SV
//******************************************************************************
void	Signal_prn(double *sv_signal_I,double *sv_signal_Q, int prn, double * t_vect, int t_length,double **RxPos, int Num_RxPos, 
				   double **EphData, int Num_eph, OPTION *pOption) 
{
	int		icount, code_index, n_index;
	double	CN0, Sig_BW;	// Assuming constant at first.
	double  Noise_power, Sig_power, Sig_amp, SNR;
	double	IF2pi, L12pi, code_time, nav_time;
    long iTableIndex;
    double phasecycle;

	// Determine signal power
	CN0	=	pOption ->CN0 ;
	Sig_BW	= pOption ->Filter_BandWidth; 

	SNR	=	CN0 - 10*log10(Sig_BW);		// SNR in dB
	SNR =	pow(10, (SNR/10) );			// SNR in ratio
	Noise_power	=	Sig_BW*pow(10, (NOISE_DENSITY/10) );		// NOISE_DENSITY: dBW/Hz,  Noise_power on W
	Sig_power	=	Noise_power*SNR;
	Sig_amp		=	sqrt(2*Sig_power);	


	// Compute propagation time and Doppler
	Tp_Doppler( Tp, Tc, t_vect, t_length, prn, RxPos, Num_RxPos, pOption );

	//	Compute carrier phase
	IF2pi	=	2*PI*f_IF;
	L12pi	=	2*PI*FrequencyL1;



	if (pOption->IQ_Switch)
	{
		for ( icount = 0; icount < t_length; icount ++ )
		{
			//Carrier_I[icount]	= cos( IF2pi*t_vect[icount] - L12pi*Tc[icount] );	// Phase			/*	Can be simplified	*/
			phasecycle = f_IF*t_vect[icount] - FrequencyL1*Tc[icount];
			phasecycle = fmod(phasecycle,1.0);
			if (phasecycle<0) phasecycle+=1.0;
			iTableIndex = (int)(phasecycle*TableLength);
			Carrier_I[icount]= cosTable[iTableIndex];
			Carrier_Q[icount]= sinTable[iTableIndex];
		}
	}
	else
	{
		for ( icount = 0; icount < t_length; icount ++ )
		{
			//Carrier_I[icount]	= cos( IF2pi*t_vect[icount] - L12pi*Tc[icount] );	// Phase			/*	Can be simplified	*/
			phasecycle = f_IF*t_vect[icount] - FrequencyL1*Tc[icount];
			phasecycle = fmod(phasecycle,1.0);
			if (phasecycle<0) phasecycle+=1.0;
			iTableIndex = (int)(phasecycle*TableLength);
			Carrier_I[icount]= cosTable[iTableIndex];
		}
	}

	// Compute the combination of navigation message and PN chip
	//		With analog filter taken into consideration
	for ( icount = 0; icount < t_length; icount ++ )
	{
		double nav_t, nav_bit, chip_v, del_time, del_t, code_v;
		int	wave_index, jcount;
		wave_sample[icount] = 0;

		nav_time	=	t_vect[icount] - Tp[icount] + StartTime;
		// determine the residue time in current chip
		nav_t = nav_time - (int)nav_time;			// in second
		nav_t = nav_t*1000 - (int)(nav_t*1000);	// In ms 
		nav_t = 1023.0 * nav_t;							// in chips
		del_time = (nav_t - (int)nav_t) * (1e-3/1023.0) ;		// in s

		for ( jcount = 0; jcount < 5; jcount ++ )
		{
				// Consider all chips that have significant impact on the current sample
				// jcount == 0 obviously means the current chip

				// determin navigation message of this 
				nav_t			=	nav_time - jcount*((1e-3/1023.0)); 
				n_index		=	(int)((nav_t-DataStartTime)*50 + 1);
				nav_bit		=	EphData[prn][n_index];

				// determine chip value
				code_time	=	nav_t*1000 - (int)(nav_t*1000);  // Time within ms and with ms as the unit
				code_index	=	(int)(1 + code_time*1023.0);
				chip_v		=	CA_Table[prn-1][code_index];

				code_v		= nav_bit*chip_v;
				del_t			= del_time + jcount*((1e-3/1023.0));

				wave_index	=	(int)((del_t/WAVEINTERVAL));
				wave_sample[icount]	+= code_v*Waveform[wave_index];
		}
	}

	//	Compute Amplitude and generate signal
	for ( icount = 0; icount < t_length; icount ++ )
		sv_signal_I[icount]	=	 Sig_amp * Carrier_I[icount] * wave_sample[ icount ];

	if (pOption->IQ_Switch)
	{
		for ( icount = 0; icount < t_length; icount ++ )
				sv_signal_Q[icount]	=	Sig_amp * Carrier_Q[icount] * wave_sample[ icount ];
	}

}

//******************************************************************************
//**	Function:	Tp_Doppler												
//**	Tasks:		Compute propagation time and Doppler
//******************************************************************************
void Tp_Doppler(	    double *Tp,
						double *Tc,
						double *time, int n_T,
						int prn,
						double  **Rx_pos, int m_RX,
						OPTION *pOption	)
{
	double TimeStep,A,B,C;
	double xu,yu,zu,vxu,vyu,vzu;
	double xs=0,ys=0,zs=0,vxs=0,vys=0,vzs=0,xs1=0,ys1=0,zs1=0,xs2=0,ys2=0,zs2=0,vxs1=0,vys1=0,vzs1=0;
	double r,Tp2,Tp22,clk_bias=0;
	int m1;
	double Diff_Tp;
	int		start_count, end_count;
	double *dTrop, *dIono;
	int num;

	
	TimeStep = TrajTD; //(Rx_pos[m_RX-1][0]-Rx_pos[0][0])/(m_RX - 1);

	start_count = (int)((time[0]+ StartTime - Rx_pos[0][0])/TimeStep - 5);
	start_count = (start_count < 0)? 0: start_count;
	end_count = (int)((time[n_T-1]+StartTime - Rx_pos[0][0])/TimeStep + 5);
	end_count = (start_count >=  m_RX)? m_RX: end_count;
	

	dTrop=NULL;
	dIono=NULL;
	num=end_count-start_count+1;
	dTrop=(double*)malloc(sizeof(double)*num);
	dIono=(double*)malloc(sizeof(double)*num);
	
	if(pOption->Tropo_Switch)
	{
		double SvXyz[3];
		double troprate,timediff;
		if(dTrop==NULL) 
		{
			perror("Cannot allocate memory!");
			exit(0);
		}
		comp_sat_pos(prn, &SvXyz[0], &SvXyz[1], &SvXyz[2], &clk_bias, Rx_pos[start_count][0]);
		dTrop[0] =GenerateTropDelay(SvXyz, &Rx_pos[start_count][1], pOption);
		comp_sat_pos(prn, &SvXyz[0], &SvXyz[1], &SvXyz[2], &clk_bias, Rx_pos[end_count][0]);
		dTrop[num-1] =GenerateTropDelay(SvXyz, &Rx_pos[end_count][1], pOption);
		timediff=Rx_pos[end_count][0]-Rx_pos[start_count][0];
		if(fabs(timediff)<1.0e-8)
		{
			for(m1=1; m1<num-1; m1++)
				dTrop[m1]=dTrop[0];
		}
		else
		{
			// User linear interpolation
			troprate=(dTrop[num-1]-dTrop[0])/timediff;
			for (m1=1; m1<num-1; m1++)
				dTrop[m1]=dTrop[0]+troprate*(Rx_pos[start_count+m1][0]-Rx_pos[end_count][0]);
		}
	}
	else
	{
		for(m1=0; m1<num; m1++)
			dTrop[m1]=0.0;
	}

	if(pOption->Iono_Switch)
	{
		double SvXyz[3];
		double ionorate,timediff;
		if(dIono==NULL) 
		{
			perror("Cannot allocate memory!");
			exit(0);
		}
		comp_sat_pos(prn, &SvXyz[0], &SvXyz[1], &SvXyz[2], &clk_bias, Rx_pos[start_count][0]);
		dIono[0] =GenerateIonoDelay(SvXyz, &Rx_pos[start_count][1], Rx_pos[start_count][0],pOption);
		comp_sat_pos(prn, &SvXyz[0], &SvXyz[1], &SvXyz[2], &clk_bias, Rx_pos[end_count][0]);
		dIono[num-1] =GenerateIonoDelay(SvXyz, &Rx_pos[end_count][1], Rx_pos[end_count][0],pOption);
		timediff=Rx_pos[end_count][0]-Rx_pos[start_count][0];
		if(fabs(timediff)<1.0e-8)
		{
			for(m1=1; m1<num-1; m1++)
				dIono[m1]=dIono[0];
		}
		else
		{
			// User linear interpolation
			ionorate=(dIono[num-1]-dIono[0])/timediff;
			for (m1=1; m1<num-1; m1++)
				dIono[m1]=dIono[0]+ionorate*(Rx_pos[start_count+m1][0]-Rx_pos[end_count][0]);
		}
	}
	else
	{
		for(m1=0; m1<num; m1++)
			dIono[m1]=0.0;
	}

	//Compute Tp doppler with time interval 'TimeStep'
	for (m1 = start_count; m1<= end_count; m1++) 
	{
		time_Rx[m1] = Rx_pos[m1][0];
		xu = Rx_pos[m1][1];   vxu = Rx_pos[m1][4];
		yu = Rx_pos[m1][2];   vyu = Rx_pos[m1][5];
		zu = Rx_pos[m1][3];   vzu = Rx_pos[m1][6];
		
		//Get satellite position at time_Rx[m1] (receiving time Trx)
		//read ephemeris first
		//comp_eph(*prn,time_Rx[m1]);
		
		//Compute satellite position at time_Rx[m1] according ephemeris in frame 2
		comp_SV_posvel(prn, &xs,&ys,&zs,&vxs,&vys,&vzs, &clk_bias, time_Rx[m1]);

		//Compute Tp in frame 2 (frame at receiving time)      
		A = (vxs-vxu)*(vxs-vxu) + (vys-vyu)*(vys-vyu) + (vzs-vzu)*(vzs-vzu) - LightSpeed*LightSpeed;
		B = (xs-xu)*vxs + (ys-yu)*vys + (zs-zu)*vzs;
		C = (xs-xu)*(xs-xu) + (ys-yu)*(ys-yu) + (zs-zu)*(zs-zu);
		
		Tp2 = (B-sqrt(B*B-A*C))/A;

		//mm2 =0;