gpsrcvr.cpp

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

/***********************************************************************
  GPS RECEIVER (GPSRCVR) Ver. 1.10

  12 Channel All-in-View GPS Receiver Program based on Mitel GP2021
  chipset
  Clifford Kelley <[e-mail]>
  Copyright (c) 1996-2001 Clifford Kelley.  All Rights Reserved.
  This LICENSE must be included with the GPSRCVR code.

Revisions
  1.10
  Made the read of rcvr_par not so picky as long as the text field has no spaces
  Fixed a problem in CMatrix with the Borland 5.02 compiler
  Re-aligned column titles for pages 2-4
  1.09
  Corrected hot start problem
  Hooked up almanac data type check
  Started using Aberto's matrix libraries for DOP calculations
  Minor changes in pream
  Got rid of tmax in nav_fix
  Added more mesaurement integrity checks in nav_fix
  Added header and tdop to display
  1.08
  Corrected tracking loop bias problem (I hope!)
  Corrected code loop doppler correction constant
  1.07
  Added check for bit sync in pull-in, must be set to transition to track
  Extended pull-in to end of data bit to reduce transients when going into track
  Added integrated carrier phase processing for velocity
  Improved GPS time alignment mode (for any nav fix rate)
  Corrected iono correction computations (again)
  Corrected tropo correction sign
  Reduced debug arrays to reduce code size
  Added two new display pages, iono & tropo, and pseudorange and delta
  pseudorange
  Not used yet but getting set up to use Alberto Perez's matrix libraries
  1.06
  Added new format for Kalman.dat
  corrected iono correction computations
  Added more comments
  1.05
  Improved Code tracking loop
  Added GPS time alignment mode
  Added time zone input from rcvr_par.dat
  Corrected errors in satpos_ephemeris
  Corrected current.eph file format
  1.04
  Corrections in Gpsfuncs
  Added new display page for nav message HOW and TOW
  1.03
  New preamble routine
  Added Carrier phase aiding


***********************************************************************/
/*
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

			CONDITIONS

1. Redistributions of GPSRCVR source code must retain the above copyright
notice, this list of conditions, and the following disclaimer.

2. Redistributions in binary form must contain the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.

3. All modifications to the source code must be clearly marked as
such.  Binary redistributions based on modified source code must be
clearly marked as modified versions in the documentation and/or other
materials provided with the distribution.

4. Notice must be given of the location of the availability of the
unmodified current source code, e.g.,
	http://www.Kelley.com/
or
	ftp://ftp.Kelley.com
in the documentation and/or other materials provided with the
distribution.

5. All advertising and published materials mentioning features or use
of this software must display the following acknowledgment:  "This
product includes software developed by Clifford Kelley and other
contributors."

6. The name of Clifford Kelley may not be used to endorse or promote
products derived from this software without specific prior written
permission.

			DISCLAIMER

This software is provided by Clifford Kelley and contributors "as is" and
any expressed or implied warranties, including, but not limited to, the
implied warranties of merchantability and fitness for a particular
purpose are disclaimed.  In no event shall Clifford Kelley or
contributors be liable for any direct, indirect, incidental, special,
exemplary, or consequential damages (including, but not limited to,
procurement of substitute goods or services; loss of use, data, or
profits; or business interruption) however caused and on any theory of
liability, whether in contract, strict liability, or tort (including
negligence or otherwise) arising in any way out of the use of this
software, even if advised of the possibility of such damage.
*/


//  This is the primary data structure that provides access
//  to the components of each channel

