gensamples.c

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

}

void Read_SVeph( OPTION *pOption, 	double gps_sec )
{
	int prn;

	for ( prn = 1; prn <= SAT_NUM; prn ++)
	{
		// Can be put it outside this loop since ephemeris can be get as a global variable
		if ( sv_exist (prn) )
		{
			comp_eph(prn, pOption, gps_sec);
			// Result in the global variable eph;
			ephsv[ prn ]	=	eph;
		}
	}

}



int sv_exist ( int prn )
{
	// Current 29 SVs are in the sky, but only 28 work, PRN22 is not available
	static valid_sv[28] = {1,2,3,4,5,6,7,8,9,10,11,13,14,15,16,17,18,20,21,23,24,25,26,27,28,29,30,31 };
	int icount;

	for (icount = 0; icount < 28; icount ++)
		if ( valid_sv[icount] == prn)
			return 1;						// This SV(PRN) exists.
	return	0;			// This SV(PRN) does not exist. 
}


int SV_LOS_detect( int prn, double Upos[], double gps_time)
{
	int obs_cap;
	double pos_xyz[3], enu[3], clk_off, elev;

	obs_cap = sv_exist(prn);
	if ( obs_cap == 0 )
		return 0;	// This SV does not exist at all;
	if ( ephsv[prn].weekn < 1)
		return 0;

	// This SV exists, so check its elevation and azimuth
	// Get SV position first

	comp_sat_pos(prn, &pos_xyz[0], &pos_xyz[1], &pos_xyz[2], &clk_off, gps_time);

	// Tranform to ENU, local level frame
	xyz2enu( pos_xyz, Upos, enu );

	// Calculate elevation
	elev = atan(enu[2]/sqrt(enu[0]*enu[0] + enu[1]*enu[1])) * 180/PI;

	// Detect if it can be seen.
	if ( elev > ELEVCUTOFF )
		return 1;				// This SV can be seen directly (LOS)
	else
		return 0;
}


int	Read_line (char line[], double *p1, double *p2, double *p3, double *p4 )
{
	char *ppos;
	char chtmp;

	int	comp_char = 'e';

	ppos = strchr(line,'e');
	ppos +=4;
	chtmp	= *ppos;
	*ppos = 0;
	sscanf(line, "%lf", p1 );
	*ppos = chtmp;
	line = ppos;

	ppos = strchr(line,'e');
	ppos +=4;
	chtmp	= *ppos;
	*ppos = 0;
	sscanf(line, "%lf", p2 );
	*ppos = chtmp;
	line = ppos;

	ppos = strchr(line,'e');
	ppos +=4;
	chtmp	= *ppos;
	*ppos = 0;
	sscanf(line, "%lf", p3 );
	*ppos = chtmp;
	line = ppos;

	ppos = strchr(line,'e');
	ppos +=4;
	chtmp	= *ppos;
	*ppos = 0;
	sscanf(line, "%lf", p4 );
	*ppos = chtmp;
	line = ppos;

	return 0;
}


int Read_line1( char line[], int *pi1, int *pi2,int *pi3,int *pi4,int *pi5, int *pi6,
				double *pd1, double *pd2, double *pd3, double *pd4 )
{
	char chtmp, *pch;

	chtmp = line[22];

	line[22] = 0;

	sscanf(line, " %d %d %d %d %d %d %lf ", pi1 , pi2, pi3, pi4, pi5, pi6, pd1 );

	line[22] = chtmp;

	pch = &line[22];


	Read_line2(pch, pd2, pd3, pd4);

	return 0;

}


int Read_line2( char line[],double *p1, double *p2, double *p3)
{
	char *ppos;
	char chtmp;

	ppos = strchr(line,'e');
	ppos +=4;
	chtmp	= *ppos;
	*ppos = 0;
	sscanf(line, "%lf", p1 );
	*ppos = chtmp;
	line = ppos;

	ppos = strchr(line,'e');
	ppos +=4;
	chtmp	= *ppos;
	*ppos = 0;
	sscanf(line, "%lf", p2 );
	*ppos = chtmp;
	line = ppos;

	ppos = strchr(line,'e');
	ppos +=4;
	chtmp	= *ppos;
	*ppos = 0;
	sscanf(line, "%lf", p3 );
	*ppos = chtmp;
	line = ppos;

	return 0;

}


