gpstest.cpp

开源GPS源码

struct channel
{
  int  state;
  long code_freq,carrier_freq,doppler,carrier_corr;
  char message[1500],sat_use,prn,bit,frame_ready;
  int  offset,codes,n_freq,del_freq,snr;
  int  bit_no,t_count,ms_count,i_confirm;
  int  ms_epoch,n_frame,ch_time,i_count;
  int  con_thresh,n_thresh,sfid,missed,page5;
  int  i_track,q_track,i_prompt,q_prompt;
  long sum,avg,old_theta,old_q_sum,th_rms;
  long dfreq,dfreq1,dcarr1,dcarr;
  long q_track_20,q_prompt_20,i_track_20,i_prompt_20;
  long prompt_mag,track_mag;
  long tr_bit_time,meas_bit_time,TOW;
  unsigned int epoch_ms,epoch_bit,epoch_code_phase,epoch_code_DCO_phase;
  unsigned int epoch_carrier_DCO_phase;
  long epoch_carrier_cycle;
  float CNo;
};

enum {off,acquisition,confirm,pull_in,track};
//     0       1         2       3      4
channel chan[12];
long carrier_ref=0x1f7b1b9L,code_ref=0x016ea4a8L;

int a_missed,test_cnt;
int prn_code[37]={0,0x3f6,0x3ec,0x3d8,0x3b0,0x04b,0x096,0x2cb,0x196,0x32c,
		    0x3ba,0x374,0x1d0,0x3a0,0x340,0x280,0x100,0x113,0x226,
		    0x04c,0x098,0x130,0x260,0x267,0x338,0x270,0x0e0,0x1c0,
			 0x380,0x22b,0x056,0x0ac,0x158,0x058,0x18b,0x316,0x058};
//int GIC[8]={0x2c4,0x10a,0x3e3,0x0f8,0x25f,0x1e7,0x2b5,0x10e};
unsigned test[16]=
 {0x0001,0x0002,0x0004,0x0008,0x0010,0x0020,0x0040,0x0080,
  0x0100,0x0200,0x0400,0x0800,0x1000,0x2000,0x4000,0x8000};
float clock_offset=0.0;

//int iii,out_debug;

#include "gp2021.cpp"
//#include "gpsfuncs.cpp"
inline long rss(long a,long b );
inline long fix_atan2(long y,long x);
long fix_sqrt(long x);

#define IRQLEVEL        0       // IRQ Line

void interrupt (*Old_Interrupt)(...); // Old IRQ0 interrupt handler
void interrupt  GPS_Interrupt(...);   // New IRQ0 interrupt handler

void Interrupt_Install(void);
void Interrupt_Remove(void);
void display(void);
long near_int(double);
void test_allocate(void);

//inline int bit_test(int,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);


int  nav_tic,search_max_f,search_min_f=0,cold_prn=1;
long rms=312,acq_thresh=575,code_corr,time_on=0;
long pull_code_k=111,pull_code_d=7,pull_carr_k=-12,pull_carr_d=28;
long trk_code_k=55, trk_code_d=3, trk_carr_k=-9, trk_carr_d=21;
double speed,heading,nav_up=1.0;
int  pull_in_time=1500,phase_test=500;
long  d_freq=2349,d_tow,trk_div=19643;
int   confirm_m=5,n_of_m_thresh=5,key,tic_count=0,hms_count=0;
int   nav_count,min_flag,nav_flag,sec_flag,n_track,test_type,counter=0;
unsigned int interr_int=512;

void main()
{
//  int   i,j;//,indatam,imask;
  char  ch;
  self_test();
  io_config(0x301);
  test_cnt=0x10;
  test_control(test_cnt);  // set up ch 0 as a test pattern generator
  system_setup(0x00);
  reset_cntl(0x1fff);             // release reset
  ch_status=0x1;
  nav_tic=nav_up*10;
  interr_int=512;
  code_corr=0.0;
  for (ch=0;ch<=11;ch++) chan[ch].state=off;
  clrscr();
  test_allocate();
  Interrupt_Install();
//
  do
  {
	 if (kbhit()) key = getch();
	 else         key = '\0';
	 if (sec_flag==1)
	 {
//        key='x';
	  sec_flag=0;
	  for (ch=0;ch<=11;ch++)
	  {
		  if (chan[ch].state==track)
		  {
			  chan[ch].CNo=10.*log10(chan[ch].avg/1395.*chan[ch].avg/1395.*25.*1.7777+1.0);
			  if (chan[ch].CNo<25.0)
			  {
			 chan[ch].carrier_corr=0.0;
	// calculate code correction
			 code_corr=0.0;
			 chan[ch].code_freq=code_ref+code_corr;
			 ch_code(ch,chan[ch].code_freq);               // 1.023 MHz chipping rate
			 chan[ch].state=acquisition;
			 chan[ch].t_count=0;
			 chan[ch].n_frame=0;
			 chan[ch].codes=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
			  }
			}
		}

	 }
	 display();
  } while (key != 'x' && key != 'X'); /* Stay in loop until 'X' key is pressed. */
  Interrupt_Remove();

}


