gpsfuncs.cpp

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

/***********************************************************************
  GPS RECEIVER (GPSRCVR) Ver. 1.10
  See comments in GPSRCVR.CPP
  12 Channel All-in-View GPS Receiver Program based on Mitel GP2021
  chipset
  Clifford Kelley <[e-mail]>
  Copyright (c) 1996-2001 Clifford Kelley.  All Rights Reserved.
  This LICENSE must be included with the GPSRCVR code.
***********************************************************************/
/*
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

			CONDITIONS

1. Redistributions of GPSRCVR source code must retain the above copyright
notice, this list of conditions, and the following disclaimer.

2. Redistributions in binary form must contain the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.

3. All modifications to the source code must be clearly marked as
such.  Binary redistributions based on modified source code must be
clearly marked as modified versions in the documentation and/or other
materials provided with the distribution.

4. Notice must be given of the location of the availability of the
unmodified current source code, e.g.,
	http://www.Kelley.com/
or
	ftp://ftp.Kelley.com
in the documentation and/or other materials provided with the
distribution.

5. All advertising and published materials mentioning features or use
of this software must display the following acknowledgment:  "This
product includes software developed by Clifford Kelley and other
contributors."

6. The name of Clifford Kelley may not be used to endorse or promote
products derived from this software without specific prior written
permission.

			DISCLAIMER

This software is provided by Clifford Kelley and contributors "as is" and
any expressed or implied warranties, including, but not limited to, the
implied warranties of merchantability and fitness for a particular
purpose are disclaimed.  In no event shall Clifford Kelley or
contributors be liable for any direct, indirect, incidental, special,
exemplary, or consequential damages (including, but not limited to,
procurement of substitute goods or services; loss of use, data, or
profits; or business interruption) however caused and on any theory of
liability, whether in contract, strict liability, or tort (including
negligence or otherwise) arising in any way out of the use of this
software, even if advised of the possibility of such damage.
*/

#include  <time.h>

//int matherr (struct exception *a);

char tzstr[40];// = "TZ=PST8PDT";
time_t thetime;

FILE *stream;

struct almanac                          // Approximate orbital parameters
{
  float w,ety,inc,rra,sqa,lan,aop,ma,toa,af0,af1;
  char text_message[23];
  int health,week,sat_file;
};
almanac gps_alm[33];

int   SVh[33],ASV[33];
float b0,b1,b2,b3,al0,al1,al2,al3; // broadcast ionospheric delay model
float a0,a1,tot,WNt,dtls,WNlsf,DN,dtlsf; //broadcast UTC data

struct ephemeris                          // Precise orbital parameters
{
 int iode,iodc,ura,valid,health,week;
 double dn,tgd,toe,toc,omegadot,idot,cuc,cus,crc,crs,cic,cis;
 double ma,e,sqra,w0,inc0,w,wm,ety,af0,af1,af2;
};
ephemeris gps_eph[33];

struct satvis
{
  float azimuth,elevation,doppler;
  float x,y,z;
};

struct ecef
{
  double x,y,z;
};

struct eceft
{
  double x,y,z,tb;
  float az,el;
};

struct llh
{
  double lat,lon,hae;
};

struct pvt
{
   double x,y,z,dt;
   double xv,yv,zv,df;
};

pvt rpvt;
float gdop,pdop,hdop,vdop,tdop,alm_toa;
unsigned long  clock_tow;
llh  rec_pos_llh;
llh current_loc,rp_llh;
eceft track_sat[13];
ecef satuse[13],rec_pos_xyz;
int alm_gps_week,gps_week,almanac_valid,almanac_flag,handle;
double Pr[12],dPr[12],tropo[12],iono[12];
unsigned long sf[6][11];
int p_error[6],status;

enum {off,acquisition,confirm,pull_in,track};
//     0       1         2       3      4
enum {cold_start,warm_start,hot_start,tracking,navigating};
//        0          1          2        3          4

unsigned long test_l[33]={0x00000000L,              // single bit set numbers
  0x00000001L,0x00000002L,0x00000004L,0x00000008L,  // for testing bit positions
  0x00000010L,0x00000020L,0x00000040L,0x00000080L,
  0x00000100L,0x00000200L,0x00000400L,0x00000800L,
  0x00001000L,0x00002000L,0x00004000L,0x00008000L,
  0x00010000L,0x00020000L,0x00040000L,0x00080000L,
  0x00100000L,0x00200000L,0x00400000L,0x00800000L,
  0x01000000L,0x02000000L,0x04000000L,0x08000000L,
  0x10000000L,0x20000000L,0x40000000L,0x80000000L};

