gpsrcvr.cpp

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

	 int astat,mstat;
	 unsigned int add;
	 char ch;
	 to_gps(0x80,0);             // tell 2021 to latch the correllators
	 a_missed=from_gps(0x83);    // did we miss any corellation data
	 astat=from_gps(0x82);       // get info on what channels have data ready
	 for (ch=0;ch<=chmax;ch++)
	 {
		if (astat & test[ch])
		{
	  add=0x84+(ch<<2);
	  chan[ch].i_dith=from_gps(add);    // inphase dither
	  add++;
	  chan[ch].q_dith=from_gps(add);    // quadrature dither
	  add++;
	  chan[ch].i_prompt=from_gps(add);  // inphase prompt
	  add++;
	  chan[ch].q_prompt=from_gps(add);  // quadrature prompt
	  ch_accum_reset(ch);
	  if (a_missed & test[ch])
	  {
		chan[ch].missed++;
		ch_accum_reset(ch);
		}
	  }
	 }
	 for (ch=0;ch<=chmax;ch++)
	 {
		if (astat & test[ch])
		{
	switch(chan[ch].state)
	{
	  case acquisition:
		 ch_acq(ch);
		 break;
	  case confirm    :
		 ch_confirm(ch);
		 break;
	  case pull_in    :
		 ch_pull_in(ch);
		 break;
	  case track      :
		 ch_track(ch);
		 break;
	}
		}
	 }
	 mstat=a_missed & 0x2000;             // has a tic occured?
	 if (mstat)
	 {
		tic_count=(++tic_count)%10;
		if (tic_count==0) sec_flag=1;      // one second has passed
		hms_count=(++hms_count)%600;
		if (hms_count==0) min_flag=1;      // one minute has passed
		nav_count=(++nav_count)%nav_tic;
		if (nav_count==0) nav_flag=1;
		TIC_sum+=TIC_cntr+1;
		add=1;
		for (ch=0;ch<=chmax;ch++)
	  {
		 add++;
		 chan[ch].carr_cycle_l=from_gps(add);    // for computing
		 add+=4;                                 // delta-pseudorange
		 chan[ch].carr_cycle_h=from_gps(add);
		 add+=3;
		 chan[ch].cycle_sum+=chan[ch].carr_cycle_l+65536L*chan[ch].carr_cycle_h;
		}

	 if (nav_count==0)                    // time to get measurement data
	 {
		add=1;
		for (ch=0;ch<=chmax;ch++)
	  {
		 chan[ch].code_phase=from_gps(add);      // get carrier
		 add++;                                  // and code data
															  // for computing
		 add++;                                  // pseudorange and
		 chan[ch].carr_dco_phase=from_gps(add);  // delta-pseudorange
		 add++;
		 chan[ch].epoch=from_gps(add);
		 add++;
		 chan[ch].code_dco_phase=from_gps(add);
		 add++;
		 add++;
		 add++;
		 add++;
		 chan[ch].meas_bit_time=chan[ch].tr_bit_time;
		 chan[ch].doppler=chan[ch].carrier_freq;
		 chan[ch].carrier_counter=chan[ch].cycle_sum;
		 chan[ch].d_carr_phase=chan[ch].carr_dco_phase - chan[ch].old_carr_dco_phase;
		 chan[ch].old_carr_dco_phase=chan[ch].carr_dco_phase;
		 chan[ch].cycle_sum=0;
		 }
		 i_TIC_dt= TIC_sum+old_TIC_cntr-TIC_cntr;
		 TIC_sum=0;
		 }
	 }
// reset the interrupt
	 outportb(0x20,0x20);
}