int	Generate_Signalwaveform( OPTION *pOption, double Wf[] )
{
	int Filter_Order = 10;			// Tenth order Butterworth filter is used here
	double wc = 2*PI*pOption->Filter_BandWidth/2;
	double re_pole[20], im_pole[20], re_K_Cof[20], im_K_Cof[20];
	int icount, pole_count;
	double t, wave1[WAVEFORM_LENGTH], wave2[WAVEFORM_LENGTH];
	double	cos_pole, sin_pole,	 k_re, k_im;


	// Calculation of poles of the butterworth filter 
	for (icount = 0; icount < Filter_Order; icount ++)
	{
		double ang;
		ang = PI/Filter_Order/2 + icount*PI/Filter_Order + PI/2; 
		re_pole[icount]	=	wc*cos(ang);
		im_pole[icount]	=	wc*sin(ang);
	}

	// Decompose the transfer function into the summation of the first order partial fractions
	for (icount = 0; icount < Filter_Order; icount ++)
	{
		double p_re, p_im,  re_tmp, im_tmp;

		p_re  =	re_pole[icount];
		p_im	=	im_pole[icount];
		complex_div(pow(wc,Filter_Order),0, p_re, p_im, &k_re, &k_im);
		for (pole_count = 0; pole_count < Filter_Order; pole_count ++ )
		{
			if ( pole_count != icount )
			{
				complex_sub(p_re, p_im, re_pole[pole_count], im_pole[pole_count], &re_tmp, &im_tmp);
				complex_div(k_re,k_im, re_tmp, im_tmp, &k_re, &k_im  );
			}
		}
		re_K_Cof[icount] = k_re;
		im_K_Cof[icount] = k_im;
	}

	for (icount = 0; icount < WAVEFORM_LENGTH; icount ++)
	{
		t					=	WAVEINTERVAL*icount;
		wave1[icount]	=	0;
		for (pole_count = 0; pole_count < Filter_Order; pole_count ++ )
		{
			cos_pole =	cos(im_pole[pole_count]*t);
			sin_pole =	sin(im_pole[pole_count]*t);
			k_re		=	re_K_Cof[pole_count];
			k_im		=	im_K_Cof[pole_count];
			complex_multiply(k_re,k_im,cos_pole, sin_pole, &k_re, &k_im);
			k_re		*= exp(re_pole[pole_count]*t);
			k_im		*= exp(re_pole[pole_count]*t);			
			wave1[icount] += k_re;
		}
		wave1[icount] += 1;
	}

	for (icount = 0; icount < WAVEFORM_LENGTH; icount ++)
	{
		wave2[icount]	=	0;
		t					=	WAVEINTERVAL*icount - 1e-3/1023.0;

		if  (t<0)
			continue;

		for (pole_count = 0; pole_count < Filter_Order; pole_count ++ )
		{
			cos_pole =	cos(im_pole[pole_count]*t);
			sin_pole =	sin(im_pole[pole_count]*t);
			k_re		=	re_K_Cof[pole_count];
			k_im		=	im_K_Cof[pole_count];
			complex_multiply(k_re,k_im,cos_pole, sin_pole, &k_re, &k_im);
			k_re		*= exp(re_pole[pole_count]*t);
			k_im		*= exp(re_pole[pole_count]*t);			
			wave2[icount] += k_re;
		}
		wave2[icount] += 1;

	}

	for (icount = 0; icount < WAVEFORM_LENGTH; icount ++)
		Wf[icount]	=	wave1[icount] - wave2[icount];

	return 0;

}

