gpsisr.cpp

OpenSource GPS is software for x86 PCs that allows you to acquire, track and demodulate signals from

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

/*    if (ch==0)
    {
      qdither0[ms_count]=chan[ch].q_dith;
	   qprompt0[ms_count]=chan[ch].q_prompt;
	   idither0[ms_count]=chan[ch].i_dith;
	   iprompt0[ms_count]=chan[ch].i_prompt;
      ms_count++;
      if (ms_count==30000) ms_count=0;
    } 
*/
//  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;
    chan[ch].frame_bit=chan[ch].bit_counter-chan[ch].offset;
    
    if (chan[ch].frame_bit-ext_bit_count>1400 && chan[ch].frame_bit-ext_bit_count<1600)
    {
       chan[ch].bit_counter-=1500;
       chan[ch].bit_counter=(chan[ch].bit_counter+3000)%3000;
    }

    if (ext_bit_count-chan[ch].frame_bit>1400 && ext_bit_count-chan[ch].frame_bit<1600)
    {
       chan[ch].bit_counter+=1500;
       chan[ch].bit_counter=chan[ch].bit_counter%3000;
    }

    if (chan[ch].frame_bit<0) chan[ch].frame_bit+=3000;
    if (chan[ch].frame_bit<1500) data_0[chan[ch].frame_bit]+=test[ch]*chan[ch].bit;
    else data_1[chan[ch].frame_bit-1500]+=test[ch]*chan[ch].bit;
    chan[ch].bit_counter++;
    if (chan[ch].bit_counter>=3000) chan[ch].bit_counter=0;

/*    if (ch<6)
    {
       qdither[ch][chan[ch].t_count]=chan[ch].q_dith_20;
	    qprompt[ch][chan[ch].t_count]=chan[ch].q_prom_20;
	    idither[ch][chan[ch].t_count]=chan[ch].i_dith_20;
	    iprompt[ch][chan[ch].t_count]=chan[ch].i_prom_20;
    }
*/
	 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
			  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 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;
}