init.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Function: void SetUpCorrelators(void)
*
* Open the configuration file GPSBLDDR2.CFG and read and validate the board
* base i/o port number and interrupt number. Then initialise the correlator
* and some base address constants.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void SetUpCorrelators(void)
{
    FILE *fpConfigFile;       /* Pointer to the file strat-up config file. */

    int Base_IO_Port;                   /* The board base i/o port number. */
    int Interrupt_Number;                   /* The board interrupt number. */
  
    char InputLine[SCREEN_WIDTH];           /* Line read from config file. */

    /* Open up the config file which contains the board i/o port base
       address and the interrupt number assigned to the master correlator. */

    if((fpConfigFile=fopen("GPSBLDR2.CFG","r"))==NULL)
    {
	printf("\n*** ERROR ***");
	printf("\n\nCan't open configuration file GPSBLDR2.CFG.\n");
	QuitGpsBuilder();
    }

    {
	/* Read and validate the base i/o port number. */

	int ValidPort[NUMBER_OF_VALID_PORTS] = 
		     {0x200, 0x220, 0x240, 0x260, 0x280, 0x2a0, 0x2c0, 0x2e0,
		      0x300, 0x320, 0x340, 0x360, 0x380, 0x3a0, 0x3c0, 0x3e0};
	int PortCounter;

	fgets(InputLine,SCREEN_WIDTH,fpConfigFile);
	sscanf(InputLine,"%x",&Base_IO_Port);

	for(PortCounter=0;PortCounter<=NUMBER_OF_VALID_PORTS-1;PortCounter++)
	{
	    if(Base_IO_Port==ValidPort[PortCounter])
		break;
	}
	if(PortCounter==NUMBER_OF_VALID_PORTS)
	{
	    printf("\n*** ERROR ***");
	    printf("\nSpecified i/o port %3.3x in GPSBLDR2.CFG is not "
		   "valid.\n",Base_IO_Port);
	    QuitGpsBuilder();  
	}
    }

    {
	/* Read and validate the master correlator interrupt number. */

	int ValidInterrupt[NUMBER_OF_VALID_INTERRUPTS] = {10, 11, 12, 14, 15};
	int InterruptCounter;

	fgets(InputLine,SCREEN_WIDTH,fpConfigFile);
	sscanf(InputLine,"%d",&Interrupt_Number);

	for(InterruptCounter=0;
	    InterruptCounter<=NUMBER_OF_VALID_INTERRUPTS-1;InterruptCounter++)
	{
	    if(Interrupt_Number==ValidInterrupt[InterruptCounter])
		break;
	}
	if(InterruptCounter==NUMBER_OF_VALID_INTERRUPTS)
	{
	    printf("\n*** ERROR ***");
	    printf("\nSpecified interrupt number %2.2d in GPSBLDR2.CFG is "
		   "not valid.\n",Interrupt_Number);
	    QuitGpsBuilder();
	}
    }    
	
    /* Read and validate the time zone. */

    fgets(InputLine,SCREEN_WIDTH,fpConfigFile);
    sscanf(InputLine,"%d",&TimeZone);

    if(TimeZone<-12 || TimeZone>12)
    {
	printf("\n*** ERROR ***");
	printf("\nSpecified time zone %2.2d in GPSBLDR2.CFG is not valid.\n",
	       TimeZone);
	QuitGpsBuilder();  
    }

    fclose(fpConfigFile);

    CORRELATOR = Base_IO_Port;           /* Global for correlator address. */
    GPIMASK = CORRELATOR+4;                  /* Global for interrupt mask. */
    GPRESET = CORRELATOR+6;                           /* Global for reset. */
    GPINT = Interrupt_Number;              /* Global for interrupt number. */

    /* Initialize the GP2021 in preparation for the basic interface test. */

    outpw(GPIMASK,MASK_INTERRUPT);                      /* Mask interrupt. */
    outpw(GPRESET,ASSERT_MASTER_RESET);            /* Assert master reset. */
    outpw(GPRESET,DEASSERT_MASTER_RESET);       /* De-assert master reset. */
}