float mask_angle;
char  header[45],text[27],trailer;
double meas_dop[13];

ecef d_sat[13];

long carrier_ref=0x1f7b1b9L,code_ref=0x016ea4a8L;
double  dt[13],cbias;
//
// constants defined for GPS
//
double const pi=3.1415926535898E0,r_to_d=57.29577951308232;
double const c=2.99792458e8,omegae=7.2921151467E-5;
//    WGS-84 speed of light m/sec and earth rotation rate rad/sec
double const a=6378137.0,b=6356752.314; //  WGS-84 ellipsoid parameters
double const lambda=.1902936728; // L1 wavelength in meters

//
// binary constants for nav message decoding
//

double c_2p12 = 4096;
double c_2p4  = 16;
double c_2m5  = 0.03125;
double c_2m11 = 4.8828125e-4;
double c_2m19 = 1.9073486328125e-6;
double c_2m20 = 9.5367431640625e-7;
double c_2m21 = 4.76837158203125e-7;
double c_2m23 = 1.19209289550781e-7;
double c_2m24 = 5.96046447753906e-8;
double c_2m27 = 7.45058059692383e-9;
double c_2m29 = 1.86264514923096e-9;
double c_2m30 = 9.31322574615479e-10;
double c_2m31 = 4.65661287307739e-10;
double c_2m33 = 1.16415321826935E-10;
double c_2m38 = 3.63797880709171e-12;
double c_2m43 = 1.13686837721616e-13;
double c_2m50 = 8.881784197e-16;
double c_2m55 = 2.77555756156289e-17;


int m_tropo, m_iono, align_t; // flags for using tropo and iono models

FILE *in,*out,*kalm;

satvis  xyz[33];
inline int  bit_test_l(unsigned long ,char);
void  parity_check(void);
int exor(char, long);
long fix_sqrt(long);
inline long i_sqrt(long);
inline long fix_atan2(long ,long );
inline long rss(long ,long);
satvis satfind(char);

void read_initial_data(void);

void read_almanac(void);
void write_almanac(void);

void read_ephemeris(void);
void write_ephemeris(void);

void read_ion_utc(void);
void write_ion_utc(void);

ecef satpos_almanac( float , char );
eceft satpos_ephemeris(double,char);
llh receiver_loc(void);
llh ecef_to_llh(ecef);
ecef llh_to_ecef(llh);
double tropo_iono(char,float,float,double);
void navmess(char,char);

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

PARAMETERS None.

PURPOSE

	THIS FUNCTION DETERMINES THE SATELLITES TO SEARCH FOR
	WHEN ALMANAC DATA IS AVAILABLE

WRITTEN BY
	Clifford Kelley

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

satvis satfind(char i)
{
	float ralt,tdot,beta,az;
   float satang,alm_time,almanac_date;
   double range1,range2,xls,yls,zls,xaz,yaz,xn,yn,zn,xe,ye;
   long  jd_yr;
   ecef gpspos1,gpspos2;
   satvis result;
   int  jd_m;
   struct tm *gmt;
   double time_s;
/*
      INITIALIZE ALL THE CONSTANTS
*/
//	gotoxy(1,24);
//	printf("->satfind");
	putenv(tzstr);
	tzset();
//   thetime=time(NULL);
	gmt=gmtime(&thetime);
// set up the correct time
	if (gmt->tm_mon <= 1)
	{
	  jd_yr =365.25*(gmt->tm_year-1.+1900.);
	  jd_m  =30.6001*(gmt->tm_mon+14.);
	}
	else
	{
	  jd_yr=365.25*(gmt->tm_year+1900.);
	  jd_m =30.6001*(gmt->tm_mon+2.);
	}
	time_s=gmt->tm_min/1440.+gmt->tm_sec/86400.+1720981.5+gmt->tm_hour/24.
	 +jd_yr+jd_m+gmt->tm_mday;
	gps_week=int((time_s-2444244.5)/7.);
	almanac_date=gps_alm[i].week*7.0+2444244.5;
   if (gps_week-gps_alm[i].week>512) almanac_date+=1024*7.0;
   alm_time=(time_s-almanac_date)*86400.;
   clock_tow=(time_s-gps_week*7.-2444244.5)*86400.;
/*
      CALCULATE THE POSITION OF THE SATELLITES
*/
   if (gps_alm[i].inc > 0.0 && i>0)
      {
	gpspos1=satpos_almanac(alm_time,i);
	gpspos2=satpos_almanac(alm_time+1.0,i);
/*
      CALCULATE THE POSITION OF THE RECEIVER
*/
	rec_pos_xyz=llh_to_ecef(current_loc);
	xn  =-cos(current_loc.lon)*sin(current_loc.lat);
	yn  =-sin(current_loc.lon)*sin(current_loc.lat);
	zn  = cos(current_loc.lat);
	xe  =-sin(current_loc.lon);
	ye  = cos(current_loc.lon);
/*
     DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
*/
	xls =gpspos1.x-rec_pos_xyz.x;
	yls =gpspos1.y-rec_pos_xyz.y;
	zls =gpspos1.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;
	range2=sqrt(pow(gpspos2.x-rec_pos_xyz.x-rpvt.xv,2)+
					pow(gpspos2.y-rec_pos_xyz.y-rpvt.yv,2)+
					pow(gpspos2.z-rec_pos_xyz.z-rpvt.zv,2));
	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.x=gpspos1.x;
	result.y=gpspos1.y;
	result.z=gpspos1.z;
	result.elevation=satang;
	result.azimuth  =az;
	result.doppler  =(range1-range2)*5.2514;
   }
//	gotoxy(1,24);
//	printf("satfind->");
	return(result);
}