struct channel
{
  int  state;
  long code_freq,carrier_freq,doppler,carrier_corr;
  char message[1500],tow_sync,prn,bit,frame_ready;
  int  offset,codes,n_freq,del_freq;
  int  t_count,ms_count,ms_set,i_confirm;
  int  ms_epoch,n_frame,ch_time,i_count;
  int  con_thresh,n_thresh,sfid,missed,page5;
  int  i_dith,q_dith,i_prompt,q_prompt;
  long sum,avg,old_theta,old_q_sum,th_rms;
  long dfreq,dfreq1,dcarr1,dcarr,cycle_sum;
  long old_carr_dco_phase;
  long q_dith_20,q_prom_20,i_dith_20,i_prom_20;
  long prompt_mag,dith_mag;
  long tr_bit_time,meas_bit_time,TOW,TLM;
  long carrier_counter;
  long  d_carr_phase;
  double int_carr_phase;
  unsigned long fifo0,fifo1;
  unsigned int carr_dco_phase,carr_cycle_l,carr_cycle_h;
  unsigned int epoch,code_phase,code_dco_phase;
  float CNo;
};


channel chan[12];
int a_missed,n_chan,chmax=11,display_page=0;;
unsigned test[16]=
{ 0x0001,0x0002,0x0004,0x0008,0x0010,0x0020,0x0040,0x0080,
  0x0100,0x0200,0x0400,0x0800,0x1000,0x2000,0x4000,0x8000};

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};
//
//   Include files
//
int out_debug,out_pos,out_vel,out_time,out_kalman;

//#include "CMatrix3.h"              // Matrix function libraries from
#include "CMatrix3.cpp"              // Alberto Perez,  Thanks Alberto!!
												 // They will help make the transition
												 // to a Kalman filter much easier
#include "gp2021.cpp"
#include "gpsfuncs.cpp"

//
//  Additional structure definitions
//

struct velocity
{
  double  east,north,up;
  double  clock_err;
  ecef    x,y,z;
};

struct hms
{
 int deg,min;
 float sec;
};

hms cur_lat,cur_long;
//double up;

struct state
{
  velocity  vel;
  eceft     pos;
  llh       loc;
};

state receiver;
#define IRQLEVEL        0       // IRQ Line

// Function definitions

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 read_rcvr_par(void);
void display(void);
void pream(char ,char );
void nav_fix(void);
long near_int(double);
void chan_allocate(void);
void cold_allocate(void);
void  velocity(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);

//  These are arrays for debugging
//  they can be written into while running and dumped to a
//  file at the end of the run
//  long far i_prompta[4500],far q_prompta[4500];
//  long far i_dithera[4500],far q_dithera[4500];
//  long far car_freq[4500],far chip_freq[4500];
//  int far data_bit[4500];
  long store_code,store_carrier;
// definitions with default values which can be overridden in
// file rcvr_par.dat
int  nav_tic,search_max_f=30,search_min_f=0,cold_prn=1;
long rms=312,acq_thresh=650,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;
long cc_scale=1540;
float nav_up=1.0;
double speed,heading;
int  pull_in_time=1000,phase_test=500;
long  d_freq=4698,d_tow,trk_div=19643;
int   confirm_m=10,n_of_m_thresh=8,key,tic_count=0,hms_count=0;
int   nav_count,min_flag,nav_flag,sec_flag,n_track;
unsigned int interr_int=512;
float clock_offset=-0.6;
ecef rec_pos_ecef;
long   i_TIC_dt;
double m_time[3],delta_m_time,m_error,TIC_dt;
long   TIC_cntr,old_TIC_cntr,TIC_ref=571427L,TIC_sum;

int debug_counter;

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

PARAMETERS None.

PURPOSE
	This is the main program to control the GPS receiver

WRITTEN BY
	Clifford Kelley

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

