procacc.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Function: void ProcAccumPhase(void)
*
* Processes the accumulation data for those channels which have data pending.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void ProcAccumPhase(void)
{
    switch (ActiveChannels)
    {
	case 12:
	    while(CH[11].AccumPending)
		ProcAccum(&CH[11]);
	case 11:
	    while(CH[10].AccumPending)
		ProcAccum(&CH[10]);
	case 10:
	    while(CH[9].AccumPending)
		ProcAccum(&CH[9]);
	case 9:
	    while(CH[8].AccumPending)
		ProcAccum(&CH[8]);
	case 8:
	    while(CH[7].AccumPending)
		ProcAccum(&CH[7]);
	case 7:
	    while(CH[6].AccumPending)
		ProcAccum(&CH[6]);
	case 6: 
	    while(CH[5].AccumPending)
		ProcAccum(&CH[5]);
	case 5:               
	    while(CH[4].AccumPending)
		ProcAccum(&CH[4]);
	case 4:
	    while(CH[3].AccumPending)
		ProcAccum(&CH[3]);
	case 3:
	    while(CH[2].AccumPending)
		ProcAccum(&CH[2]);
	case 2:
	    while(CH[1].AccumPending)
		ProcAccum(&CH[1]);
	case 1:
	    while(CH[0].AccumPending)
		ProcAccum(&CH[0]);
	default: break;
    };
}

/****************************************************************************    
* Function: void ProcAccum(register chanstruc _ds *CHPTR)
*
* Processes the accumulation data for (i) data demodulation, (ii) bitsync,
* (iii) code and carrier lock monitoring and (iv) code tracking loop updates.
*
* Input: *CHPTR - parameter block for the channel in question.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void ProcAccum(register chanstruc _ds *CHPTR)
{
    register accumstruc _ds *A;            /* Pointer to the Accum Buffer. */

    /* Point to the current accumulation buffer within the channel control
       block. */

    A = &CHPTR->ACCUM[CHPTR->iCURACCUM];

    /* Update the maximum accumulations pending count. */

    if(CHPTR->AccumPending > MaxAccumPending)
	MaxAccumPending=CHPTR->AccumPending;
    
    /* Maintain a running sum of the I arm over the past 20ms (1 data bit is
       20ms long). To maintain a sliding window of length 20ms (20 accumula-
       tions), each time a new I arm sum is added the I arm sum 20 readings
       before needs to be subtracted. */

    CHPTR->IPSUM20 += A->IDataBit
	  - CHPTR->ACCUM[(CHPTR->iCURACCUM+NACCUM-20)%NACCUM].IDataBit;

    /* Only do further processing if the channel is not idle (SV!=0). */

    if(CHPTR->SV!=0)
    {
	/* Update the code and carrier lock indicators. */

	UpdateLockIndicators(CHPTR,A);

	/* Check is coast mode should be entered or aborted. */

	CoastMode(CHPTR);

	/* Only attempt to bitsync and monitor the locks if we're not
	   coasting and we have code and carrier lock. */

	if(CHPTR->coasting==0 && CHPTR->corlk!=0 && CHPTR->carfrlk!=0)
	    BitSync(CHPTR,A->_1ms_epoch);

	/* Only attempt to monitor the locks if we're not coasting. */

	if(CHPTR->coasting==0)
	{
	    CodeLockMonitor(CHPTR,A->I2_Plus_Q2);
	    CarrierLockMonitor(CHPTR);
	}
 
	CodeTrackingLoop(CHPTR,A); 

	/* If the 1ms epoch counter is zero then a new data bit is ready for
	   processing. */

	if(A->_1ms_epoch==0)
	    ProcessDataBit(CHPTR,(CHPTR->IPSUM20>=0),A->_20ms_epoch);
    }

    if(++CHPTR->iCURACCUM >= NACCUM)       /* Check if the buffer is full. */
	CHPTR->iCURACCUM=0;            
    CHPTR->AccumPending--;      /* An accumulation set has been processed. */
}

