gpstest.cpp

开源GPS源码

		 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;
  }
}
}

void ch_confirm(char ch)
{
  long prompt_mag,track_mag;
  prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
  track_mag  =rss(chan[ch].i_track,chan[ch].q_track);
  if ((prompt_mag > acq_thresh) || (track_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;
	 }
	 else chan[ch].state=acquisition;
  }
  chan[ch].i_confirm++;
}

void ch_pull_in(char ch)
{
  long ddf,ddcar,theta_e,wdot_gain;
  long q_sum,i_sum,theta,theta_dot;
  long prompt_mag,track_mag;
  if ( chan[ch].ch_time <= pull_in_time-1 )
  {
	 prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
	 track_mag  =rss(chan[ch].i_track,chan[ch].q_track);
	 chan[ch].sum+=prompt_mag+track_mag;
// code tracking loop
	 if ( prompt_mag != 0 || track_mag != 0)
	 {
		chan[ch].dfreq=((prompt_mag-track_mag)<<14)*pull_code_k/(prompt_mag+track_mag);
		ddf  =(chan[ch].dfreq-chan[ch].dfreq1)*pull_code_d;
		if ( chan[ch].ch_time > 2 )
		{
	 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_track+chan[ch].q_prompt;
	 i_sum=chan[ch].i_track+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++;
	 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 )
	{
	  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 (chan[ch].ch_time==pull_in_time)
  {
	 chan[ch].ms_count=chan[ch].ms_count%20;
	 chan[ch].avg=chan[ch].sum/pull_in_time/2;
	 chan[ch].th_rms=fix_sqrt(chan[ch].th_rms/phase_test);
	 if ( chan[ch].avg>14*rms/10 && chan[ch].th_rms<9830)
	 {
	 chan[ch].avg=chan[ch].avg*20;
	 chan[ch].state=track;
	 chan[ch].t_count=0;
	 chan[ch].sum=0;
	 chan[ch].q_track_20=0;
	 chan[ch].q_prompt_20=0;
	 chan[ch].i_track_20=0;
	 chan[ch].i_prompt_20=0;
	 }
	 else
	 {
		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
	 }
  }
}

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;
  if (chan[ch].ms_count<=19)
  {
	 chan[ch].q_track_20+=chan[ch].q_track;
	 chan[ch].q_prompt_20+=chan[ch].q_prompt;
	 chan[ch].i_track_20+=chan[ch].i_track;
	 chan[ch].i_prompt_20+=chan[ch].i_prompt;
//  now the carrier loop
	 q_sum=chan[ch].q_track+chan[ch].q_prompt;
	 i_sum=chan[ch].i_track+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 here
	 ddcar=(chan[ch].dcarr-chan[ch].dcarr1)*trk_carr_d;
	 chan[ch].carrier_freq=((chan[ch].dcarr+ddcar)>>14)+chan[ch].carrier_freq;
	 ch_carrier(ch,chan[ch].carrier_freq);
	 if ((a_missed & test[ch])==0 && chan[ch].ms_count<10 &&
	 sign(q_sum)==-sign(chan[ch].old_q_sum) &&
	 labs(chan[ch].dcarr1)<-4096*trk_carr_k &&
	 labs( chan[ch].dcarr)<-4096*trk_carr_k)
	 {
		 chan[ch].ms_count=0;
		 chan[ch].q_track_20=chan[ch].q_track;
		 chan[ch].q_prompt_20=chan[ch].q_prompt;
		 chan[ch].i_track_20=chan[ch].i_track;
		 chan[ch].i_prompt_20=chan[ch].i_prompt;
	 }
	 if ((a_missed & test[ch])==0 && chan[ch].ms_count>=10 &&
	 sign(q_sum)==-sign(chan[ch].old_q_sum) &&
	 labs(chan[ch].dcarr1)<-4096*trk_carr_k &&
	 labs( chan[ch].dcarr)<-4096*trk_carr_k)
	 {
		 chan[ch].ms_count=20;
	 }
	 }
	 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_prompt_20,chan[ch].q_prompt_20);
	 chan[ch].track_mag  =rss(chan[ch].i_track_20,chan[ch].q_track_20);
	 chan[ch].sum+=chan[ch].prompt_mag+chan[ch].track_mag;
// code tracking loop
	 if ( chan[ch].prompt_mag != 0 || chan[ch].track_mag != 0)
	 {
		chan[ch].dfreq=trk_code_k*(chan[ch].prompt_mag-chan[ch].track_mag);
		ddf  =(chan[ch].dfreq-chan[ch].dfreq1)*trk_code_d;
		chan[ch].code_freq =(chan[ch].dfreq+ddf)/trk_div+chan[ch].code_freq;     // here was >>12
		ch_code(ch,chan[ch].code_freq);
	 }
	 chan[ch].dfreq1=chan[ch].dfreq;
//	 chan[ch].bit=bsign(chan[ch].q_prompt_20+chan[ch].q_track_20);
//	 chan[ch].message[chan[ch].t_count]=chan[ch].bit;
	 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_track_20=0;
	 chan[ch].q_prompt_20=0;
	 chan[ch].i_track_20=0;
	 chan[ch].i_prompt_20=0;
  }
  if (chan[ch].t_count==1500)
  {
	 chan[ch].n_frame++;
	 chan[ch].t_count=0;
  }
}

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

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);
}

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

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

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