/*******************************************************************************
FUNCTION ch_acq(char ch)
RETURNS  None.

PARAMETERS
			ch  char

PURPOSE  to perform initial acquisition by searching code and frequency space
			looking for a high correllation

WRITTEN BY
	Clifford Kelley

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

void ch_acq(char ch)
{
  long prompt_mag,dith_mag;
  if (abs(chan[ch].n_freq)<=search_max_f)//  search frequencies
  {
	 prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
	 dith_mag  =rss(chan[ch].i_dith,chan[ch].q_dith);
	 if ((dith_mag > acq_thresh) && (prompt_mag > acq_thresh))
	 {
		 ch_code_slew(ch,2044);          // slew back 1 chip so we can
		 chan[ch].state=confirm;         // confirm the signal
		 chan[ch].i_confirm=0;
		 chan[ch].n_thresh=0;
	 }
	 else
	 {
		ch_code_slew(ch,2);
		chan[ch].codes+=2;
	 }
	 if (chan[ch].codes==2044)
	 {
		chan[ch].n_freq+=chan[ch].del_freq;
		chan[ch].del_freq=-(chan[ch].del_freq+sign(chan[ch].del_freq));
		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
		chan[ch].codes=0;
	 }
  }
  else
  {
		 chan[ch].n_freq=search_min_f;                    // keep looping
		 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
		 chan[ch].codes=0;
  }
}

/*******************************************************************************
FUNCTION ch_confirm(char ch)
RETURNS  None.

PARAMETERS
			ch  char  channel number

PURPOSE  to confirm the presence of a high correllation peak using an n of m
			algorithm

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
void ch_confirm(char ch)
{
  long prompt_mag,dith_mag;
  prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
  dith_mag  =rss(chan[ch].i_dith,chan[ch].q_dith);
  if ((prompt_mag > acq_thresh) || (dith_mag > acq_thresh)) chan[ch].n_thresh++;
  if (chan[ch].i_confirm==confirm_m)
  {
	 if (chan[ch].n_thresh >= n_of_m_thresh)
	 {
		chan[ch].state=pull_in;
		chan[ch].ch_time=0;
		chan[ch].sum=0;
		chan[ch].th_rms=0;
		chan[ch].ms_set=0;
	 }
	 else chan[ch].state=acquisition;
  }
  chan[ch].i_confirm++;
}

/*******************************************************************************
FUNCTION ch_pull_in(char ch)
RETURNS  None.

PARAMETERS
			ch  char  channel number

PURPOSE
			pull in the frequency by trying to track the signal with a
			combination FLL and PLL
			it will attempt to track for xxx ms, the last xxx ms of data will be
			gathered to determine if we have both code and carrier lock
			if so we will transition to track

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
void ch_pull_in(char ch)
{
  long ddf,ddcar,theta_e,wdot_gain;
  long q_sum,i_sum,theta,theta_dot;
  long prompt_mag,dith_mag;
	 prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
	 dith_mag  =rss(chan[ch].i_dith,chan[ch].q_dith);
	 chan[ch].sum+=prompt_mag+dith_mag;
// code tracking loop
	 if ( prompt_mag != 0 || dith_mag != 0)
	 {
		chan[ch].dfreq=((prompt_mag-dith_mag)<<14)/(prompt_mag+dith_mag)*pull_code_k;
		ddf  =(chan[ch].dfreq-chan[ch].dfreq1)*pull_code_d;
		if ( chan[ch].ch_time > 2 )           // don't close the loop for 2 ms
		{                                     // to avoid transients
			chan[ch].code_freq =((chan[ch].dfreq+ddf)>>14)+chan[ch].code_freq;
			ch_code(ch,chan[ch].code_freq);
		}
	 }
	 chan[ch].dfreq1=chan[ch].dfreq;
	 q_sum=chan[ch].q_dith+chan[ch].q_prompt;
	 i_sum=chan[ch].i_dith+chan[ch].i_prompt;
	 if (i_sum !=0 || q_sum !=0) theta=fix_atan2(q_sum,-i_sum);
	 else theta=chan[ch].old_theta;
	 theta_dot=theta-chan[ch].old_theta;
	 chan[ch].ms_count++;
	 if ( sign(q_sum)==-sign(chan[ch].old_q_sum) && (a_missed & test[ch])==0
		&& labs(labs(theta)-25736)<4096
		&& labs(labs(chan[ch].old_theta)-25736)<4096)
	 {
		chan[ch].ms_count=0;
		ch_epoch_load(ch,0x1);
		chan[ch].ms_set=1;
	 }
	 chan[ch].old_theta=theta;
	 if      ( theta> 0 ) theta_e=theta-25736;
	 else if ( theta<= 0) theta_e=theta+25736;
	 if (chan[ch].ch_time>pull_in_time-phase_test) chan[ch].th_rms+=
																  (theta_e*theta_e)>>14;
	 if ( labs(theta_dot) < 32768L )
	 {
		if (q_sum != 0 || i_sum !=0)
		{
	wdot_gain=chan[ch].ch_time/499;
	wdot_gain*=wdot_gain;
	wdot_gain*=wdot_gain;
	chan[ch].dcarr=pull_carr_k*(theta_dot*5/(1+wdot_gain)+theta_e);
	ddcar=(chan[ch].dcarr-chan[ch].dcarr1)*pull_carr_d;
	if ( chan[ch].ch_time > 5 )        // don't close the loop for
	{                                  // 5 ms to avoid transients
	  chan[ch].carrier_freq=((chan[ch].dcarr+ddcar)>>14)+chan[ch].carrier_freq;
	  ch_carrier(ch,chan[ch].carrier_freq);
	}
		}
	 }
	 chan[ch].dcarr1=chan[ch].dcarr;
	 chan[ch].old_q_sum=q_sum;
	 chan[ch].ch_time++;
//	 if ( ch==0 && chan[ch].ch_time<1500)
//	 {
//		i_prompta[chan[ch].ch_time]=chan[ch].i_prompt;
//		q_prompta[chan[ch].ch_time]=chan[ch].q_prompt;
//		i_dithera[chan[ch].ch_time]=chan[ch].i_dith;
//		q_dithera[chan[ch].ch_time]=chan[ch].q_dith;
//		car_freq[chan[ch].ch_time]=chan[ch].carrier_freq;
//		chip_freq[chan[ch].ch_time]=chan[ch].ms_count;
//	 }
	 chan[ch].ms_count=chan[ch].ms_count%20;
	 if (chan[ch].ch_time>=pull_in_time && chan[ch].ms_count==19)   // done with pull-in
	 {                                                              // wait until end of
		chan[ch].avg=chan[ch].sum/pull_in_time/2;                    // a data bit
		chan[ch].th_rms=fix_sqrt(chan[ch].th_rms/phase_test);
		if ( chan[ch].avg>14*rms/10 && chan[ch].th_rms<12000 && chan[ch].ms_set)
		{                                         // go to track
		  chan[ch].avg=chan[ch].avg*20;
		  chan[ch].state=track;
		  chan[ch].t_count=0;
		  chan[ch].sum=0;
		  chan[ch].q_dith_20=0;
		  chan[ch].q_prom_20=0;
		  chan[ch].i_dith_20=0;
		  chan[ch].i_prom_20=0;
		  if (ch==0)
		  {
			 debug_counter=0;
			 store_code=chan[ch].code_freq;
			 store_carrier=chan[ch].carrier_freq;
		  }
//	 chan[ch].dfreq=trk_code_d*4096*(chan[ch].code_freq-code_ref-
//	 (carrier_ref-chan[ch].carrier_freq)/cc_scale);
		  chan[ch].dfreq=0;
		}
		else                                      // else go back to acquisition
		{
		  chan[ch].state=acquisition;
		  chan[ch].codes=0;
		  chan[ch].code_freq=code_ref+code_corr;
		  ch_code(ch,chan[ch].code_freq);        // 1.023 MHz chipping rate
		}
	 }
}

/*******************************************************************************
FUNCTION ch_track(char ch)
RETURNS  None.

PARAMETERS
			ch  char  channel number

PURPOSE  to track in carrier and code the GPS satellite and partially
			decode the navigation message (to determine TOW, subframe etc.)

WRITTEN BY
	Clifford Kelley
added Carrier Aiding as suggested by Jenna Cheng, UCR
*******************************************************************************/
void ch_track(char ch)
{
  long ddf,ddcar,q_sum,i_sum;
//
// 50 Hz tracking loop
//
  chan[ch].ms_count=(++chan[ch].ms_count)%20;
	 chan[ch].q_dith_20+=chan[ch].q_dith;
	 chan[ch].q_prom_20+=chan[ch].q_prompt;
	 chan[ch].i_dith_20+=chan[ch].i_dith;
	 chan[ch].i_prom_20+=chan[ch].i_prompt;
//  now the carrier loop
	 q_sum=chan[ch].q_dith+chan[ch].q_prompt;
	 i_sum=chan[ch].i_dith+chan[ch].i_prompt;
	 if ( q_sum != 0 || i_sum != 0)
	 {
	 chan[ch].dcarr=(i_sum<<14)*trk_carr_k*sign(q_sum)/rss(q_sum,i_sum);//check
	 ddcar=(chan[ch].dcarr-chan[ch].dcarr1)*trk_carr_d;                 //here
	 chan[ch].carrier_freq=((chan[ch].dcarr+ddcar)>>14)+chan[ch].carrier_freq;
	 ch_carrier(ch,chan[ch].carrier_freq);
	 }
	 chan[ch].old_q_sum=q_sum;
	 chan[ch].dcarr1=chan[ch].dcarr;
//  }
  if (chan[ch].ms_count==19)
  {
	 chan[ch].tr_bit_time++;
	 chan[ch].prompt_mag=rss(chan[ch].i_prom_20,chan[ch].q_prom_20);
	 chan[ch].dith_mag  =rss(chan[ch].i_dith_20,chan[ch].q_dith_20);
	 chan[ch].sum+=chan[ch].prompt_mag+chan[ch].dith_mag;
// code tracking loop
	 if ( chan[ch].prompt_mag != 0 || chan[ch].dith_mag != 0)
	 {

// without carrier aiding
/*		chan[ch].dfreq=(chan[ch].prompt_mag-chan[ch].dith_mag)*trk_code_k;
		ddf=(chan[ch].dfreq-chan[ch].dfreq1)*trk_code_d;
		chan[ch].code_freq+=(chan[ch].dfreq+ddf)/trk_div;
*/
// with carrier aiding
		ddf=((chan[ch].prompt_mag-chan[ch].dith_mag)*2048)/
		(chan[ch].prompt_mag+chan[ch].dith_mag)*trk_code_k;
		chan[ch].dfreq+=ddf;
		chan[ch].code_freq =(chan[ch].dfreq/trk_code_d+ddf)/256+
		(carrier_ref-chan[ch].carrier_freq)/cc_scale +code_ref;   // here was >>12
		ch_code(ch,chan[ch].code_freq);
	 }
	 chan[ch].dfreq1=chan[ch].dfreq;

	 chan[ch].bit=bsign(chan[ch].q_prom_20+chan[ch].q_dith_20);//bsign is data bit
	 pream(ch,chan[ch].bit);   // see if we can find the preamble
	 chan[ch].message[chan[ch].t_count]=chan[ch].bit;
//	 if ( ch==0 && debug_counter<4500)
//	 {
//		i_prompta[debug_counter]=ddf;
//		q_prompta[debug_counter]=chan[ch].dfreq;
//		i_dithera[debug_counter]=chan[ch].prompt_mag;
//		q_dithera[debug_counter]=chan[ch].dith_mag;
//		car_freq[debug_counter]=chan[ch].carrier_freq;
//		chip_freq[debug_counter]=chan[ch].code_freq;
//		data_bit[debug_counter]=chan[ch].bit;
//		debug_counter++;
//	 }
	 chan[ch].t_count++;
	 if (chan[ch].t_count%5==0)
	 {
		 chan[ch].avg=chan[ch].sum/10;
		 chan[ch].sum=0;
	 }
	 chan[ch].q_dith_20=0;
	 chan[ch].q_prom_20=0;
	 chan[ch].i_dith_20=0;
	 chan[ch].i_prom_20=0;
  }
  if (chan[ch].t_count==1500)
  {
	 chan[ch].n_frame++;
	 chan[ch].t_count=0;
  }
}

/*******************************************************************************
FUNCTION xors(long pattern)
RETURNS  Integer

PARAMETERS