gpsrcvr.cpp

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

			  pattern  long 32 bit data

PURPOSE
			  count the number of bits set in "pattern"
WRITTEN BY
	Clifford Kelley

*******************************************************************************/
int xors(long pattern)
{
  int count,i;
  count=0;
  pattern=pattern>>6;
  for (i=0;i<=25;i++)
  {
	 count+=int(pattern & 0x1);
	 pattern=pattern>>1;
  }
  count=count%2;
  return(count);
}

/*******************************************************************************
FUNCTION sign(long data)
RETURNS  Integer

PARAMETERS
			data Long

PURPOSE
		 This function returns
				  +1 when positive
			      0 when zero
			     -1 when negative

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
inline int sign(long data)
{
  int result;
  if      ( data  > 0 ) result= 1;
  else if ( data == 0 ) result= 0;
  else if ( data  < 0 ) result=-1;
  return(result);
}

/*******************************************************************************
FUNCTION bsign(long data)
RETURNS  Integer

PARAMETERS
			data  long integer

PURPOSE

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
inline int bsign(long data)
{
  int result;
  if      ( data  > 0 ) result= 1;
  else  result= 0;
  return(result);
}

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

PARAMETERS None.

PURPOSE
	  Determine if a bit in the data word has been set

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
inline int  bit_test(int data,char bit_n)
{
  return(data & test[bit_n]);
}

/*******************************************************************************
FUNCTION near_int(double input)
RETURNS  long

PARAMETERS
			input  double

PURPOSE
	This function finds the nearest integer of a double

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
long near_int(double input)
{
  long result;
  if (input >0.0 )result=input+0.5;
  else result=input-0.5;
  return(result);
}

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

PARAMETERS None.

PURPOSE  To allocate the PRNs to channels for a cold start, start by searching
	  for PRN 1 through 12 and cycling through all PRN numbers skipping channels
	  that are tracking

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
void  cold_allocate()
{
	 satvis dummy;
	 char ch,i,alloc;
//    d_freq=4698;           // search 200 Hz intervals
	 search_max_f=50;         // widen the search for a cold start
	 dummy=satfind(0);
	 almanac_valid=1;
	 reset_cntl(0x1fff);
	 for (i=1;i<=32;i++)
	 {
		if (gps_alm[i].inc>0.0 && gps_alm[i].week!=gps_week%1024) almanac_valid=0;
	 }
	 if (al0==0.0 && b0==0.0)almanac_valid=0;
	 for (ch=0;ch<=chmax;ch++) // if no satellite is being tracked
										// turn the channel off
	 {
		if ( chan[ch].CNo<30.0 )// if C/No is too low turn the channel off
		{
		  chan[ch].state=off;
		  chan[ch].prn=0;
		}
	 }
	 for (i=0;i<=chmax;i++)
	 {
		 alloc=0;
		 for (ch=0;ch<=chmax;ch++)
		 {
			if (chan[ch].prn==cold_prn)
			{
				alloc=1;// satellite is already allocated a channel
				break;
			}
		 }
		 if (alloc==0) // if not allocated find an empty channel
		 {
	for (ch=0;ch<=chmax;ch++)
	{
	  if (chan[ch].state==off)
	  {
		 chan[ch].carrier_corr=-clock_offset*1575.42/42.57475e-3;
		 chan[ch].carrier_freq=carrier_ref+chan[ch].carrier_corr;   // set carrier
		 ch_carrier(ch,chan[ch].carrier_freq);             // select carrier
		 chan[ch].code_freq=code_ref;
		 ch_code(ch,chan[ch].code_freq);       // 1.023 MHz chipping rate
		 ch_cntl(ch,prn_code[cold_prn]|0xa000);// 0xa000 for late select satellite
		 chan[ch].prn=cold_prn;
		 ch_on(ch);
		 chan[ch].state=acquisition;
		 chan[ch].codes=0;
		 chan[ch].n_freq=search_min_f;
		 chan[ch].del_freq=1;
		 cold_prn=cold_prn%32+1;
		 break;
	  }
	}
		}
	 }
}

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

PARAMETERS None.

PURPOSE  To read in from the rcvr_par file the receiver parameters that control
			acquisition, tracking etc.

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
void read_rcvr_par(void)
{
    char intext[40];
	 if ((in = fopen("rcvr_par.dat", "rt")) == NULL)
	 {
		printf("Cannot open rcvr_par.dat file.\n");
		exit(0);
	 }
	 else
	 {
		fscanf(in,"%s %s",intext,tzstr);
		fscanf(in,"%s %f",intext,&mask_angle);
		mask_angle=mask_angle/r_to_d;
		fscanf(in,"%s %f",intext,&clock_offset);
		fscanf(in,"%s %d",intext,&interr_int);
		fscanf(in,"%s %d",intext,&acq_thresh);
		fscanf(in,"%s %d",intext,&search_min_f);
		fscanf(in,"%s %d",intext,&cold_prn);
		fscanf(in,"%s %d",intext,&rms);
		fscanf(in,"%s %d",intext,&pull_code_k);
		fscanf(in,"%s %d",intext,&pull_code_d);
		fscanf(in,"%s %d",intext,&pull_carr_k);
		fscanf(in,"%s %d",intext,&pull_carr_d);
		fscanf(in,"%s %d",intext,&trk_code_k);
		fscanf(in,"%s %d",intext,&trk_code_d);
		fscanf(in,"%s %d",intext,&trk_carr_k);
		fscanf(in,"%s %d",intext,&trk_carr_d);
		fscanf(in,"%s %f",intext,&nav_up);
		fscanf(in,"%s %d",intext,&pull_in_time);
		fscanf(in,"%s %d",intext,&phase_test);
		fscanf(in,"%s %d",intext,&d_freq);
		fscanf(in,"%s %d",intext,&trk_div);
		fscanf(in,"%s %d",intext,&n_of_m_thresh);
		fscanf(in,"%s %d",intext,&confirm_m);
		fscanf(in,"%s %d",intext,&out_pos);
		fscanf(in,"%s %d",intext,&out_vel);
		fscanf(in,"%s %d",intext,&out_time);
		fscanf(in,"%s %d",intext,&out_kalman);
		fscanf(in,"%s %d",intext,&out_debug);
		fscanf(in,"%s %d",intext,&m_tropo);
		fscanf(in,"%s %d",intext,&m_iono);
		fscanf(in,"%s %d",intext,&align_t);
		}
	 fclose(in);
}
/*******************************************************************************
FUNCTION velocity(void)
RETURNS  None.

PARAMETERS None.

PURPOSE  To convert velocity from ecef to local level (WGS-84) axes

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
void velocity(void)
{
	 double  pt,pb,dxn,dyn,dzn,dn_mag,dxe,dye,de_mag,dxu,dyu,dzu;
	 double  north,east,up;
//	 gotoxy(1,24);
//	 printf("->velocity");
	 pt=(a*a-b*b)*sin(rp_llh.lat)*cos(rp_llh.lat);
	 pb=sqrt(a*a*cos(rp_llh.lat)*cos(rp_llh.lat) +
		 b*b*sin(rp_llh.lat)*sin(rp_llh.lat));
	 dxn=a*a*pt/pow(pb,3)*cos(rp_llh.lat)*cos(rp_llh.lon)-(a*a/pb+rp_llh.hae)*
			sin(rp_llh.lat)*cos(rp_llh.lon);
	 dyn=a*a*pt/pow(pb,3)*cos(rp_llh.lat)*sin(rp_llh.lon)-(a*a/pb+rp_llh.hae)*
			sin(rp_llh.lat)*sin(rp_llh.lon);
	 dzn=b*b*pt/pow(pb,3)*sin(rp_llh.lat)+(b*b/pb+rp_llh.hae)*cos(rp_llh.lat);
	 dn_mag=sqrt(dxn*dxn+dyn*dyn+dzn*dzn);     // north magnitude
	 if (dn_mag==0.0) north =0.0;
	 else north=(rpvt.xv*dxn+rpvt.yv*dyn+rpvt.zv*dzn)/dn_mag;
	 dxe=-(a*a/pb+rp_llh.hae)*cos(rp_llh.lat)*sin(rp_llh.lon);
	 dye= (a*a/pb+rp_llh.hae)*cos(rp_llh.lat)*cos(rp_llh.lon);
	 de_mag=sqrt(dxe*dxe+dye*dye);             // east magnitude
	 if (de_mag==0.0) east=0.0;
	 else east=(rpvt.xv*dxe+rpvt.yv*dye)/de_mag;
	 dxu=cos(rp_llh.lat)*cos(rp_llh.lon);
	 dyu=cos(rp_llh.lat)*sin(rp_llh.lon);
	 dzu=sin(rp_llh.lat);

	 up=rpvt.xv*dxu+rpvt.yv*dyu+rpvt.zv*dzu;   // up magnitude

	 speed=sqrt(north*north+east*east);
	 if (speed==0.0) heading=0.0;
	 else heading=atan2(east,north);
	 receiver.vel.north=north;
	 receiver.vel.east=east;
	 receiver.vel.up=up;
//	 gotoxy(1,24);
//	 printf("velocity->");
	 if (pb==0.0) exit(0);
}

/*******************************************************************************
FUNCTION pream(char ch, char bit)
RETURNS  None.

PARAMETERS

	ch   channel number (0-11)
	bit  the latest data bit from the satellite

PURPOSE
	This function finds the preamble in the navigation message and synchronizes
   to the nav message

WRITTEN BY
	Clifford Kelley
made changes suggested by Jenna Cheng, UCR to make routine more readable
*******************************************************************************/
void pream(char ch,char bit)
{
  static unsigned long pream=0x22c00000L;
  unsigned long  parity0,parity1;
  static unsigned long pb1=0xbb1f3480L,pb2=0x5d8f9a40L,pb3=0xaec7cd00L;
  static unsigned long pb4=0x5763e680L,pb5=0x6bb1f340L,pb6=0x8b7a89c0L;
  unsigned long TOWs,HOW,TLM,TLMs;
  int sfid_s;

  if (chan[ch].fifo1&0x20000000L)
  {
	  chan[ch].fifo0=(chan[ch].fifo0<<1)+ 1;
  }
  else
  {
	  chan[ch].fifo0=chan[ch].fifo0<<1;
  }

  chan[ch].fifo1=(chan[ch].fifo1<<1)+bit;

  if (chan[ch].fifo0&0x40000000L)
  {
	  TLM = chan[ch].fifo0^0x3fffffc0L;
  }
  else
  {
	  TLM = chan[ch].fifo0;
  }

  if (chan[ch].fifo1&0x40000000L)
  {
	  HOW = chan[ch].fifo1^0x3fffffc0L;
  }
  else
  {
	  HOW = chan[ch].fifo1;
  }

  if (((pream^TLM)&0x3fc00000L)==0)   // preamble pattern found?
  {

	  parity0=(xors(TLM & pb1)<<5)+(xors(TLM & pb2)<<4)+
			  (xors(TLM & pb3)<<3)+(xors(TLM & pb4)<<2)+
			  (xors(TLM & pb5)<<1)+(xors(TLM & pb6));

	  if (parity0==(TLM & 0x3f))  // is parity of TLM ok?
	  {
		  parity1=(xors(HOW & pb1)<<5)+(xors(HOW & pb2)<<4)+
					 (xors(HOW & pb3)<<3)+(xors(HOW & pb4)<<2)+
					 (xors(HOW & pb5)<<1)+(xors(HOW & pb6));

		  if (parity1==(HOW & 0x3f))  // is parity of HOW ok?
		 {
			sfid_s=int((HOW & 0x700)>>8); // compute the subframe id number
			TLMs=(TLM>>2) & 0x3fff;
			if (sfid_s==1)     // synchronize on subframe 1 if TOW matches
			{                         // clock within 300 seconds
			  TOWs =(HOW & 0x3fffffffL)>>13;
			  d_tow=clock_tow-TOWs*6+5; // why the +5?
			  if ( labs(d_tow)<300)
			  {
					chan[ch].offset=chan[ch].t_count-59;
					ch_epoch_load(ch,(0x1f&ch_epoch_chk(ch))|0xa00);  ///// cwk
					if (chan[ch].offset<0.0) chan[ch].offset+=1500;
					chan[ch].tr_bit_time=TOWs*300-240;
					chan[ch].TOW=TOWs*6;
					chan[ch].tow_sync=1;
					thetime=thetime-d_tow;
					clock_tow=TOWs*6-5;
					chan[ch].sfid=sfid_s;
					chan[ch].TLM=TLMs;
			  }
			}
// allow resync on other subframes if TOW matches clock to 3 seconds
// this should improve the re-acquisition time
			else if (sfid_s>1 && sfid_s<6)
			{
			  TOWs =(HOW & 0x3fffffffL)>>13;
			  d_tow=clock_tow-TOWs*6+5;  // why the +5?
			  if ( labs(d_tow)<3)
			  {
					chan[ch].offset=chan[ch].t_count-59-(sfid_s-1)*300;
		         ch_epoch_load(ch,(0x1f&ch_epoch_chk(ch))|0xa00);  ///// cwk
		         if (chan[ch].offset<0.0) chan[ch].offset+=1500;
		         chan[ch].tr_bit_time=TOWs*300-240;
					chan[ch].tow_sync=1;
					chan[ch].TOW=TOWs*6;
					chan[ch].sfid=sfid_s;
					chan[ch].TLM=TLMs;
			  }
		   }
		 }
	  }
  }
//  a 1500 bit frame of data is ready to be read
  if ((chan[ch].t_count-chan[ch].offset)%1500==0) chan[ch].frame_ready=1;
}