/****************************************************************************    
* Function: void SetUpGlobals(void)
*
* Set the inital operating parameters of the GPS Builder. This includes data
* for the acquisition and tracking loops.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void SetUpGlobals(void)
{
    int channel;                                  /* Channel loop counter. */
    int satellite;                              /* Satellite loop counter. */

    double TadB;                        /* Code acquisition threshold, dB. */
    double TldB;                       /* Code loss-of-lock threshold, dB. */
    double CdLIavtime;         /* Code Lock indicator avgeraging time, ms. */
    double CodeSearchRate;          /* The C/A code search rate, chips/ms. */

    InitOscAcc = 4.0;      /* Initial (maximum) 10 MHz ref osc error, ppm. */

    /* Slightly pessimistic estimate of the reference oscillator's maximum
       rate of frequency movement under steady-state operating conditions/ */

    OscFreqChgRate = 0.01;   /* Units of ppm per SECOND (not per day) */

    /* Width of Doppler bins for search (Hz) */

    DoppBinWidth = 500;

    /* Code search rate in C/A code chips per millisecond */

    CodeSearchRate = 0.25;

    /* Acquisition threshold expressed in dB above the noise floor. */

    TadB = 6.0;

    /* Loss-of-signal threshold expressed similarly.  The receiver will hold
       on to a signal until we are 95% confident that the signal has dropped
       below this threshold. */

    TldB = 4.0;  /* Too low gives false locks */

    /* Code lock indicator averaging time is 2**CdLIav milliseconds */

    CdLIav = 5;

    /* Carrier lock indicator averaging time is 2**CrLIav milliseconds */

    CrLIav = 4;

    /* Set the number of ms a channel will wait before re-entering the
       search mode, if data lock has once been achieved.  (The code
       and carrier phases will continue to be extrapolated during this
       time.  If these phases are used as measurements, the process is
       called "clock coasting". */

    Coast = 20000;

    OscAcc = InitOscAcc;
    CurClkModel.VARClkError = InitOscAcc*InitOscAcc;

    /* The number of Doppler bins for search depends on the oscillator
       accuracy (initially InitOscAcc) and the doppler bin width. */

    NDoppBin = OscAcc*(L1/1.0E6)*2.0/DoppBinWidth + 0.5;
    if((NDoppBin&1) == 0)
	NDoppBin++; /* Make NDoppBin an even number. */

  /* Express the Doppler bin width (Hz) in terms of code and carrier DCO
     increments. These quantities tell how much to add to code and carrier
     DCO settings for DoppBinWidth Hz of Doppler shift. 
     
     For CodeDoppBinWidth the additional 'divide by 2.0' is included because
     the Code DCO runs at twice the C/A chip rate. */
    
    CodeDoppBinWidth = 
	(long)(DoppBinWidth/L1_TO_CA/(CODE_DCO_RESOLUTION/2.0)*CODE_DCO_SCALE
	       + 0.5);
    CarrDoppBinWidth = 
	 (long)(DoppBinWidth/CARRIER_DCO_RESOLUTION*CARRIER_DCO_SCALE + 0.5);

    /* For guaranteed acquisition of a signal which is TadB above the noise
      floor, the threshold needs to be set to a lower value which depends on
      the amount by which the scanning C/A code can miss the actual
      correlation peak in 1 ms and the maximum carrier frequency error at
      code correlation (half the bin width). 

      1.0 - CodeSearchRate - the amount by which the scanning C/A code
			     could miss the actual correlation peak in 1 ms.

      sin(PI*DoppBinWidth*0.001/2.0)/(PI*DoppBinWidth*0.001/2.0) 
			   - the loss due to a carrier frequency error of
			     DoppBinWidth/2. */

    Ta = NOISE_FLOOR_FLOAT*(1.0-CodeSearchRate)*pow(10.0,TadB/10.0)* 
	 sin(PI*DoppBinWidth*0.001/2.0)/(PI*DoppBinWidth*0.001/2.0);

    /* Once signal presence has been detected, hold on until we are 95%
       confident (SD=1.645) that the signal has dropped below the loss-of-
       signal threshold. */

    CdLIavtime = ldexp(1.0,CdLIav);                     /* In milliseconds */
    Tl = NOISE_FLOOR_FLOAT*(pow(10.0,TldB/10.0)
	* sin(PI*DoppBinWidth*0.001/2.0)/(PI*DoppBinWidth*0.001/2.0)
	- SD95PERCENT/sqrt(CdLIavtime));

    /* When the code is being scanned to search for correlation, the code DCO
       is set to a higher frequency than normal in order to scan (or slide)
       the generated code with respect to the satellite's C/A code
       modulation. CodeSrchIncr is the amount by which the Code DCO is to be
       set higher than its nominal value to produce the code search rate
       specified by the user in CodeSearchRate. 
       
       The factor of 1000.0 accounts for CodeSearchRate being in ms and 
       CodeSrchIncr in seconds. The factor of 2.0 is included because the
       Code DCO runs at twice the C/A code chip rate.*/

    CodeSrchIncr =
	CodeSearchRate*1000.0*CODE_DCO_SCALE/(CODE_DCO_RESOLUTION/2.0) + 0.5;

    TrackMode = HIGHEST_ELEVATIONS;             /* The deafult Track mode. */

    for(channel=0;channel<MAXCHANNELS;channel++)
	svsel[channel] = 0;                         /* No SV yet selected. */

    for(satellite=1;satellite<MAXSATELLITES+1;satellite++) /* No data yet. */
	TimeOfLastSubframe[satellite] = 0;

    ElvMask = 10.0;                         /* The default elevation mask. */
    GdopMask = 10.0;                             /* The default GDOP mask. */

    CurNavState.lat = 0.0;               /* The default receiver location. */
    CurNavState.lon = 0.0;
    CurNavState.hgt = 0.0;

    /* Zero out the quantities reported to the display task */
    
    for(channel=0; channel<MAXCHANNELS; channel++)
    {
	CurNavState.Erngerr[channel] = 0;
	CurNavState.Erraterr[channel] = 0;
	CurNavState.Eicperr[channel] = 0;
	CurNavState.EPRcorr[channel] = NO_DGPS; 
    }

    /* Obtain observer's XYZ coordinates. */

    LatLonHgtToXYZ(CurNavState.lat,CurNavState.lon,CurNavState.hgt,
		   &CurNavState.x,&CurNavState.y,&CurNavState.z);
      
    /* Compute the matrix to transform XYZ vectors to NEU (North-East-Up)
       vectors at the observer's estimated position. */

    LatLonToNorthEastUp(CurNavState.lat,CurNavState.lon,CurNavState.topo);
}

