simulator.c

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

//******************************************************************************
//**																									**
//**	          GPS L1 C/A Intermediate Frequency  Signal simulator
//**																
//******************************************************************************

#include	<stdio.h>
#include	<conio.h>
#include	<stdlib.h>
#include	<math.h>
#include	<time.h>
#include	"simulator.h"
#include	"Matrix_Tools.h"
#include	"GenNavDataBit.h"
#include	"GenSamples.h"
#include	"Sim_Util.h"


OPTION		stOption;

double		StartTime;
double		EndTime;

double		loop_time		=	0.5; 
double		f_IF;		//	Intermediate frequency
double		fs	;		//	Sampling frequency
double		f_BF;		// Base band frequency

double		DataStartTime, DataEndTime;
char			CA_Table[SAT_NUM][1025];	// used to buffer C/A code for every SV
double		Waveform[WAVEFORM_LENGTH];					// Buffer waveform when banswidth is not infinite.
double      cosTable[TableLength];
double		sinTable[TableLength];

double		**RxPos			=	NULL;
double		**EphData		=	NULL;

FILE		*fp_sample_I, *fp_sample_Q;

void CreateCosTable()
{
    double deltaPhase = 2*PI/TableLength;
    int i;
    for (i=0; i<=TableLength; i++)
    {
        cosTable[i]=cos(i*deltaPhase);
		sinTable[i]=sin(i*deltaPhase);
    }

}