void display(void)
{
  char ch;
  gotoxy(1,1);
  printf("   \n");
  printf(  " ch prn state code carr bit  ms cd_ph cd_dco car_dco carcycle\n");
  for (ch=0;ch<=11;ch++)
  {
	 printf(" %2d %2d %2d %6lx %6lx %4d %4d %4d %5d %5d %6ld\n",
	 ch,chan[ch].prn,chan[ch].state,chan[ch].code_freq,
	 chan[ch].carrier_freq,chan[ch].epoch_bit,
	 chan[ch].epoch_ms,chan[ch].epoch_code_phase,
	 chan[ch].epoch_code_DCO_phase,chan[ch].epoch_carrier_DCO_phase,
	 chan[ch].epoch_carrier_cycle);
  }
}

/*
******************************************************************************
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.

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

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

}

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

PARAMETERS None.

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

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

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


void  test_allocate()
{
	 char ch;
	 search_max_f=20;
	 reset_cntl(0x1fff); // turn all the channels on
	 for (ch=0;ch<=11;ch++)
	 {
		chan[ch].code_freq=code_ref;
		ch_code(ch,chan[ch].code_freq);        // 1.023 MHz chipping rate
		ch_cntl(ch,prn_code[1]|0xa000);       // select satellite track late
		chan[ch].prn=1;
		chan[ch].state=acquisition;
		chan[ch].codes=0;
		chan[ch].n_freq=search_min_f;
		chan[ch].del_freq=1;
		chan[ch].carrier_freq=carrier_ref;    // set carrier
		if (ch>0) chan[ch].carrier_freq+=15000;
		ch_carrier(ch,chan[ch].carrier_freq); // select carrier
	 }
}

void interrupt GPS_Interrupt(...)
{
	 int astat,mstat,itemp;
	 unsigned int epoch,add;
	 char ch;
	 to_gps(0x80,0);
	 a_missed=from_gps(0x83);
	 astat=from_gps(0x82);
	 for (ch=0;ch<=11;ch++)
	 {
		if (astat & test[ch])
		{
	  add=(ch<<2)+0x84;
	  itemp=from_gps(add);
	  chan[ch].i_track=itemp;
	  add++;
	  itemp=from_gps(add);
	  chan[ch].q_track=itemp;
	  add++;
	  itemp=from_gps(add);
	  chan[ch].i_prompt=itemp;
	  add++;
	  itemp=from_gps(add);
	  chan[ch].q_prompt=itemp;
	  if (a_missed & test[ch])
	  {
		chan[ch].missed++;
		ch_accum_reset(ch);
		}
		 }
	 }
	 mstat=from_gps(0x81)& 0x2000;
	 if (mstat)
	 {
		tic_count=(++tic_count)%10;
		if (tic_count==0) sec_flag=1;
		hms_count=(++hms_count)%600;
		if (hms_count==0) min_flag=1;
		nav_count=(nav_count+1)%nav_tic;
		if (nav_count==0) nav_flag=1;
		test_cnt=test_cnt ^ 0x20;
		test_control(test_cnt);
	 }
	 for (ch=0;ch<=11;ch++)
	 {
		if (nav_count==0)
		{
	epoch=ch_epoch(ch);
	chan[ch].epoch_ms=epoch&0x1f;
	chan[ch].epoch_bit=epoch>>8;
	chan[ch].epoch_code_phase=ch_code_phase(ch);
	chan[ch].epoch_code_DCO_phase=ch_code_DCO_phase(ch);
	chan[ch].meas_bit_time=chan[ch].tr_bit_time;
	chan[ch].epoch_carrier_DCO_phase=ch_carrier_DCO_phase(ch);
	chan[ch].epoch_carrier_cycle=ch_carrier_cycle(ch);
	chan[ch].doppler=chan[ch].carrier_freq;
		 }
	 }
	 for (ch=0;ch<=11;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;
	}
		}
	 }
// reset the interrupt
	 outportb(0x20,0x20);
}

void ch_acq(char ch)
{
  long prompt_mag,track_mag;
if (ch>0)
{
  if (abs(chan[ch].n_freq)<=search_max_f)//  search frequencies
  {
	 prompt_mag=rss(chan[ch].i_prompt,chan[ch].q_prompt);
	 track_mag  =rss(chan[ch].i_track,chan[ch].q_track);
	 if ((track_mag > acq_thresh) && (prompt_mag > acq_thresh))
	 {
		 ch_code_slew(ch,2044);     // slew back 1 chip
		 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;