/****************************************************************************
* Function: void UpdateLockIndicators(register chanstruc _ds *CHPTR,
*                                     register accumstruc _ds *A)
*
* Updates the code and carrier lock indicators.
*
* Input: *CHPTR - parameter block for the channel in question.
*        *A - accumulation data for the channel in question.          
*
* Output: *CHPTR - updates to the code and carrier lock indicators.
*         *A - update of the signal power.
*
* Return Value: None.
****************************************************************************/
void UpdateLockIndicators(register chanstruc _ds *CHPTR,
			  register accumstruc _ds *A)
{
    long i2;                           /* The inphase correlation squared. */
    long q2;                        /* The quadrature correlation squared. */
    long iim1_plus_qqm1;                     /* i(k)*i(k-1) + q(k)*q(k-1). */

    i2 = A->I_Prompt*A->I_Prompt;
    q2 = A->Q_Prompt*A->Q_Prompt;
    A->I2_Plus_Q2 = (i2 + q2)/4;         /* Power in the signal */
    iim1_plus_qqm1 = (A->I_Prompt*(long)CHPTR->IM1 + 
		      A->Q_Prompt*(long)CHPTR->QM1)/4;

    CHPTR->CdLI += iSAR(A->I2_Plus_Q2-CHPTR->CdLI+128,8);
    CHPTR->CrfrLI += iSAR(iim1_plus_qqm1 - CHPTR->CrfrLI+2048,12);
}

/****************************************************************************
* Function: void CoastMode(register chanstruc _ds *CHPTR)
*
* If coast mode is active then determines if (i) signal is back or (ii)
* coasting should continue or (iii) loss of signal. If coast mode is not
* active then determines if coast mode should be entered.
*
* Input: *CHPTR - parameter block for the channel in question.
*
* Output: *CHPTR - update to the coast time-out counter.
*
* Return Value: None.
****************************************************************************/
void CoastMode(register chanstruc _ds *CHPTR)
{
    /* If the channel is already in coast mode then determine if it 
       should continue or abandon (due to a timeout or the signal
       re-acquired). */

    if(CHPTR->coasting)
    {
	/* Compare the code and carrier lock indicators against their
	   threshold values. If both are above the threshold then quit
	   coast mode. */

	if((CHPTR->CdLI>=Ta) && (CHPTR->CrfrLI>=Ta))
	    CHPTR->coasting=0;     /* Quit coast mode */
	else
	{
	    /* Still don't have the signal back so decrement the coast 
	       counter. If the counter has reached zero then a timeout occurs
	       and code/carrier lock and frame sync are negated. */

	    CHPTR->coasting--;
	    if(CHPTR->coasting==0)                             /* Timeout. */
	    {
		CHPTR->carfrlk = CHPTR->bitlk = 0;
		CHPTR->FrameSync = FALSE;
		CHPTR->FrameSyncAndTIC = FALSE;
		CHPTR->LostLockDuringLastTIC = TRUE;
		CHPTR->LostCodeLockDuringLastTIC = TRUE;
		CHPTR->LostCarrierLockDuringLastTIC = TRUE;
	    }
	}
    }
    else
    {
	/* Coast mode is entered if whilst in carrier lock and bit lock
	   either the code or carrier lock indicators drop below the
	   thresholds. */

	if(CHPTR->carfrlk && CHPTR->bitlk 
				  && ((CHPTR->CrfrLI<Tl)||(CHPTR->CdLI<Tl)))
	CHPTR->coasting = Coast;         /* Determines the timeout period. */
    }
}

