gpsrcvr.cpp

开源GPS源码

  cur_long.sec=(rec_pos_llh.lon*r_to_d-cur_long.deg-cur_long.min/60.)*3600.;
  printf("   latitude    longitude          HAE      clock error (ppm)\n");
  printf("  %4d:%2d:%5.2f  %4d:%2d:%5.2f  %10.2f  %f\n",
  cur_lat.deg,abs(cur_lat.min),fabs(cur_lat.sec),cur_long.deg,abs(cur_long.min),
  fabs(cur_long.sec),rec_pos_llh.hae,clock_offset);
  printf(" Speed     Heading      TIC_dt\n");
  printf(" %lf   %lf   %lf\n",speed,heading*r_to_d,TIC_dt);
  printf("   \n");
  printf("tracking %2d status %1d almanac valid %1d gps week %4d alm_page %2d\n",
  n_track,status,almanac_valid,gps_week%1024,alm_page);
  if (display_page==0)
  {
	 printf(
" ch prn state n_freq az el doppler t_count n_frame sfid ura page missed CNo\n");
	 for (ch=0;ch<N_channels;ch++)
	 {
		printf(
" %2d %2d  %2d  %3d   %4.0f  %3.0f   %6.0f   %4d  %4d  %2d  %3d  %3d%5d   %4.1f\n",
		ch,chan[ch].prn,chan[ch].state,chan[ch].n_freq,
		xyz[chan[ch].prn].azimuth*57.3,xyz[chan[ch].prn].elevation*57.3,
		xyz[chan[ch].prn].doppler,chan[ch].frame_bit%1500,chan[ch].n_frame,chan[ch].sfid,
		gps_eph[chan[ch].prn].ura,chan[ch].page5,chan[ch].missed,chan[ch].CNo);
	 }
  printf(" GDOP=%6.3f  HDOP=%6.3f  VDOP=%6.3f  TDOP=%6.3f\n",gdop,hdop,vdop,tdop);  }
  else if (display_page==1)
  {
	 printf(  " ch prn state TLM      TOW  Health  Valid  TOW_sync offset\n");
	 for (ch=0;ch<N_channels;ch++)
	 {
		printf(" %2d %2d  %2d  %6ld   %6ld   %2d     %2d     %2d   %4d\n",
		ch, chan[ch].prn, chan[ch].state, chan[ch].TLM, chan[ch].TOW,
		gps_eph[chan[ch].prn].health,gps_eph[chan[ch].prn].valid,chan[ch].tow_sync,
		chan[ch].offset);
	 }
  }
  else if (display_page==2)
  {
	 printf(" ch prn state n_freq az  el        tropo        iono\n");
	 for (ch=0;ch<N_channels;ch++)
	 {
		printf(" %2d %2d  %2d  %3d   %4.0f  %3.0f   %10.4lf   %10.4lf\n",
		ch, chan[ch].prn, chan[ch].state, chan[ch].n_freq,
		xyz[chan[ch].prn].azimuth*57.3, xyz[chan[ch].prn].elevation*57.3,
		chan[ch].Tropo*c, chan[ch].Iono*c);
	 }
  }
  else if (display_page==3)
  {
	 printf(" ch prn state      Pseudorange     delta Pseudorange\n");
	 for (ch=0;ch<N_channels;ch++)
	 {
		printf(" %2d %2d  %2d  %20.10lf   %15.10lf\n",
		ch, chan[ch].prn, chan[ch].state, chan[ch].Pr, chan[ch].dPr);
	 }
  }
  else if (display_page==4)// can be used for debugging purposes
  {
	 printf(" ch prn state sfid page SF1  SF2  SF3  SF4  SF5\n");
	 for (ch=0;ch<N_channels;ch++)
	 {
		printf(" %2d %2d   %2d   %2d  %2d  %3x  %3x  %3x  %3x  %3x\n",
		ch, chan[ch].prn, chan[ch].state,chan[ch].sfid,chan[ch].page5,
      chan[ch].word_error[0],chan[ch].word_error[1],chan[ch].word_error[2],
      chan[ch].word_error[3],chan[ch].word_error[4]);
	 }
  }

}

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

PARAMETERS None.

PURPOSE
	This function replaces the current IRQ0 Interrupt service routine with
	our own custom function. The old vector is stored in a global variable
	and will be reinstalled at the end of program execution. IRQ0 is
	enabled by altering the interrupt mask stored by the 8259 interrupt
	handler.

