gpsfuncs.cpp

此程序为GPS软件接收机的源码

	  R    - RADIUS OF SATELLITE AT TIME T
     Crc  - RADIUS COSINE CORRECTION TERM
     Crs  - RADIUS SINE   CORRECTION TERM
     SLAT - SATELLITE LATITUDE
     SLONG- SATELLITE LONGITUDE
     TOE  - TIME OF EPHEMERIS FROM START OF WEEKLY EPOCH
	  ETY  - ORBITAL INITIAL ECCENTRICITY
	  TOA  - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
     INC  - ORBITAL INCLINATION
     IDOT - RATE OF INCLINATION ANGLE
     CUC  - ARGUMENT OF LATITUDE COSINE CORRECTION TERM
     CUS  - ARGUMENT OF LATITUDE SINE   CORRECTION TERM
     CIC  - INCLINATION COSINE CORRECTION TERM
     CIS  - INCLINATION SINE   CORRECTION TERM
     RRA  - RATE OF RIGHT ASCENSION
     SQA  - SQUARE ROOT OF SEMIMAJOR AXIS
     LAN  - LONGITUDE OF NODE AT WEEKLY EPOCH
     AOP  - ARGUMENT OF PERIGEE
     MA   - MEAN ANOMALY AT TOA
     DN   - MEAN MOTION DIFFERENCE

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