/****************************************************************************
* Function: void CodeLockMonitor(register chanstruc _ds *CHPTR,
*                                  unsigned long I2pQ2)
*
* Update the code lock monitor to check if the signal is present or if it
* has been lost.
*
* Input: *CHPTR - parameter block for the channel in question.
*        I2pQ2 - signal power.
*
* Output: *CHPTR - code lock monitor and initial parameters for code loop.
*
* Return Value: None.
****************************************************************************/
void CodeLockMonitor(register chanstruc _ds *CHPTR, unsigned long I2pQ2)
{
    if(I2pQ2 > Ta)
    {
	/* Instantaneous signal power is above the acquisition threshold.
	   Is this the initial acquisition? */

	if(CHPTR->corlk==0)
	{
	    /* Yes - initialize all the following. */

	    CHPTR->carfrlk = CHPTR->bitlk = 0;
	    CHPTR->LostLockDuringLastTIC = TRUE;
	    CHPTR->LostCodeLockDuringLastTIC = TRUE;
	    CHPTR->CdLI = 200000L;
	    CHPTR->EML = 0L;
	    CHPTR->NEML = 0;
	    CHPTR->CARRDCO
		    = CHPTR->pCARRDCO + CarrDoppFromClk - CHPTR->CarrDoppBin;

	    /* 770 is the scaling to go from carrier to code frequency and is 
	       equal to 1575.42E6/1.023E6/2. The factor of 2 accounts for the 
	       difference in scaling between the 2 constants
	       CARR_DCO_ZERO_DOPPLER and CODE_DCO_ZERO_DOPPLER. */

	    CHPTR->CODEDCO = CODE_DCO_ZERO_DOPPLER
		     - (long)(CHPTR->CARRDCO-CARR_DCO_ZERO_DOPPLER+385L)/770;

	    CHPTR->wdot_c = 0;
	    CHPTR->CrfrLI = 0;
	    CHPTR->corlk=1;
	}
    }

    if(CHPTR->CdLI < Tl)
    {
	/* Signal power is below the signal loss threshold.  Lose locks
	   on code, carrier, and also lose bit and frame sync. */

	CHPTR->corlk = CHPTR->carfrlk = CHPTR->bitlk = 0;
	CHPTR->FrameSync = FALSE;
	CHPTR->FrameSyncAndTIC = FALSE;
	CHPTR->LostLockDuringLastTIC = TRUE;
	CHPTR->LostCodeLockDuringLastTIC = TRUE;
	CHPTR->CARRDCO = CHPTR->pCARRDCO + CarrDoppFromClk 
			 - CHPTR->CarrDoppBin;
	CHPTR->CODEDCO = CHPTR->pCODEDCO + CodeSrchIncr
			 + CodeDoppFromClk + CHPTR->CodeDoppBin;
    }
}

/****************************************************************************
* Function: void CarrierLockMonitor(register chanstruc _ds *CHPTR)
*
* Update the carrier lock monitor to check if the signal is present or if it
* has been lost.
*
* Input: *CHPTR - parameter block for the channel in question.
*
* Output: *CHPTR - carrier lock monitor and initial parameter for carrier 
*                  loop.
*
* Return Value: None.
****************************************************************************/
void CarrierLockMonitor(register chanstruc _ds *CHPTR)
{
    if(CHPTR->carfrlk==0)                    /* No carrier frequency lock. */
    {
	if(CHPTR->CrfrLI > Ta)              /* Got carrier frequency lock. */
	{
	    CHPTR->carfrlk=1;
	    CHPTR->phase_error = 0;
	    CHPTR->avg_phase_change = 12860L;
	}
    }
    else
    {
	if(CHPTR->CrfrLI <= Tl)            /* Lost carrier frequency lock. */
	{
	    /* We have just lost carrier lock.  Indicate carrier lock and
	       also set the carrier loss-of-lock flag.*/

	    CHPTR->carfrlk = 0;
	    CHPTR->LostLockDuringLastTIC = TRUE;
	    CHPTR->LostCarrierLockDuringLastTIC = TRUE;
	    CHPTR->phase_error = 0;
	    CHPTR->avg_phase_change = 12860L;
	}
    }
}

