gpsrcvr(1).cpp

开源GPS源码

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

  12 Channel All-in-View GPS Receiver Program based on Mitel GP2021
  chipset
  Clifford Kelley <[e-mail]>
  This program is licensed under GNU GENERAL PUBLIC LICENSE
  This LICENSE file must be included with the GPSRCVR code.

  I would like to thank the following people who have contributed to
  OpenSource GPS.  If I have left someone out I appologize and ask you
  to correct my oversight.

  Joel Barnes      University of New South Wales
  Jingrong Cheng   University of California Riverside
  Georg Beyerle    GFZ
  Alberto Perez    Boeing
  Phil Bender      AMCC
  Doug Baker       Navman
  Elmer Thomas     University of California Riverside
  Andrew Greenberg Portland State University


Revisions
  1.15
  Added more integrity checking into the data bit synchronization in the pull-in
  state.  Instead of just checking the change in phase and checking that it is
  very close to a +-90 to -+90 transition it also stores the last 20 ms sign
  values and they must all be the same and opposite of the transitioned ms sign
  before declaring the edge of a data bit.  Modified parity check algorithm to
  be more efficient. Also made constants for number of channels and register
  address and data address.
  1.14
  Fixed error in nmea checksum routine.  Added another display page and
  supporting data structure to show parity errors in navigation message
  decoding.
  1.13
  Made mods to get ICP working better, trying to use just the last 0.1 second
  of carrier phase data.  Phil Bender added NMEA serial port messages (needs
  some more work, see files serport.cpp, nmea.cpp, fwinter.cpp). Additional
  entries at the bottom of rcvr_par.dat are used to set up the NMEA messages.
  Adapted program so it can compile under Borland (#ifdef BCPP) or Microsoft
  Visual CPP (#ifdef VCPP)  Also split up the program into library components
  to make it more modular (see GPS_ISR.CPP and NAVFIX.CPP)
  1.12
  Removed some of the ephemeris checks that were causing the rejection of
  valid ephemeris messages and also caused it not to accept ephemeris
  messages from a constellation simulator.  Fixed up the 1 second data bit
  ambiguities to recover measurements that previously had been thrown out.
  1.11
  Cleaned up the display when a satellite is removed from consideration
  Fixed an ephemeris management problem that caused occasional use of the
  ephemeris from the wrong satellite
  Modified the ephemeris mgmt to check IODE and IODC to decide when
  to grab a new ephemeris + ephemeris integrity checks
  Improved the almanac page redundancy check
  Moved Pr, dPR, Tropo and Iono into the main channel data structure
  Set up default iono parameters when ion_utc.dat not available
  Added a debug output that provides raw measurements data and ephemeris
  which provides more processing capability than pseudorange & delta pseudorange
  Fully implemented the GPSRCVR.LOG output options
  Added the option in RCVR_PAR.DAT to use CTL (carrier tracking loop) or
  ICP (integrated carrier phase) data for velocity computations (ICP seems
  to have bias problems)
  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


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

#ifdef VCPP

#include "graph.h"  // PGB MS

#endif

#include  "serport.h" // NMEA
#include  "NMEA.h"     // NMEA
#include  <time.h>
#include  <stdio.h>
#include  <stdlib.h>
#include  <conio.h>
#include  <math.h>
#include  <string.h>
#include  <io.h>
#include  <Dos.h>

#include "gp2021.h"
#define MAIN
#include  "consts.h"
#include  "structs.h"

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

#include "gpsfuncs.h"
#include "gpsrcvr.h"


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

PARAMETERS None.

PURPOSE
	This is the main program to control the GPS receiver

WRITTEN BY
	Clifford Kelley

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





void main()
{
  char  ch;
  int i;
  self_test();
  io_config(0x301);
  test_control(0);
  system_setup(0);
  reset_cntl(0x0);
//  delay(100);
  reset_cntl(0x1fff);
  ch_status=1;
  read_rcvr_par();
  rec_pos_xyz.x=0.0;
  rec_pos_xyz.y=0.0;
  rec_pos_xyz.z=0.0;

  if (out_kalman==1) kalm   =fopen("gpskalm.log","w+");
  if (out_pos==1 || out_vel==1 ||out_time==1) stream=fopen("gpsrcvr.log","w+");
  if (out_debug==1) debug=fopen("debug.log","w+");
  read_initial_data();
  current_loc=receiver_loc();
  rec_pos_llh.lon=current_loc.lon;
  rec_pos_llh.lat=current_loc.lat;
  rec_pos_llh.hae=current_loc.hae;
  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<N_channels;ch++)
  {
    chan[ch].state=off;
    for (i=0;i<5;i++)chan[ch].word_error[i]=0;
  }
  time(&thetime);  // set up thetime so it can be taken over by this program
#ifdef VCPP

  

  _setbkcolor(1);
  _displaycursor( _GCURSOROFF);
  _clearscreen( _GCLEARSCREEN);  // PGB MS
	
  
	//
	// Get the currnet system time.  MS_TIME
	//
	struct _dostime_t gDosTime;
    struct _dosdate_t gDosDate;
	_dos_gettime( &gDosTime );
	_dos_getdate( &gDosDate );

#endif

#ifdef BCPP
  clrscr();
#endif

  if ( status != cold_start ) chan_allocate();
  else if (status==cold_start ) cold_allocate();
  m_time[1]=clock_tow;
  read_ephemeris();
  int err;
  open_com( 0, Com0Baud, 0, 1, 8, &err ); // NMEA

  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)
    {
		SendNMEA();

//	gotoxy(1,24);
//	printf("->start display");
     almanac_flag=0;
	  thetime++;
#ifdef BCPP
	  stime(&thetime);
#endif


			
	  clock_tow=(++clock_tow)%604800L;
	  time_on++;
	  sec_flag=0;
	  for (ch=0;ch<N_channels;ch++)
	  {
		  if (chan[ch].state==track)
		  {
	//  Estimate  C/No
           if (chan[ch].avg>0 )
           {
			  chan[ch].CNo=10.*log10(chan[ch].avg/1395.*
								chan[ch].avg/1395.*25.*1.7777);
           }
           else chan[ch].CNo=0.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;
#ifdef BCPP
       clrscr();
#endif
#ifdef VCPP
       _clearscreen( _GCLEARSCREEN); // PGB MS
#endif
    }
	 display();
	 if (key =='p' || key=='P')
	 {
		display_page++;
		display_page=display_page % 5;
#ifdef BCPP
               clrscr();
#endif
#ifdef VCPP
       _clearscreen( _GCLEARSCREEN); // PGB MS
#endif
    }
  } while (key != 'x' && key != 'X');/*Stay in loop until 'X' key is pressed.*/

#ifdef VCPP  // PGB

    //
	// There may be a better way????
	//

	struct tm* TempTime;
	TempTime = localtime( &thetime );

	//
	// Update system time.
	//
	gDosTime.hour	= TempTime->tm_hour;   
	gDosTime.minute	= TempTime->tm_min; 
	gDosTime.second = TempTime->tm_sec;

	gDosDate.month  = TempTime->tm_mon + 1;
	gDosDate.day    = TempTime->tm_mday;
	//
	// tm_year is the current year - 1900
	//
	gDosDate.year   = 1900 + TempTime->tm_year;

	gDosDate.dayofweek = TempTime->tm_wday;

	_dos_settime( &gDosTime ); //  TODO 
	_dos_setdate( &gDosDate ); //  TODO 
  
#endif  
   
  
  //
// Remove our interrupt and restore the old one
//
  Interrupt_Remove();
  close_com(); // NMEA
// 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 (ch=0;ch<6;ch++)
  {
    fprintf(out," ch=%d  offset=%d  \n",ch,chan[ch].offset);
    for ((int)i=0;i<1500;i++)
    {
		 fprintf(out," %d %d, %ld, %ld, %ld, %ld\n",
		 ch, i,qdither[ch][i],qprompt[ch][i],idither[ch][i],iprompt[ch][i]);
    }
  }
  for (i=0;i<30000;i++)
  {
		 fprintf(out," %d, %d, %d, %d, %d\n",
		 i,qdither0[i],qprompt0[i],idither0[i],iprompt0[i]);
  }
*/
  fcloseall();
}

/*******************************************************************************
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;
#ifdef VCPP
  _settextposition(1,1);
#endif
#ifdef BCPP
  gotoxy(1,1);
#endif
  printf("                      OpenSource GPS Software Version 1.15\n");
  printf("%s",ctime(&thetime));
  printf("TOW  %6ld\n",clock_tow);
  printf("meas time %f  error %f  delta %f\n",m_time[1],m_error,delta_m_time);
  cur_lat.deg=rec_pos_llh.lat*r_to_d;
  cur_lat.min=(rec_pos_llh.lat*r_to_d-cur_lat.deg)*60;
  cur_lat.sec=(rec_pos_llh.lat*r_to_d-cur_lat.deg-cur_lat.min/60.)*3600.;
  cur_long.deg=rec_pos_llh.lon*r_to_d;
  cur_long.min=(rec_pos_llh.lon*r_to_d-cur_long.deg)*60;
  cur_long.sec=(rec_pos_llh.lon*r_to_d-cur_long.deg-cur_long.min/60.)*3600.;
  printf("   latitude    longitude          HAE      clock error (ppm)\n");
  printf("  %4d:%2d:%5.2f  %4d:%2d:%5.2f  %10.2f  %f\n",
  cur_lat.deg,abs(cur_lat.min),fabs(cur_lat.sec),cur_long.deg,abs(cur_long.min),
  fabs(cur_long.sec),rec_pos_llh.hae,clock_offset);
  printf(" Speed     Heading      TIC_dt\n");
  printf(" %lf   %lf   %lf\n",speed,heading*r_to_d,TIC_dt);
  printf("   \n");
  printf("tracking %2d status %1d almanac valid %1d gps week %4d\n",
  n_track,status,almanac_valid,gps_week%1024);
  if (display_page==0)
  {
	 printf(
" ch prn state n_freq az el doppler t_count n_frame sfid ura page missed CNo\n");
	 for (ch=0;ch<N_channels;ch++)
	 {
		printf(
" %2d %2d  %2d  %3d   %4.0f  %3.0f   %6.0f   %4d  %4d  %2d  %3d  %3d%5d   %4.1f\n",
		ch,chan[ch].prn,chan[ch].state,chan[ch].n_freq,
		xyz[chan[ch].prn].azimuth*57.3,xyz[chan[ch].prn].elevation*57.3,
		xyz[chan[ch].prn].doppler,chan[ch].t_count,chan[ch].n_frame,chan[ch].sfid,
		gps_eph[chan[ch].prn].ura,chan[ch].page5,chan[ch].missed,chan[ch].CNo);