/****************************************************************************    
* Function: void InitialiseGP2021(void)
*
* Sets up register addresses and then initialises the GP2021 for normal
* operation.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void InitialiseGP2021(void)
{
    int channel;                                  /* Channel loop counter. */

    /* Set precomputed correlator I/O port addresses into the
       channel control data structures.  TR is a macro to transform
       from a correlator address (8 bits) to a specific PC I/O port address.
       Some address bit scrambling occurs in order to put the correlator
       select bits in the LSB's of the PC port address, thus conforming to ISA
       bus conventions. The remaining address bits can be placed into
       don't-care positions in the 6 MSB's of the port address.

       The use of high order address bits in don't-care positions is
       customary and fully supported by the ISA and EISA bus standards. */

#if(MAXCHANNELS>=1)
    CH[0]._SATCNTL = TR(CH01_SATCNTL);
    CH[0]._CARRIER_DCO_INCR_HIGH = TR(CH01_CARRIER_DCO_INCR_HIGH);
    CH[0]._CARRIER_DCO_INCR_LOW = TR(CH01_CARRIER_DCO_INCR_LOW);
    CH[0]._CODE_DCO_INCR_HIGH = TR(CH01_CODE_DCO_INCR_HIGH);
    CH[0]._CODE_DCO_INCR_LOW = TR(CH01_CODE_DCO_INCR_LOW);
    CH[0]._EPOCH_LOAD = TR(CH01_EPOCH_LOAD);
    CH[0]._EPOCH_CHECK = TR(CH01_EPOCH_CHECK);
    CH[0]._EPOCH_COUNT = TR(CH01_EPOCH_COUNT);