int	main()
{

	int			itemp, loop_count, Num_eph;
	double		start_end, total_sim_length;
	double		t_handle, sample_length;
	double		*time_vect, *ComSignal_I, *ComSignal_Q;
	char			pos, neg;
	char*		ptempchar;
	int			err_code;
	long int		t_vec_length;
	int iSV;

	//	Allocate memory for trajectory in the beginning
	//    The maximum simulation length is determined by it
	//	  Currently, one hour limit is assumed.
	long int	Num_RxPos ;
	long int		Max_Num_RxPos = (long int)(3600/TrajTD);

	char* ssign;

    CreateCosTable();

	RxPos		=	dalloc2d(Max_Num_RxPos,10);	

	// print the tile of the software, 
	//		including brief software description and copyright
	PrintHeader();

	// get processing parameters
	err_code	=	GetOptions(&stOption);
	if (err_code	==	FAILURE)
		exit(0);

	// confirm processing parameters
	err_code	=	ConfirmOptions(&stOption);
	if (err_code	==	FAILURE)
		exit(0);

	// Calculate trajectory from the trajectory input file
	err_code	=	Load_Trajectory( &stOption, & Num_RxPos);
	if (err_code	==	FAILURE)
		exit(0);

	pos = 1;	neg = -1;

	//	Determine simulation length
	StartTime	=	RxPos[0][0];
	EndTime		=	RxPos[Num_RxPos-1][0];
	start_end	=	 EndTime -  StartTime;

	// Derive filtered waveform, and save in a global buffer
	err_code		=	Generate_Signalwaveform(&stOption, Waveform );
	if (err_code	==	FAILURE)
		exit(0);


	// Create CA_Code Table
	for (iSV = 1; iSV<= SAT_NUM; iSV ++ )
	{
		// A little bit tricky. Format: CA[1023] CA[1-1023] CA[1] 
		//  By extending the C/A code, only one pointer is necessary. 
		create_cacode(&(CA_Table[iSV-1][1]), iSV);
		CA_Table[iSV-1][0]		= CA_Table[iSV-1][1023];
		CA_Table[iSV-1][1024]	= CA_Table[iSV-1][1];
	}


	system("CLS");
	printf("\r\n\r\n  Pre-processing ... \r\n\r\n");

	// Create output sample file.
	fp_sample_I = fopen(stOption.Output_filename_I,"wb");
	if (fp_sample_I == NULL )
	{
		printf("Cannot create sample file, and program exists. \r\n");
		exit(0);
	}

	if (stOption.IQ_Switch)
	{
		fp_sample_Q = fopen(stOption.Output_filename_Q,"wb");
		if (fp_sample_Q == NULL )
		{
			printf("Cannot create sample file, and program exists. \r\n");
			exit(0);
		}
	}
		
	// Calculate Base-band frequency and check if aliasing exists
	f_IF	=	stOption.IF_Frequency;
	fs		=	stOption.Sampling_Frequency;
	f_BF = f_IF - floor(f_IF/fs)*fs;
	if	( ((int)floor(f_IF*2/fs)) % 2 )
		f_BF = fs- f_BF;
	
	if  ((f_BF-SIG_BW/2<0)|(f_BF+SIG_BW/2>fs/2) )
	{
		printf("Sampling rate is not valid \r\n" );
		exit(0);
	}

	// Get sat ephemeris
	Read_SVeph( &stOption, StartTime );

	// Navigation Data simulation for all SVs.
	{
		int		prn;

		DataStartTime		=	floor(StartTime/30.0)*30.0-30.0 -30.0;
		DataEndTime			=	floor(EndTime/30.0)*30.0 + 30.0 + 30.0;

		Num_eph				=	(int)ceil((DataEndTime-DataStartTime)/TrajTD);	// Will the time difference.
		EphData				=	dalloc2d(SAT_NUM+1,Num_eph + 1);
		for (prn = 1; prn <= SAT_NUM;  prn ++ )
		{
			if ( sv_exist ( prn ) )
			{
				EphData[prn][0]	=	prn;
				//Gen_NavData_Bit( &EphData[prn][1], prn, DataStartTime, DataEndTime, Num_eph, &stOption);
				Gen_NavData_Bit( &EphData[prn][1], prn, DataStartTime, Num_eph);
			}
		}
	}

	//loop_time = start_end;
    t_vec_length	= (long int)(loop_time*fs);

	// Allocate all the necessary memory before simulation to save simulation time
	Allocate_Mem ( t_vec_length , Num_RxPos );

	ssign=(char*) malloc(sizeof(char)*t_vec_length);
	if(ssign==NULL)
	{
		perror("cannot allocate ssgin");
		exit(0);
	}
	// Generate sampling data
	t_handle	=	0;
	loop_count		=	0;
	sample_length	=	1/fs;
	time_vect	=	dalloc1d(t_vec_length);
	ComSignal_I	=	dalloc1d(t_vec_length);
	ComSignal_Q	=	dalloc1d(t_vec_length);


	printf( "\r\n Start to Simulate Data for Each Satellite, Press 'Q' to quit. \r\n ");

	total_sim_length = floor((start_end- TrajTD)/loop_time)*loop_time;
	
	//Start Simulation
	while ( t_handle < start_end )
	{
		double d_temp, d_len, noise_amp;
		d_len = t_vec_length;
		for (itemp = 0; itemp < t_vec_length; itemp ++ )
		{
			d_temp = (double)itemp;
			time_vect[itemp] = t_handle + loop_time*d_temp/d_len ;
		}
		t_handle += loop_time;

		// Signal simulation, the major function of the simulation
		
		GenComSig(time_vect, t_vec_length, RxPos, Num_RxPos, EphData, Num_eph, ComSignal_I, ComSignal_Q, &stOption);

		// Add noise
		if (stOption.CN0<55)
			{
			noise_amp	=	sqrt(stOption.Filter_BandWidth*pow(10, (NOISE_DENSITY/10) ) );
			srand( (unsigned)time( NULL ) );
 			for (itemp = 0; itemp < t_vec_length; itemp ++ )
				ComSignal_I[itemp] +=  noise_amp*randn( );		//	randn N(0,1) standard normal distribution   

			if (stOption.IQ_Switch)
			{
				srand( (unsigned)time( NULL ) );
				for (itemp = 0; itemp < t_vec_length; itemp ++ )
					ComSignal_Q[itemp] +=  noise_amp*randn( );		//	randn N(0,1) standard normal distribution   
			}
		}
		// Quantization 
		// One bit synchronization
		if ( stOption.Quantization_Bit == 1)
		{
			char pos, neg;
			pos = 1;
			//neg = -1;
            neg = 0;
			ptempchar=ssign;
			for (itemp = 0; itemp < t_vec_length; itemp ++ )
			{
				if (ComSignal_I[itemp] >0)
					*ptempchar++=pos;
				else
					*ptempchar++=neg;
			}
 			fwrite(ssign,sizeof(char),t_vec_length,fp_sample_I);

			ptempchar=ssign;
			if (stOption.IQ_Switch)
			{
				for (itemp = 0; itemp < t_vec_length; itemp ++ )
				{
					if (ComSignal_Q[itemp] >0)
						*ptempchar++=pos;
					else
						*ptempchar++=neg;
				}
				fwrite(ssign,sizeof(char),t_vec_length,fp_sample_Q);
			}
		}
		else
		{
			double	rms_I, rms_Q, max_th_I, delta_th_I, max_th_Q, delta_th_Q;
			double   opt_k[6] = {0.0,0.0,0.9860,1.7310, 2.2910,2.2910 };
			int	   Q_bit;

			Q_bit = stOption.Quantization_Bit;

			//	Determine the threshold
			rms_I = 0.0;
			for (itemp = 0; itemp < t_vec_length; itemp ++ )
				rms_I +=	(ComSignal_I[itemp]*ComSignal_I[itemp]);

			rms_I	=	sqrt(rms_I/t_vec_length);
			max_th_I = opt_k[Q_bit]*rms_I;
			delta_th_I	=	max_th_I/((1<<(Q_bit -1)) - 1);

			ptempchar=ssign;
			for (itemp = 0; itemp < t_vec_length; itemp ++ )
			{

				if (ComSignal_I[itemp] >= 0 )
					*ptempchar++ = ((int)floor(ComSignal_I[itemp]/delta_th_I)) * 2 + 1;
				else
					*ptempchar++ = - ((int)floor(-ComSignal_I[itemp]/delta_th_I)) * 2 - 1;
				
			}
			fwrite(ssign,sizeof(char),t_vec_length,fp_sample_I);

			if (stOption.IQ_Switch)
			{
				rms_Q = 0.0;
				ptempchar=ssign;
				for (itemp = 0; itemp < t_vec_length; itemp ++ )
					rms_Q +=	(ComSignal_Q[itemp]*ComSignal_Q[itemp]);

				rms_Q =	sqrt(rms_Q/t_vec_length);
				max_th_Q = opt_k[Q_bit]*rms_Q;
				delta_th_Q	=	max_th_Q/((1<<(Q_bit -1)) - 1);

				for (itemp = 0; itemp < t_vec_length; itemp ++ )
				{
					if (ComSignal_Q[itemp] >= 0 )
						*ptempchar++ = ((int)floor(ComSignal_Q[itemp]/delta_th_Q)) * 2 + 1;
					else
						*ptempchar++ = - ((int)floor(-ComSignal_Q[itemp]/delta_th_Q)) * 2 - 1;
					
				}
				fwrite(ssign,sizeof(char),t_vec_length,fp_sample_Q);
			}

		}

		printf("\r\nProcessed time:  %f (second)    Completed: %%%2d\r\n", t_handle, (int)(100*t_handle/total_sim_length) );
		printf( "\r\nKeep Simulating Data, Press 'Q' to quit. \r\n\r\n ");

		/* Quit at any time by pressign "Q"*/
		if ( _kbhit() )
		{
			char comm_char;
			comm_char	=	getch();
			if (comm_char == 'Q' || comm_char == 'q')
				break;
		}	

		if ( (t_handle + loop_time) >= start_end )
		{
			printf("\n\nData Simulation is Over ! \n\n");
			break;
		}

	}

	// Free Memory

	if (EphData)
		dfree2d(EphData);

	if (RxPos)
		dfree2d(RxPos);

	if (ComSignal_I)
		dfree1d(ComSignal_I);

	if (ComSignal_I)
		dfree1d(ComSignal_Q);

	if (time_vect)
		dfree1d(time_vect);

	FREE_Mem( );
	free(ssign);

	fclose(fp_sample_I);
	if (stOption.IQ_Switch)
		fclose(fp_sample_Q);

	return 0;
}