/*******************************************************************************
FUNCTION rss(long a, long b)
RETURNS  long integer

PARAMETERS
		a  long integer
		b  long integer

PURPOSE
	This function finds the fixed point magnitude of a 2 dimensional vector

WRITTEN BY
	Clifford Kelley

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

inline long rss(long a,long b )
{
   long result,c,d;
   c=labs(a);
   d=labs(b);
   if (c==0 && d==0) result=0;
   else
   {
       if (c>d) result=(d>>1)+c;
       else     result=(c>>1)+d;
   }
   return (result);
}

/*******************************************************************************
FUNCTION fix_sqrt(long x)
RETURNS  long integer

PARAMETERS
      x long integer

PURPOSE
	This function finds the fixed point square root of a long integer

WRITTEN BY
	Clifford Kelley

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

long fix_sqrt(long x)
{
  long xt,scr;
  int i;
  i=0;
  xt=x;
  do
  {
     xt=xt>>1;
     i++;
  } while (xt>0);
  i=(i>>1)+1;
  xt=x>>i;
  do
  {
     scr=xt*xt;
     scr=x-scr;
     scr=scr>>1;
     scr=scr/xt;
     xt=scr+xt;
  } while (scr!=0);
  xt=xt<<7;
  return(xt);
}

/*******************************************************************************
FUNCTION fix_atan2(long y,long x)
RETURNS  long integer

PARAMETERS 
		x  long   in-phase fixed point value
		y  long   quadrature fixed point value

PURPOSE
      This function computes the fixed point arctangent represented by
      x and y in the parameter list
      1 radian = 16384
      based on the power series  f-f^3*2/9

WRITTEN BY
	Clifford Kelley
	Fixed for y==x  added special code for x==0 suggested by Joel Barnes, UNSW
*******************************************************************************/

#define SCALED_PI_ON_2  25736L
#define SCALED_PI       51472L

inline long fix_atan2(long y,long x)
{
  long result,n,n3;
  if ((x==0) && (y==0))
	  return(0); // invalid case

  if (x>0 &&  x>=labs(y))
  {
		 n=(y<<14)/x;
		 n3=((((n*n)>>14)*n)>>13)/9;
		 result=n-n3;
  }
  else if (x<=0 && -x>=labs(y))
  {
		 n=(y<<14)/x;
		 n3=((((n*n)>>14)*n)>>13)/9;
		 if      ( y>0) result=n-n3+SCALED_PI;
		 else if (y<=0) result=n-n3-SCALED_PI;
  }
  else if (y>0 &&  y>labs(x))
  {
		 n=(x<<14)/y;
		 n3=((((n*n)>>14)*n)>>13)/9;
		 result=SCALED_PI_ON_2-n+n3;
  }
  else if (y<0 && -y>labs(x))
  {
		 n=(x<<14)/y;
		 n3=((((n*n)>>14)*n)>>13)/9;
		 result=-n+n3-SCALED_PI_ON_2;
  }
  return(result);
}