/*******************************************************************************
FUNCTION satpos_almanac(float time, char n)
RETURNS  None.

PARAMETERS
			time   float  time of week
			n      char   satellite prn

PURPOSE

	  THIS SUBROUTINE CALCULATES THE SATELLITE POSITION
	  BASED ON ALMANAC DATA
	  
     R    - RADIUS OF SATELLITE AT TIME T
     SLAT - SATELLITE LATITUDE
     SLONG- SATELLITE LONGITUDE
     T    - TIME FROM START OF WEEKLY EPOCH
     ETY  - ORBITAL ECCENTRICITY
     TOA  - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
     INC  - ORBITAL INCLINATION
     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

WRITTEN BY
	Clifford Kelley

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

ecef satpos_almanac( float time, char n)
{
      double ei,ea,diff,r,ta,la,aol,xp,yp,d_toa;
      ecef result;
/*
      MA IS THE ANGLE FROM PERIGEE AT TOA
*/
		d_toa=time-gps_alm[n].toa;
		if (d_toa>302400.0) d_toa=d_toa-604800.0;
		else if (d_toa<-302400.0)d_toa=d_toa+604800.0;
		ei=gps_alm[n].ma+d_toa*gps_alm[n].w;
      ea=ei;
		do
		 {
       diff=(ei-(ea-gps_alm[n].ety*sin(ea)))/(1.-gps_alm[n].ety*cos(ea));
       ea=ea+diff;
       } while (fabs(diff) > 1.0e-6);
/*
      EA IS THE ECCENTRIC ANOMALY
*/
      if (gps_alm[n].ety != 0.0 )
	ta=atan2(sqrt(1.-pow(gps_alm[n].ety,2))*sin(ea),cos(ea)-gps_alm[n].ety);
      else
	ta=ea;
/*
      TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
*/
      r=pow(gps_alm[n].sqa,2)*(1.-pow(gps_alm[n].ety,2)*cos(ea));
/*
      R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
*/
      aol=ta+gps_alm[n].aop;
/*
      AOL IS THE ARGUMENT OF LATITUDE

		LA IS THE LONGITUDE OF THE ASCENDING NODE
*/
      la=gps_alm[n].lan+(gps_alm[n].rra-omegae)*d_toa-gps_alm[n].toa*omegae;
      xp=r*cos(aol);
      yp=r*sin(aol);
      result.x=xp*cos(la)-yp*cos(gps_alm[n].inc)*sin(la);
      result.y=xp*sin(la)+yp*cos(gps_alm[n].inc)*cos(la);
      result.z=yp*sin(gps_alm[n].inc);
      return(result);
}

/*******************************************************************************
FUNCTION satpos_ephemeris(double t, char n)
RETURNS  None.

PARAMETERS
			t  double   time of week
			n  char     satellite prn

PURPOSE

     THIS SUBROUTINE CALCULATES THE SATELLITE POSITION
     BASED ON BROADCAST EPHEMERIS DATA