*******************************************************************************/
#ifdef BCPP  
void Interrupt_Install()
{
  unsigned char     int_mask,i_high,i_low;
  i_high=interr_int>>8;
  i_low=interr_int&0xff;
  Old_Interrupt = getvect(8 + IRQLEVEL);
  disable();
  setvect(8 + IRQLEVEL,GPS_Interrupt);
  int_mask = inportb(0x21);     // get hardware interrupt mask
  int_mask = int_mask & ~(1 << IRQLEVEL);
  outportb(0x21,int_mask);      // send new mask to 8259
  enable();
// modify the timer to divide by interr_int
  outportb(0x43,0x34);
  outportb(0x40,i_low);
  outportb(0x40,i_high);
  outportb(0x20,0x20); // Clear PIC

}
#endif
#ifdef VCPP  // MS
void Interrupt_Install()
{
	unsigned char     int_mask, i_high, i_low;
	i_high = interr_int >> 8;
	i_low  = interr_int & 0xff;
	Old_Interrupt = _dos_getvect( 8 + IRQLEVEL);
	_disable();
	_dos_setvect( 8 + IRQLEVEL, GPS_Interrupt);
	int_mask = _inp(0x21);     // Get hardware interrupt mask

	int_mask = int_mask & ~(1 << IRQLEVEL);

	_outp( 0x21, int_mask );

	_enable();

	// Modify the timer to divide by interr_int
	_outp(0x43,0x34 );

	_outp( 0x40,i_low );

	_outp( 0x40,i_high );

	_outp( 0x20,0x20 ); // Clear the PIC

}

#endif

#ifdef DJGPP  // DJGPP compiler
void Interrupt_Install()
{
	unsigned char     i_high, i_low;
        int int_handler;
        
	i_high = interr_int >> 8;
	i_low  = interr_int & 0xff;

   GPS_Interrupt.pm_offset = int_handler;
   GPS_Interrupt.pm_selector = _go32_my_cs();
   _go32_dpmi_get_protected_mode_interrupt_vector(0x08, &Old_Interrupt);
   _go32_dpmi_allocate_iret_wrapper(&GPS_Interrupt);
   _go32_dpmi_set_protected_mode_interrupt_vector(0x08, &GPS_Interrupt);

	// Modify the timer to divide by interr_int
	outportb(0x43,0x34 );

	outportb( 0x40,i_low );

	outportb( 0x40,i_high );

}

#endif

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

PARAMETERS None.

PURPOSE
	This function removes the custom interrupt vector from the vector
	table and restores the previous vector.

*******************************************************************************/
#ifdef BCPP

void Interrupt_Remove()
{
  unsigned char     int_mask;

  outportb(0x20,0x20);           // clear interrupt and allow next one
  int_mask = inportb(0x21);      // get hardware interrupt mask
  int_mask = int_mask | (1 << IRQLEVEL);
  disable();
//outportb(0x21,int_mask);       // send new mask to 8259
  setvect(8 + IRQLEVEL,Old_Interrupt);
  enable();                      // allow hardware interrupts
  outportb(0x20,0x20);           // clear interrupt and allow next one
  outportb(0x43,0x34);           // reset clock
  outportb(0x40,0xff);
  outportb(0x40,0xff);
}
#endif
#ifdef VCPP
void Interrupt_Remove()
{
	unsigned char     int_mask;

	_outp( 0x20,0x20);
	// PGB outportb(0x20,0x20);           // clear interrupt and allow next one
	int_mask = inp(0x21);
	// PGB int_mask = inportb(0x21);      // get hardware interrupt mask
	int_mask = int_mask | (1 << IRQLEVEL);
	_disable();
	//outportb(0x21,int_mask);       // send new mask to 8259
	_dos_setvect(8 + IRQLEVEL,Old_Interrupt);
	_enable();                      // allow hardware interrupts
	_outp(0x20,0x20);           // clear interrupt and allow next one
	_outp(0x43,0x34);           // reset clock
	_outp(0x40,0xff);
	_outp(0x40,0xff);
}

#endif
#ifdef DJGPP
void Interrupt_Remove()
{

   _go32_dpmi_set_protected_mode_interrupt_vector(0x08, &Old_Interrupt);
   _go32_dpmi_free_iret_wrapper(&GPS_Interrupt);

   outportb(0x43,0x34);           // reset clock
   outportb(0x40,0xff);
   outportb(0x40,0xff);
}

#endif

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

PARAMETERS None.

PURPOSE
	This function allocates the channels with PRN numbers

WRITTEN BY
	Clifford Kelley

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

