singldif.c

根据GPS官方网站提供的gps定位方法而编写的C语言gps定位代码

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




/*****************************************************************************/
void singldif_input_phase(double * Trc, vec3 Xr, vecb32 SV,  vec32 Prs, vec32 Pms, double eph[32][16] )
{
	FILE *inp;
	int prn;
	int i;

  	/*the following data should be available:
    	1. Pseudoranges at ref rcvr with receiver time of reception for each SV
    	2. Ephemeris for each SV
    	3. Pseudoranges at mov rcvr*/

  	/*open input datafile*/
  	inp = fopen("inpsd.txt","r");
  	fscanf(inp, "%*[^\n]");   /*skip comment line*/

  	/*read GPS time of reception and ref rcvr coordinates*/
  	getc(inp);
  	fscanf(inp, "%lg%lg%lg%lg%*[^\n]", Trc, Xr, &Xr[1], &Xr[2]);
  	getc(inp);

  	/*read pseudoranges*/
  	fscanf(inp, "%*[^\n]");   /*skip comment line*/
  	getc(inp);
 	for (prn = 1; prn <= 32; prn++)
	{
    		SV[prn - 1] = false;
	}
  	do 
	{
    		fscanf(inp, "%ld", &prn);
    		if (prn != 0)
		{
      			fscanf(inp, "%lg%lg%*[^\n]", &Prs[prn - 1], &Pms[prn - 1]);
      			getc(inp);
      			SV[prn - 1] = true;
    		} 
		else 
		{
      			fscanf(inp, "%*[^\n]");
      			getc(inp);
    		}
  	} while (prn != 0);

  	fscanf(inp, "%*[^\n]");   /*skip comment line*/
  	getc(inp);
  	/*read ephemeris data*/
  	while ( fscanf(inp, "%ld%*[^\n]", &prn) == 1 )
	{
    		getc(inp);
    		for (i = 1; i <= 16; i++) 
		{
      			fscanf(inp, "%lg%*[^\n]", &eph[prn - 1][i - 1]);
      			getc(inp);
    		}
  	}
    	fclose(inp);
}
void singldif_calc_phase(double Trc, vec3 Xr, vecb32 SV,  vec32 Prs, vec32 Pms, double eph[32][16])
{
	FILE *out;
	int prn;
	int i;
	static double Ttr;
	static double  tau, Trel, alpha, Rrs, Crm;
	static vec32 Pmsc;
	static mat96 Xs;
	static vec3 Xm, Xlla, tmp3;
	static boolean status;


  	/*open output data file*/
	out = fopen("outsd.txt","w");
	if ( out == NULL )
	{
		fprintf(stderr,"unable to open file outsd.txt for writing\n");
		exit(-1);
	}

  	for (prn = 1; prn <= 32; prn++) 
	{
    		if (SV[prn - 1]) 
		{  	/*do for each SV*/
      			/*calculate transit time and time of transmission*/
      			tau = Prs[prn - 1] / c;	
      			Ttr = Trc - tau;

      			/*calculate SV position and correct for earth rotation*/
      			satpos(eph[prn - 1], Ttr, &Trel, tmp3);
      			alpha = tau * We;
      			Xs[prn - 1][0] = tmp3[0] * cos(alpha) + tmp3[1] * sin(alpha);
      			Xs[prn - 1][1] = tmp3[1] * cos(alpha) - tmp3[0] * sin(alpha);
      			Xs[prn - 1][2] = tmp3[2];
      			fprintf(out, "SV     : %2ld%15.3f%15.3f%15.3f\n",
	      			prn, Xs[prn - 1][0], Xs[prn - 1][1], Xs[prn - 1][2]);

      			/*calculate differential corrected pseudorange*/
 
      			Rrs = sqrt((Xr[0] - Xs[prn - 1][0]) * (Xr[0] - Xs[prn - 1][0]) +
		 		(Xr[1] - Xs[prn - 1][1]) * (Xr[1] - Xs[prn - 1][1]) +
		 		(Xr[2] - Xs[prn - 1][2]) * (Xr[2] - Xs[prn - 1][2]));

      			Pmsc[prn - 1] = Pms[prn - 1] - Prs[prn - 1] + Rrs;

    		}  /*do for each SV*/

  	}

  	/*calculate receiver position*/
  	for (i = 1; i <= 3; i++)   /*initial guess for mov rcvr position*/
	{
    		Xm[i - 1] = Xr[i - 1];
	}
 	solve(Xs, SV, Pmsc, Xm, &Crm, &status);
  	if (!status) 
	{
    		printf("Error in solve - check input data\n");
    		return;
  	}
  	fprintf(out, "Pos XYZ: %12.3f%12.3f%12.3f%12.3f\n",
	  	Xm[0], Xm[1], Xm[2], Crm);
 	/*convert back to Lat, Lon, Alt*/
  	XYZ2LLA(Xm, Xlla);
  	fprintf(out, "Pos LLA: %15.8f%15.8f%12.3f\n",
	 	 Xlla[0] * 180.0 / pi, Xlla[1] * 180.0 / pi, Xlla[2]);

    	fclose(out);
}