/****************************************************************************
* Function: void CodeTrackingLoop(register chanstruc _ds *CHPTR,
*                                 register accumstruc _ds *A)
*
* Updates the code tracking loop.
*
* Input: *CHPTR - parameter block for the channel in question.
*        *A - accumulation data for the channel in question.          
*
* Output: *CHPTR - updates to the code tracking parameters.
*
* Return Value: None.
****************************************************************************/
void CodeTrackingLoop(register chanstruc _ds *CHPTR,
		      register accumstruc _ds *A)
{                
    long id2;                                   /* Inphase dither squared. */
    long qd2;                                /* Quadrature dither squared. */ 
    long ip2;                                   /* Inphase prompt squared. */ 
    long qp2;                                /* Quadrature prompt squared. */ 
    long codedco_update;          /* Update to the code DCO tracking loop. */

    /* Get the squares of the prompt and dither correlations from the 
       inphase a quadrature arms. */

    ip2 = ISquare(A->IP);
    qp2 = ISquare(A->QP);
    id2 = ISquare(A->ID);
    qd2 = ISquare(A->QD);

    /* Get the running sum early minus late. This is the measurement of
       the code phase error. */

    CHPTR->EML += iSAR(id2+qd2,2) - iSAR(ip2+qp2,2);

    /* If channel actually has code lock then tracking can occur. */

    if(CHPTR->corlk)
    {   
	CHPTR->NEML++;                       /* Increment the EML counter. */

	/* Only update the code tracking if EML has been accumulated over
	   EMLREADINGS readings. */

	if(CHPTR->NEML>=EMLREADINGS)
	{
	    if(CHPTR->carfrlk)              /* Carrier aiding can be used. */
	    {
		/* 770 is the scaling to go from carrier to code frequency
		   and is equal to 1575.42E6/1.023E6/2. The factor of 2 
		   accounts for the difference in scaling between the 2
		   constants CARR_DCO_ZERO_DOPPLER and 
		   CODE_DCO_ZERO_DOPPLER. */
		
		CHPTR->CODEDCO = CODE_DCO_ZERO_DOPPLER
		     - (long)(CHPTR->CARRDCO-CARR_DCO_ZERO_DOPPLER+385L)/770;

		/* This filter is equivalent to:
		 *
		 * codedco(i) = codedco(i-1)
		 *              + (phase_error(i) - phase_error(i-1))/2^16
		 *              + phase_error(i)/2^21
		 */

		codedco_update = iSAR(CHPTR->EML+16,5)
				 + CHPTR->EML - CHPTR->EMLOLD;
		CHPTR->CODEDCO += iSAR(codedco_update+32768L,16);
	    }
	    else                                     /* No carrier aiding. */
	    {
		/* This filter is equivalent to:
		 *
		 * codedco(i) = codedco(i-1)
		 *              + (phase_error(i) - phase_error(i-1))/2^16
		 *              + phase_error(i)/2^20
		 */

		codedco_update = iSAR(CHPTR->EML+8,4)
				 + CHPTR->EML - CHPTR->EMLOLD;
		CHPTR->CODEDCO += iSAR(codedco_update+32768L,16);
	    }

	    /* Save the old early minus late value for the next iteration
	       of the tracking loop. */

	    CHPTR->EMLOLD = CHPTR->EML;

	    /* Reset the early minus late running total and accumulations 
	       counter. */  

	    CHPTR->EML = 0L;           
	    CHPTR->NEML = 0;
	}
    }                                                     /* If code lock. */

    /* Send out the new code DCO increment to the correlator after 
       scaling the loop gain. See the GPS Builder manual for an explaination
       and derivation of these values. */

    codedco_update = (CHPTR->CODEDCO+16L)>>5;

    disable();        /* Disable the interrupts before writing to the DCO. */
    outpw(CHPTR->_CODE_DCO_INCR_HIGH,(unsigned)(codedco_update>>16));
    outpw(CHPTR->_CODE_DCO_INCR_LOW,(unsigned)codedco_update);
    enable();                                 /* Re-enable the interrupts. */
}