gpsisr.cpp

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

#include  <stdio.h>
#include  <stdlib.h>
#include  <conio.h>
#include  <math.h>
#include  <string.h>
#include  <io.h>
#include  <Dos.h>
#include  <time.h>
#include  "structs.h"
#include  "consts.h"
#undef MAIN
#include  "globals.h"
#include  "gpsfuncs.h"
#include  "gp2021.h"

inline int bit_test(int,char);
void pream(char ,char );
void ch_acq(char);
void ch_confirm(char);
void ch_pull_in(char);
void ch_track(char);
inline int sign(long);
inline int bsign(long);
int xors(long);

/*******************************************************************************
FUNCTION GPS_Interrupt()

RETURNS  None.

PARAMETERS None.

PURPOSE
	This function replaces the current IRQ0 Interrupt service routine with
	our GPS function which will perform the acquisition - tracking functions

WRITTEN BY
	Clifford Kelley

*******************************************************************************/ 
#ifdef VCPP  

void __interrupt _far GPS_Interrupt(void) // MS  

#endif


#ifdef BCPP

void interrupt GPS_Interrupt(...)

#endif

#ifdef DJGPP

void GPS_Interrupt(void)

#endif
{
//	 int astat,mstat;
	 unsigned int add;
	 char ch;
#ifdef DJGPP

asm("cli; pusha");

#endif
	 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<N_channels;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<N_channels;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<N_channels;ch++)
	  {
		 add++;
		 chan[ch].carr_cycle_l=from_gps(add);    // get carrier data for
         add++;                                  // computing
		 chan[ch].carr_dco_phase=from_gps(add);  // delta-pseudorange
         add+=3;
		 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 set up measurement data
	   {
		  add=1;
		  for (ch=0;ch<N_channels;ch++)
	     {
		     chan[ch].code_phase=from_gps(add); // get code data for computing
		     add+=3;                            // pseudorange
		     chan[ch].epoch=from_gps(add);
		     add++;
		     chan[ch].code_dco_phase=from_gps(add);
		     add+=4;
		     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;
		   }
		   i_TIC_dt= TIC_sum+old_TIC_cntr-TIC_cntr;
		   TIC_sum=0;
		 }
		 for (ch=0;ch<N_channels;ch++)
	    {
          chan[ch].old_carr_dco_phase=chan[ch].carr_dco_phase;
       }
	 }
// reset the interrupt
#ifdef VCPP
	 _outp( 0x20,0x20 ); // MS
#endif

#ifdef BCPP
	 outportb(0x20,0x20);
#endif

#ifdef DJGPP
    outportb(0x20, 0x20);
    asm("popa; sti");
#endif
}

/*******************************************************************************
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)==-1 && chan[ch].ms_sign==0x00000) || // check if last 20ms
        (sign(q_sum)== 1 && chan[ch].ms_sign==0xfffff))   // have the same sign
    {
	    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) // We have detected the
       {                                               // start if a data bit
		   chan[ch].ms_count=0;                          // set up counters to keep
		   ch_epoch_load(ch,0x1);                        // track of them
		   chan[ch].ms_set=1;                            // data bit set
	    }
    }
    chan[ch].ms_sign=chan[ch].ms_sign<<1;              // shift sign left
    if (q_sum < 0) chan[ch].ms_sign=chan[ch].ms_sign | 0x1; // set bit to 1 if negative
	 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++;
	 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)
//		  {
//			 ms_count=0;
//		  }
      chan[ch].bit_counter=0;