eceft satpos_ephemeris(double t,char n)
{
      double ei,ea,diff,ta,aol,delr,delal,delinc,r,inc;
      double la,xp,yp,bclk,tc,d_toc,d_toe;
      double xn,yn,zn,xe,ye,xls,yls,zls,range1,ralt,tdot,satang,xaz,yaz;
      double beta,az;
      eceft result;
//
//     MA IS THE ANGLE FROM PERIGEE AT TOA
//
      d_toc=t-gps_eph[n].toc;
      if (d_toc>302400.0) d_toc=d_toc-604800.0;
      else if (d_toc<-302400.0)d_toc=d_toc+604800.0;
      bclk=gps_eph[n].af0+gps_eph[n].af1*d_toc+gps_eph[n].af2*d_toc*d_toc
	  -gps_eph[n].tgd;
      tc=t-bclk;
      d_toe=tc-gps_eph[n].toe;
      if (d_toe>302400.0) d_toe=d_toe-604800.0;
		else if (d_toe<-302400.0)d_toe=d_toe+604800.0;
		ei=gps_eph[n].ma+d_toe*(gps_eph[n].wm+gps_eph[n].dn);
      ea=ei;
      do
      {
	 diff=(ei-(ea-gps_eph[n].ety*sin(ea)))/(1.0E0-gps_eph[n].ety*cos(ea));
	 ea=ea+diff;
		} while (fabs(diff) > 1.0e-12 );
      bclk=bclk-4.442807633E-10*gps_eph[n].ety*gps_eph[n].sqra*sin(ea);
		result.tb=bclk;
//
//     ea is the eccentric anomaly
//
      ta=atan2(sqrt(1.00-pow(gps_eph[n].ety,2))*sin(ea),cos(ea)-gps_eph[n].ety);
//
//     TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
//
      aol=ta+gps_eph[n].w;
//
//     AOL IS THE ARGUMENT OF LATITUDE OF THE SATELLITE
//
//     calculate the second harmonic perturbations of the orbit
//
      delr  =gps_eph[n].crc*cos(2.0*aol)+gps_eph[n].crs*sin(2.0*aol);
      delal =gps_eph[n].cuc*cos(2.0*aol)+gps_eph[n].cus*sin(2.0*aol);
      delinc=gps_eph[n].cic*cos(2.0*aol)+gps_eph[n].cis*sin(2.0*aol);
//
//     R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
//
      r=pow(gps_eph[n].sqra,2)*(1.00-gps_eph[n].ety*cos(ea))+delr;
      aol=aol+delal;
      inc=gps_eph[n].inc0+delinc+gps_eph[n].idot*d_toe;
//      WRITE(6,*)T-TOE(N)
//
//     LA IS THE CORRECTED LONGITUDE OF THE ASCENDING NODE
//
      la=gps_eph[n].w0+(gps_eph[n].omegadot-omegae)*d_toe-
			  omegae*gps_eph[n].toe;
      xp=r*cos(aol);
      yp=r*sin(aol);
      result.x=xp*cos(la)-yp*cos(inc)*sin(la);
      result.y=xp*sin(la)+yp*cos(inc)*cos(la);
      result.z=yp*sin(inc);
      result.az=0.0;
		result.el=0.0;
      if (rec_pos_xyz.x != 0.0 || rec_pos_xyz.y != 0.0 || rec_pos_xyz.z != 0.0)
		{
//            fprintf(stream,"x=%20.10lf,y=%20.10lf,z=%20.10lf\n",rec_pos_xyz.x,
//            rec_pos_xyz.y,rec_pos_xyz.z);
/*
      CALCULATE THE POSITION OF THE RECEIVER
*/
	xn  =-cos(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
	yn  =-sin(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
	zn  = cos(rec_pos_llh.lat);
	xe  =-sin(rec_pos_llh.lon);
	ye  = cos(rec_pos_llh.lon);
/*
     DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
*/
	xls =result.x-rec_pos_xyz.x;
	yls =result.y-rec_pos_xyz.y;
	zls =result.z-rec_pos_xyz.z;
	range1=sqrt(xls*xls+yls*yls+zls*zls);
	ralt=sqrt(rec_pos_xyz.x*rec_pos_xyz.x+rec_pos_xyz.y*rec_pos_xyz.y +
				 rec_pos_xyz.z*rec_pos_xyz.z);
	tdot=(rec_pos_xyz.x*xls+rec_pos_xyz.y*yls+rec_pos_xyz.z*zls)/range1/ralt;
	xls =xls/range1;
	yls =yls/range1;
	zls =zls/range1;
	if ( tdot >= 1.00 )
		beta=0.0;
	else if ( tdot <= -1.00 )
	   beta=pi;
	else
	{
		beta=acos(tdot);
	}
	satang=pi/2.0-beta;
	xaz=xe*xls+ye*yls;
	yaz=xn*xls+yn*yls+zn*zls;
	if (xaz !=0.0 || yaz !=0.0) az=atan2(xaz,yaz);
	else az=0.0;
	result.el=satang;
	result.az=az;
      }
      return(result);
}

/*******************************************************************************
FUNCTION read_initial_data(void)
RETURNS  None.

PARAMETERS None.

PURPOSE
		  To read in all of the receiver initialization files

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

void read_initial_data(void)
{
		int   id;
		satvis dummy;
		for (id=1;id<=32;id++) gps_alm[id].inc=0.0;
/*
     READ THE INPUT DATA FILE(s)
*/
	  status=cold_start;       // for initialization, if we have enough data we
	  read_ion_utc();          // will switch to warm or hot start
     read_almanac();
     dummy=satfind(0);
     thetime=time(NULL);
}

/*******************************************************************************
FUNCTION receiver_loc(void)
RETURNS  None.

PARAMETERS None.

PURPOSE
		To read in the last location of the receiver from file "curloc.dat"
		to help in warm and hot starts

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

llh receiver_loc(void)
{
		float latitude,longitude,height;
      char  text[10];
      llh result;
      result.lat=0.0;
      result.lon=0.0;
      result.hae=0.0;
/*
     READ THE CURRENT LOCATION DATA FILE
*/
     if ((in = fopen("CURLOC.DAT", "rt")) == NULL)
    {
     printf("Cannot open curloc.dat file.\n");
     status=cold_start;
    }
	 else
    {
     fscanf(in,"%10s",text);
     fscanf(in,"%f",&latitude);
     fscanf(in,"%10s",text);
     fscanf(in,"%f",&longitude);
     fscanf(in,"%10s",text);
     fscanf(in,"%f",&height);
	  result.lat=latitude/57.296;
     result.lon=longitude/57.296;
     result.hae=height;
     }
	  fclose(in);
	  return(result);
}

/*******************************************************************************
FUNCTION navmess()
RETURNS  None.

PARAMETERS None.

PURPOSE
	This function assembles and decodes the 1500 bit nav message
	into almanac and ephemeris messages

WRITTEN BY
	Clifford Kelley
3-2-2002  Made corrections suggested by Georg Beyerle GFZ
*******************************************************************************/