void main()
{
  int   i;
  char  ch;
  self_test();
  io_config(0x301);
  test_control(0);
  system_setup(0);
  reset_cntl(0x1fff);
  ch_status=1;
  read_rcvr_par();
  if (out_kalman==1) kalm   =fopen("gpskalm.log","w+");
  if (out_pos==1 || out_vel==1 ||out_time==1) stream=fopen("gpsrcvr.log","w+");
  read_initial_data();
  current_loc=receiver_loc();
  nav_tic=nav_up*10;
  old_TIC_cntr=TIC_cntr=TIC_ref;
  program_TIC(TIC_cntr);
  code_corr=clock_offset*24.0;
  for (ch=0;ch<=11;ch++) chan[ch].state=off;
  time(&thetime);  // set up thetime so it can be taken over by this program
  clrscr();
  rec_pos_llh.lon=0.0;rec_pos_xyz.x=0.0;
  rec_pos_llh.lat=0.0;rec_pos_xyz.y=0.0;
  rec_pos_llh.hae=0.0;rec_pos_xyz.z=0.0;
  out=fopen("debug.dat","w+");

  if ( status != cold_start ) chan_allocate();
  else if (status==cold_start ) cold_allocate();
  m_time[1]=clock_tow;
  read_ephemeris();
  Interrupt_Install();
//
  do
  {
    if (kbhit()) key = getch();
    else         key = '\0';
	 almanac_flag=0;
	 for (ch=0;ch<=11;ch++)
    {
		if (chan[ch].frame_ready==1 )
      {
	 navmess(chan[ch].prn,ch);   // decode the navigation message
	 chan[ch].frame_ready=0;     // for this channel
      }
    }
    if (sec_flag==1)
    {
	  thetime++;
	  stime(&thetime);
	  clock_tow=(++clock_tow)%604800L;
	  time_on++;
	  sec_flag=0;
	  for (ch=0;ch<=11;ch++)
	  {
		  if (chan[ch].state==track)
		  {
	//  Estimate  C/No
			  chan[ch].CNo=10.*log10(chan[ch].avg/1395.*
								chan[ch].avg/1395.*25.*1.7777+1.0);
			  if (chan[ch].CNo<25.0)
			  {
	// calculate carrier clock and doppler correction
			 chan[ch].carrier_corr=(-xyz[chan[ch].prn].doppler-
										  clock_offset*1575.42)/42.57475e-3;
	// calculate code clock and doppler correction
			 code_corr=clock_offset*24.+xyz[chan[ch].prn].doppler/65.5;
			 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].tow_sync=0;
			 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
			  }
		   }
	   }

    }
//    nav fix once every X seconds
    if (nav_flag==1)
    {
      nav_fix();
      nav_flag=0;
    }
//    channel allocation once every minute
    if (min_flag==1)
    {
		 if ( status != cold_start ) chan_allocate();
       else if (status==cold_start ) cold_allocate();
       min_flag=0;
    }
	 display();
	 if (key =='p' || key=='P')
	 {
		display_page++;
		display_page=display_page % 4;
		clrscr();
    }
  } while (key != 'x' && key != 'X');/*Stay in loop until 'X' key is pressed.*/
//
// Remove our interrupt and restore the old one
  Interrupt_Remove();
// Update the Almanac Data file
  if (almanac_valid==1)  write_almanac();
//  Update the Ephemeris Data file
  write_ephemeris();
//  Update the ionospheric model and UTC parameters
  write_ion_utc();
//    Update the curloc file for the next run
  if ( status==navigating )
  {
    out=fopen("curloc.dat","w+");
	 fprintf(out,"latitude  %f\n",rec_pos_llh.lat*r_to_d);
	 fprintf(out,"longitude %f\n",rec_pos_llh.lon*r_to_d);
	 fprintf(out,"hae       %f\n",rec_pos_llh.hae);
    fclose(out);
  }

//  out=fopen("debug.dat","w+");
//  fprintf(out,"carrier=%ld, code=%ld\n",store_carrier,store_code);
//  for (i=0;i<4500;i++)
//  {
//		 fprintf(out," %d, %ld, %ld, %ld, %ld, %ld, %ld, %d\n",
//		 i,i_prompta[i],q_prompta[i],i_dithera[i],q_dithera[i],
//		 car_freq[i],chip_freq[i],data_bit[i]);
//  }
  if (out_kalman==1) fclose(kalm);
}

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

PARAMETERS None.

PURPOSE
	This function displays the current status of the receiver on the
	computer screen.  It is called when there is nothing else to do

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
void display(void)
{
  char ch;
  gotoxy(1,1);
  printf("                      OpenSource GPS Software Version 1.10\n");
  printf("%s",ctime(&thetime));
  printf("TOW  %6ld\n",clock_tow);