// to compute tropospheric slant delay on a line of sight
double GenerateTropDelay(     double *SvXyz, // satellite position
                              double *StationXyz, // station position
							  OPTION* pOption)
                              
                           
{
	double dElevation;
    double dTemperature, dPressure, dHumidity;
    const double dtdh = -6.5;

	int i;
    double alw[10], ald[10];
    double e, enw, endc, hd, hw;  // change end to endc 
    double rw, rd, aw, ad, bw, bd, delw, deld;
    double ai;
    double dTrop;
    double RADIUS = 6371000.0;
	double enu[3];

	// Tranform to ENU, local level frame
	xyz2enu( SvXyz, StationXyz, enu );

	// Calculate elevation
	dElevation = atan(enu[2]/sqrt(enu[0]*enu[0] + enu[1]*enu[1]));

    // set Met parameters to default values
//    dTemperature = 293.0 + dtdh * StationLLH[2] / 1000.0;
//    dPressure = 1010.0 * pow( (1.0 + dtdh * StationLLH[2] / (1000.0 * 293.0)), 5.255);
//    dHumidity = 0.5;
    dTemperature = pOption->temperature;
    dPressure = pOption->pressure;
    dHumidity = pOption->relativehumidity;

	//  compute the indices of refractivity for wet and dry components 

    e = dHumidity * exp(-37.2465 + 0.213166 * dTemperature
        - 2.56908e-4 * dTemperature * dTemperature);

    endc = 0.776e-4 * dPressure / dTemperature;
    enw = 0.373 * e / (dTemperature * dTemperature);
    hd = 5.0 * 0.002277 * dPressure / endc ;
    hw = 5.0 * 0.002277 * ( 1255.0 / dTemperature + 0.05 ) * e / enw ;

    //  compute correction using modified Hopfield approach   

    rw = sqrt( pow((RADIUS + hw) , 2.0)
        - pow((RADIUS * cos(dElevation)) , 2.0) )
            - RADIUS * sin(dElevation);

    rd = sqrt( pow((RADIUS + hd) , 2.0)
        - pow((RADIUS * cos(dElevation)) , 2.0) )
        - RADIUS * sin(dElevation);

    aw = - sin(dElevation) / hw;
    ad = - sin(dElevation) / hd;
    bw = - cos(dElevation) * cos(dElevation) / (2.0 * hw * RADIUS);
    bd = - cos(dElevation) * cos(dElevation) / (2.0 * hd * RADIUS);
    
    alw[0] = 1.0;
    alw[1] = 4.0 * aw;
    alw[2] = 6.0 * aw * aw + 4.0 * bw;
    alw[3] = alw[1] * ( aw * aw + 3.0 * bw );
    alw[4] = 12.0 * aw * aw * bw +6.0 * bw * bw +aw * aw * aw * aw;
    alw[5] = 4.0 * aw * bw * (aw * aw + 3.0 * bw);
    alw[6] = bw * bw * (6.0 * aw * aw + 4.0 * bw);
    alw[7] = 4.0 * aw * bw * bw * bw;
    alw[8] = bw * bw * bw * bw;
    
    ald[0] = 1.0;
    ald[1] = 4.0 * ad;
    ald[2] = 6.0 * ad * ad + 4.0 * bd;
    ald[3] = ald[1] * ( ad * ad + 3.0 * bd );
    ald[4] = 12.0 * ad * ad * bd +6.0 * bd * bd +ad * ad * ad * ad;
    ald[5] = 4.0 * ad * bd * ( ad * ad + 3.0 * bd );
    ald[6] = bd * bd * ( 6.0 * ad * ad + 4.0 * bd );
    ald[7] = 4.0 * ad * bd * bd * bd;
    ald[8] = bd * bd * bd * bd;

    delw = deld = 0.0;
    
    for( i=0; i< 9; i++)
    {
        ai = (double) (i+1);
        delw +=  alw[i] / ai * pow(rw , ai);
        deld +=  ald[i] / ai * pow(rd , ai);
    }

	dTrop =  enw * delw + endc * deld;
    return dTrop;
}

double GenerateIonoDelay(double *SvXyz, double* upos, double gps_time, OPTION* pOption)
{

	double enu[3],llh[3];
	double elev, az, pusi, phii,phiu,lamdai,lamdau,phim,t,F,x,dIono;
	// Tranform to ENU, local level frame
	xyz2enu( SvXyz, upos, enu );
	xyz2llh( upos,llh);

	phiu=llh[0]/PI;
	lamdau=llh[1]/PI;

	// Calculate elevation
	elev = atan(enu[2]/sqrt(enu[0]*enu[0] + enu[1]*enu[1])) /PI;
	az = atan2(enu[0], enu[1]) / PI;

	pusi=0.0137/(elev+0.11)-0.022;
	phii=phiu+pusi*cos(az*PI);
	if(phii>0.416)
		phii=0.416;
	else if(phii<-0.416)
		phii=-0.416;
	lamdai=lamdau+(pusi*sin(az*PI)/cos(phii*PI));
	phim=phii+0.064*cos((lamdai-1.617)*PI);
	t=4.32e4*lamdai+fmod(gps_time,86400);
	if(t>86400)
		t-=86400;
	else if (t<0)
		t+=86400;
	F=1.0+16.0*pow((0.53-elev),3.0);
	x=2*PI*(t-50400)/(pOption->beta[0]+pOption->beta[1]*phim+
		pOption->beta[2]*phim*phim+pOption->beta[3]*phim*phim*phim);
	if(fabs(x)>1.57)
		dIono=F*5e-9;
	else
		dIono=F*(5e-9+(pOption->alpha[0]+pOption->alpha[1]*phim
		+pOption->alpha[2]*phim*phim+pOption->alpha[3]*phim*phim*phim)*(1-x*x*(0.5-0.25*x*x)));
	return dIono;
}