void navmess(char prn,char ch)
{
  int i,j,k;
  unsigned long isqra,ie,iomega0;
  long iaf0,iomegadot;
  char itgd,iaf2;
  int iweek,icapl2,iura,ihealth,iodc,iodct,iaf1;
  unsigned int itoe,itoc;
  int iode,icrs,idn,icuc,icus,fif,icic,iomegad;
  int icis,icrc,idoe,idot;
  unsigned int iae,iatoa;
  static i4page,i5page;
  int i4data,i5data,isv,iaomegad;
  long iaaf0,iaaf1,iadeli,iaomega0,im0,inc0,iw;
  unsigned long iasqr;
  long iaw,iam0,scale,ia0,ia1;
  char ial0,ial1,ial2,ial3,ibt0,ibt1,ibt2,ibt3;
  int  itot,iWNt,idtls,iWNlsf,iDN,idtlsf;//WNa
  int sfr,word,i4p,i5p;
  float d_toe;
//
//    assemble the 1500 data bits into subframes and words
//
  d_toe=clock_tow-gps_eph[prn].toe;
  if (d_toe>302400.0) d_toe=d_toe-604800.0;
  else if (d_toe<-302400.0)d_toe=d_toe+604800.0;
  if (gps_eph[prn].valid==0 || (almanac_valid==0 && almanac_flag==0)
		|| fabs(d_toe)>7200.0)
  {
	  j=0;
	  for (sfr=1;sfr<=5;sfr++)
	  {
		  for (word=1;word<=10;word++)
		  {
			  scale=536870912L;
			  sf[sfr][word]=0;
			  for (i=0;i<=29;i++)
			  {
				  if (chan[ch].message[(j+chan[ch].offset)%1500]==1)
				  sf[sfr][word]+=scale;
				  scale=scale>>1;
				  j++;
			  }
		  }
	  }
  parity_check(); // check the parity of the 1500 bit message
//
//   EPHEMERIS DECODE subframes 1, 2, 3
//
//  subframe 1
//
//   check parity of first 3 subframes, since it is a circular register
//   we may have over written the first few bits so allow for word 1 of
//   subframe 1 to have a problem
//
  if ((p_error[1]==0 || p_error[1]==0x200) && p_error[2]==0 && p_error[3]==0)
  {
		gps_eph[prn].valid=0;
		iodc=int(((sf[1][3] & 0x3) <<8 ) | ((sf[1][8] & 0xFF0000L) >>16));
//    iodct=iodc & 0xff;
		iode=int(sf[2][3]  >> 16);
		idoe=int(sf[3][10] >> 16);
		gps_eph[prn].iode=iode;
		gps_eph[prn].iodc=iodc;
// if ( iodct != iode || iodct!= idoe || iode != idoe || gps_eph[prn].sqra==0.0)
// {
		iweek= int(sf[1][3] >> 14);
		gps_eph[prn].week=iweek;
//      icapl2=( sf[1][3] & 0x3000 ) >> 12;
		iura=int(( sf[1][3] & 0xF00 ) >> 8);
		gps_eph[prn].ura=iura;
		ihealth=int(( sf[1][3] & 0xFC ) >> 2);
		gps_eph[prn].health=ihealth;
//		gps_eph[prn].iodc=iodc;
		itgd=int(sf[1][7] & 0xFF);
		gps_eph[prn].tgd=itgd*c_2m31;
		itoc=int(sf[1][8] & 0xFFFF);
		gps_eph[prn].toc=itoc*16.0;
		iaf2=int(sf[1][9] >> 16);
		gps_eph[prn].af2=iaf2*c_2m55;
		iaf1=int(sf[1][9] & 0xFFFF);
		gps_eph[prn].af1=iaf1*c_2m43;
		iaf0=sf[1][10] >> 2;
		if (bit_test_l(iaf0,22)) iaf0=iaf0 | 0xFFC00000L;
		gps_eph[prn].af0=iaf0*c_2m31;
//
//   subframe 2
//
		icrs=int(sf[2][3] & 0xFFFF);
		gps_eph[prn].crs=icrs*c_2m5;
		idn=int(sf[2][4] >> 8);
		gps_eph[prn].dn=idn*c_2m43*pi;
		im0=((sf[2][4] & 0xFF) << 24) | sf[2][5];
		gps_eph[prn].ma=im0*c_2m31*pi;
		icuc=int(sf[2][6] >>8);
		gps_eph[prn].cuc=icuc*c_2m29;
		ie=((sf[2][6] & 0xFF) << 24) | sf[2][7];
		gps_eph[prn].ety=ie*c_2m33;
		icus=int(sf[2][8] >> 8);
		gps_eph[prn].cus=icus*c_2m29;
		isqra=(((sf[2][8] & 0xFF) << 24) | sf[2][9]);
		gps_eph[prn].sqra=isqra*c_2m19;
		if (gps_eph[prn].sqra>0.0) gps_eph[prn].wm=19964981.84/
																 pow(gps_eph[prn].sqra,3);
		itoe=int(sf[2][10] >> 8);
		gps_eph[prn].toe=itoe*c_2p4;
		fif=int((sf[2][10] & 0x80) >> 7);
//
// subframe 3
//
		icic=int(sf[3][3] >> 8);
		gps_eph[prn].cic=icic*c_2m29;
		icis=int(sf[3][5] >> 8);
		gps_eph[prn].cis=icis*c_2m29;
		inc0=((sf[3][5] & 0xFF) << 24) | sf[3][6];
		gps_eph[prn].inc0=inc0*c_2m31*pi;
		iomega0=((sf[3][3] & 0xFF) << 24) | sf[3][4];
		gps_eph[prn].w0=iomega0*c_2m31*pi;
		icrc=int(sf[3][7] >> 8);
		gps_eph[prn].crc=icrc*c_2m5;
		iw=((sf[3][7] & 0xFF) << 24) | sf[3][8];
		gps_eph[prn].w=iw*c_2m31*pi;
		iomegadot=sf[3][9];
		if (bit_test_l(iomegadot,24)) iomegadot=iomegadot | 0xFF000000L;
		gps_eph[prn].omegadot=iomegadot*c_2m43*pi;
		idot=int((sf[3][10] & 0xFFFC) >> 2);
		if (bit_test_l(idot,14)) idot=idot | 0xC000;
		gps_eph[prn].idot=idot*c_2m43*pi;
		if (gps_eph[prn].inc0!=0.0 && gps_eph[prn].sqra>5000.0 &&
		gps_eph[prn].ety <.05) gps_eph[prn].valid=1;
  }
//!
//!    ALMANAC DECODE  subframes 4 and 5
//!
//!    SUBFRAME 4
//!
//!   check parity of subframes 4 and five and don't bother if we have the almanac
//!
	 if (p_error[4]==0 && p_error[5]==0 && almanac_valid==0 && almanac_flag==0)
	 {
		 almanac_flag=1;
		 i4data= int(sf[4][3] >> 22);
		 i4p= int((sf[4][3] & 0x3F0000L) >> 16);
		 if (i4p != i4page && i4data==1)          //! i4p all we need is a page to be
		 {                                        //! read from 1 satellite
		 i4page=i4p;
		 if (i4page > 24 && i4page < 33)
		 {
			isv=i4page ;
			gps_alm[isv].week=gps_week%1024;
			iae=int(sf[4][3] & 0x00FFFFL);
			gps_alm[isv].ety=iae*c_2m21;
			iatoa=int(sf[4][4] >> 16);
			gps_alm[isv].toa=iatoa*c_2p12;
			iadeli=sf[4][4] & 0x00FFFFL;
			if (bit_test_l(iadeli,16)) iadeli=iadeli | 0xFFFF0000L;
			gps_alm[isv].inc=(iadeli*c_2m19+0.3)*pi;
			iomegad=int(sf[4][5] >> 8);
			gps_alm[isv].rra=iomegad*c_2m38*pi;
			gps_alm[isv].health=int(sf[4][5] & 0x0000FF);
			iasqr=sf[4][6];
			gps_alm[isv].sqa=iasqr*c_2m11;
			if (gps_alm[isv].sqa>0.0) gps_alm[isv].w=19964981.84/
																  pow(gps_alm[isv].sqa,3);
			iaomega0=sf[4][7];
			if (bit_test_l(iaomega0,24)) iaomega0=iaomega0 | 0xFF000000L;
			gps_alm[isv].lan=iaomega0*c_2m23*pi;
			iaw=sf[4][8];
			if (bit_test_l(iaw,24)) iaw=iaw | 0xFF000000L;
			gps_alm[isv].aop=iaw*c_2m23*pi;
			iam0=sf[4][9];
			if (bit_test_l(iam0,24)) iam0=iam0 | 0xFF000000L;
			gps_alm[isv].ma=iam0*c_2m23*pi;
			iaaf0=(sf[4][10] >> 13) | ((sf[4][10] & 0x1C)>>2);
			if (bit_test_l(iaaf0,11)) iaaf0=iaaf0 | 0xFFFFF800L;
			gps_alm[isv].af0=iaaf0*c_2m20;
			iaaf1=(sf[4][10] | 0xFFE0) >> 5;
			if (bit_test_l(iaaf1,11)) iaaf1=iaaf1 | 0xFFFFF800L;
			gps_alm[isv].af1=iaaf1*c_2m38;
		}
		else if ( i4page == 55 )
		{
			gps_alm[prn].text_message[0]=char((sf[4][3] & 0x00FF00) >> 8);
			gps_alm[prn].text_message[1]=char( sf[4][3] & 0x0000FF);
			for ( k=1;k<=7;k++)
			{
			  gps_alm[prn].text_message[3*k-1]= char(sf[4][k+3] >> 16);
			  gps_alm[prn].text_message[3*k  ]= char((sf[4][k+3] & 0x00FF00) >> 8);
			  gps_alm[prn].text_message[3*k+1]= char(sf[4][k+3] & 0x0000FF);
			}
		}
		else if ( i4page == 56 ) // Broadcast Ionosphere Model & UTC Parameters
		{
			ial0=int((sf[4][3] & 0x00FF00) >> 8);
			al0=ial0*c_2m30;
			ial1= int(sf[4][3] & 0x0000FF);