void  chan_allocate()
{
	 char ch,prnn,alloc;
    int i;
//	 almanac_valid=1;
	 for (prnn=1;prnn<=32;prnn++)
	 {
		xyz[prnn]=satfind(prnn);
		if (gps_alm[prnn].inc>0.0 && gps_alm[prnn].week!=gps_week%1024)
		{
		 almanac_valid=0;
       }
	 }
	 if (al0==0.0 && b0==0.0)almanac_valid=0;
	 for (ch=0;ch<N_channels;ch++) // if the sat has dropped below mask angle
				// turn the channel off
	 {
		if (xyz[chan[ch].prn].elevation < mask_angle ||
			  gps_alm[chan[ch].prn].ety == 0.0)
		{
			chan[ch].state=off;
         chan[ch].tow_sync=0;
			chan[ch].prn=chan[ch].offset=0;
         chan[ch].bit_counter=0;
         chan[ch].frame_bit=0;
         chan[ch].CNo=0.0;
         chan[ch].n_freq=chan[ch].t_count=chan[ch].n_frame=chan[ch].sfid=0;
         chan[ch].Pr=chan[ch].dPr=chan[ch].Tropo=chan[ch].Iono=0.0;
         chan[ch].TOW=chan[ch].TLM=0;
         for (i=0;i<5;i++)chan[ch].word_error[i]=0;
         for (i=0;i<1500;i++) chan[ch].message[i]=0;
		}
	 }
	 for (prnn=1;prnn<=32;prnn++)
	 {
		if (xyz[prnn].elevation > mask_angle && gps_alm[prnn].health==0 &&
			 gps_alm[prnn].ety != 0.00)
		{
		  alloc=0;
		  for (ch=0;ch<N_channels;ch++)
		  {
			 if (chan[ch].prn==prnn)
			 {
				 alloc=1;// satellite already allocated a channel
				 break;
			 }
	}
	if (alloc==0) // if not allocated find an empty channel
	{
		for (ch=0;ch<N_channels;ch++)
		{
		  reset_cntl(0x1fff);
		  if (chan[ch].state==off)
		  {
	// calculate carrier clock and doppler correction
			 chan[ch].carrier_corr=(-xyz[prnn].doppler-
										  clock_offset*1575.42)/42.57475e-3;
	// calculate code clock and doppler correction
			 code_corr=clock_offset*24.+xyz[prnn].doppler/65.5;
			 chan[ch].code_freq=code_ref+code_corr;
			 ch_code(ch,chan[ch].code_freq);          // 1.023 MHz chipping rate
			 ch_cntl(ch,prn_code[prnn]|0xa000);       // select satellite
			 chan[ch].prn=prnn;
//			 ch_on(ch);
			 chan[ch].state=acquisition;
			 chan[ch].codes=0;
          chan[ch].bit_counter=0;
          chan[ch].frame_bit=0;
  			 chan[ch].n_freq=search_min_f;
			 chan[ch].del_freq=1;
			 chan[ch].carrier_freq=carrier_ref+chan[ch].carrier_corr+
											 d_freq*chan[ch].n_freq;   // set carrier
			 ch_carrier(ch,chan[ch].carrier_freq);             // select carrier
			 break;
		  }
		}
	}
		}
	 }
}
/*******************************************************************************
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<N_channels;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<N_channels;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<N_channels;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;
       chan[ch].bit_counter=0;
       chan[ch].frame_bit=0;
  		 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,&ICP_CTL);
		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,&out_data);
		fscanf(in,"%s %d",intext,&m_tropo);
		fscanf(in,"%s %d",intext,&m_iono);
		fscanf(in,"%s %d",intext,&align_t);
		fscanf(in,"%s %u",intext,&Com0Baud); // NMEA
		fscanf(in,"%s %u",intext,&Com1Baud); // NMEA
		fscanf(in,"%s %u",intext,&GPGGA); // NMEA
		fscanf(in,"%s %u",intext,&GPGSV); // NMEA
		fscanf(in,"%s %u",intext,&GPGSA); // NMEA
		fscanf(in,"%s %u",intext,&GPVTG); // NMEA
		fscanf(in,"%s %u",intext,&GPRMC); // NMEA
		fscanf(in,"%s %u",intext,&GPZDA); // NMEA
		}
	 fclose(in);
}

void write_data_0(void)
{
   int i;
   if (abs(clock_tow-last_tow)>20)
   {
     fprintf(data_bits,"%6ld  ",clock_tow);
     for (i=0;i<12;i++) fprintf(data_bits,"%d,",chan[i].prn);
     fprintf(kalm," 0\n");
     for (i=0;i<1500;i++)
     {
       fprintf(data_bits,"%x\n",data_0[i]);
       data_0[i]=0;
     }
     last_tow=clock_tow;
   }
}

void write_data_1(void)
{
   int i;
   if (abs(clock_tow-last_tow)>20)
   {
     fprintf(data_bits,"%6ld  ",clock_tow);
     for (i=0;i<12;i++) fprintf(data_bits,"%d,",chan[i].prn);
     fprintf(kalm," 1\n");
     for (i=0;i<1500;i++)
     {
        fprintf(data_bits,"%x\n",data_1[i]);
        data_1[i]=0;
     }
     last_tow=clock_tow;
   }
}