buffacc.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Function: void BufferAccumPhase(void)
*
* Extract and process accumulation data for each channel which is active and
* has new accumulation data.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void BufferAccumPhase(void)
{
    int accum_status_a; /* Contents of status register ACCUM_STATUS_A */
    int accum_status_b; /* Contents of status register ACCUM_STATUS_B */

    accum_status_a = inpw(TR(ACCUM_STATUS_A));
    accum_status_b = inpw(TR(ACCUM_STATUS_B));

    accum_status_a &= ActiveChannelMask; /* Only do the active channels. */
    accum_status_b &= ActiveChannelMask; /* Only do the active channels. */

    /* First check for missed accumulations */

    if(accum_status_b&0x001)
    {
	missa++;
	outpw(CH[0]._ACCUM_RESET,0);
	CH[0].LostCodeLockDuringLastTIC = TRUE;
	CH[0].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x002)
    {
	missa++;
	outpw(CH[1]._ACCUM_RESET,0);
	CH[1].LostCodeLockDuringLastTIC = TRUE;
	CH[1].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x004)
    {
	missa++;
	outpw(CH[2]._ACCUM_RESET,0);
	CH[2].LostCodeLockDuringLastTIC = TRUE;
	CH[2].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x008)
    {
	missa++;
	outpw(CH[3]._ACCUM_RESET,0);
	CH[3].LostCodeLockDuringLastTIC = TRUE;
	CH[3].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x010)
    {
	missa++;
	outpw(CH[4]._ACCUM_RESET,0);
	CH[4].LostCodeLockDuringLastTIC = TRUE;
	CH[4].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x020)
    {
	missa++;
	outpw(CH[5]._ACCUM_RESET,0);
	CH[5].LostCodeLockDuringLastTIC = TRUE;
	CH[5].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x040)
    {
	missa++;
	outpw(CH[6]._ACCUM_RESET,0);
	CH[6].LostCodeLockDuringLastTIC = TRUE;
	CH[6].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x080)
    {
	missa++;
	outpw(CH[7]._ACCUM_RESET,0);
	CH[7].LostCodeLockDuringLastTIC = TRUE;
	CH[7].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x100)
    {
	missa++;
	outpw(CH[8]._ACCUM_RESET,0);
	CH[8].LostCodeLockDuringLastTIC = TRUE;
	CH[8].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x200)
    {
	missa++;
	outpw(CH[9]._ACCUM_RESET,0);
	CH[9].LostCodeLockDuringLastTIC = TRUE;
	CH[9].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x400)
    {
	missa++;
	outpw(CH[10]._ACCUM_RESET,0);
	CH[10].LostCodeLockDuringLastTIC = TRUE;
	CH[10].LostCarrierLockDuringLastTIC = TRUE;
    }
    if(accum_status_b&0x800)
    {
	missa++;
	outpw(CH[11]._ACCUM_RESET,0);
	CH[11].LostCodeLockDuringLastTIC = TRUE;
	CH[11].LostCarrierLockDuringLastTIC = TRUE;
    }

    /* Now get the new accumulation data */

    if(accum_status_a&0x0001) BufferAccum(&CH[0]);
    if(accum_status_a&0x0002) BufferAccum(&CH[1]);
    if(accum_status_a&0x0004) BufferAccum(&CH[2]);
    if(accum_status_a&0x0008) BufferAccum(&CH[3]);
    if(accum_status_a&0x0010) BufferAccum(&CH[4]);
    if(accum_status_a&0x0020) BufferAccum(&CH[5]);
    if(accum_status_a&0x0040) BufferAccum(&CH[6]);
    if(accum_status_a&0x0080) BufferAccum(&CH[7]);
    if(accum_status_a&0x0100) BufferAccum(&CH[8]);
    if(accum_status_a&0x0200) BufferAccum(&CH[9]);
    if(accum_status_a&0x0400) BufferAccum(&CH[10]);
    if(accum_status_a&0x0800) BufferAccum(&CH[11]);
}

/****************************************************************************    
*
* Function: void BufferAccum(register chanstruc _ds *CHPTR)
*
* Gets the accumulation data for the current channel and stores it in the 
* accumulation buffer. Updates the carrier tracking loops.
*
* Input: *CHPTR - parameter block for the channel in question.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void BufferAccum(register chanstruc _ds *CHPTR)
{
    unsigned epoch_check;   /* Contents of EPOCH_CHECK register */
    long carrdco_update;    /* Update to the carrier DCO */
    long deltaI, deltaQ;    /* Change in I and Q over last ms */
    long Inew, Qnew;        /* I and Q rotated by carrier phase error */
    long delta_phase_error; /* Phase error of Inew and Qnew */
    long phase_change;      /* Change in carrier phase over last ms*/
    long delta_carrier_dot; /* Change in carrier frequency dot over last ms */
    long delta_carrier;     /* Change in carrier frequency over last ms*/
    long discriminator;     /* Coarse measurement of carrier phase change */
    long cos_phase_error;   /* Cosine of the I and Q phase error */
    long sin_phase_error;   /* Sine of the I and Q phase error */
    
    register accumstruc _ds *A;  /* Pointer to the structure where the
				    accumulation data is to be stored.
				    Must be in DGROUP. */

    /* Get the address of accum structure where the data is to be stored. */    

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

    /* Get the 1ms and 20ms epoch counts for the EPOCH_CHECK register. */

    epoch_check = inpw(CHPTR->_EPOCH_CHECK);
    A->_1ms_epoch = epoch_check&0x001F;
    A->_20ms_epoch = epoch_check>>8;

    /* Slew the 1ms and/or 20ms epoch counter if requested. The slews 
       can only occur when the respective epoch counters are zero.*/

    if(A->_1ms_epoch==0)
    {
	if(A->_20ms_epoch==0)
	{
	    if(CHPTR->_20MS_EPOCH_SLEW)
	    {
		outpw(CHPTR->_EPOCH_LOAD,(CHPTR->_20MS_EPOCH_SLEW<<8));
		CHPTR->_20MS_EPOCH_SLEW = 0;
	    }
	}
	if(CHPTR->_1MS_EPOCH_SLEW)
	{
	    outpw(CHPTR->_EPOCH_LOAD,CHPTR->_1MS_EPOCH_SLEW);
	    CHPTR->_1MS_EPOCH_SLEW = 0;
	}
    }

    /* Get I & Q from the Track channel (1/4 chip early). */

    A->ID = inpw(CHPTR->_I_TRACK); 
    A->QD = inpw(CHPTR->_Q_TRACK);

    /* Get I & Q from the Prompt channel (1/4 chip late). */

    A->IP = inpw(CHPTR->_I_PROMPT); 
    A->QP = inpw(CHPTR->_Q_PROMPT); 

    /* Get the true prompt I & Q. */
    
    A->I_Prompt = (long)(A->IP + A->ID);
    A->Q_Prompt = (long)(A->QP + A->QD);

    /* If correlation lock has been achieved and the channel is not    
       coasting then update the carrier tracking loop. */

    if(CHPTR->corlk && CHPTR->coasting==0)
    {
	/* Get the change in phase between the last and current
	   samples, degrees. */

	/* 37 = (2^13/554^2) approx 2^5 
	   554 is the I or Q value for 5dB above noise floor */

	phase_change = iSAR(A->Q_Prompt*CHPTR->IM1 - 
			    A->I_Prompt*CHPTR->QM1 + 16,5);

	/* Carrier acquisition is achieved using 2 acquisition loops
	   a wide-band frequency discriminator to reduce to frequency error
	   rapidly from about 500Hz down to 10 Hz and then a 2nd order
	   frequency tracking loop for fine carrier tracking. */

	/* 3758 = 10 Hz in Carrier DCO units (16 times written value) */

	if(CHPTR->avg_phase_change<3758L)
	{
	    /* The carrier filter here forms a FLL. The filter is a 2nd order
	       discretised Jaffe-Rechtin with bandwidth of 1 Hz. See Cahn et
	       al: "Software Implementation of a PN Receiver", IEEE Trans
	       COM-25 No.8, Aug. 1977, for a full explaination. */

	    /* Get the change in the carrier frequency derivative over the
	       last sample period. */

	    delta_carrier_dot = (phase_change<<2);

	    /* Get the change in the carrier frequency over the last sample
	       period scaled by 2^13. */

	    delta_carrier =  iSAR(CHPTR->wdot_c + 512L,10) 
			     + (phase_change<<1);

	    /* Save the carrier frequency rate for next time. */

	    CHPTR->wdot_c += delta_carrier_dot;

	    /* Get the carrier DCO update. delta_carrier is still scaled by
	       2^13 but only shift by 11 places because the scale factor
	       for the carrier dco unit to radians/s is approximately 2^-2. */

	    carrdco_update = iSAR(delta_carrier+1024,9);

	    /* Update the carrier DCO for this channel. */
 
	    CHPTR->CARRDCO += carrdco_update;

	    /* Now do the data demodulation loop */

	    /* Rotate I and Q by the current phase error */

	    cos_phase_error = CosApprox(CHPTR->phase_error);
	    sin_phase_error = SinApprox(CHPTR->phase_error);

	    Inew = A->I_Prompt*cos_phase_error + A->Q_Prompt*sin_phase_error;
	    Qnew = A->Q_Prompt*cos_phase_error - A->I_Prompt*sin_phase_error;

	    A->IDataBit = Inew>>15;

	    delta_phase_error = (Inew>=0)? Qnew:-Qnew;

	    /* Update the phase error, scaled by 2^13 */

	    CHPTR->phase_error += iSAR(delta_phase_error+16384L,15);
	}
	else
	{
	    /* Derive a delta-phase measurement which is used to reduce the
	       carrier difference frequency before phase lock has been
	       achieved. This is a four-quadrant frequency discriminator. */

	    deltaI = A->I_Prompt - CHPTR->IM1;  /* Change in i over last ms */
	    deltaQ = A->Q_Prompt - CHPTR->QM1;  /* Change in q over last ms */  
    
	    /* Get a measurement of the change in carrier phase over the last
	       ms. Get the correct sign of the phase change by looking at the
	       signs of the inphase and quadrature components. */

	    if(labs(A->I_Prompt) > labs(A->Q_Prompt))
		discriminator = (A->I_Prompt>0L)? deltaQ: -deltaQ;
	    else
		discriminator = (A->Q_Prompt>0L)? -deltaI: deltaI;

	    carrdco_update = iSAR(discriminator+128,8);
	    CHPTR->CARRDCO += carrdco_update;

	    CHPTR->avg_phase_change += 
		  iSAR(carrdco_update - CHPTR->avg_phase_change + 512,10);
	}
	carrdco_update = (CHPTR->CARRDCO+8)>>4; /* Scaling to DCO gain */
	outpw(CHPTR->_CARRIER_DCO_INCR_HIGH,(unsigned)(carrdco_update>>16));
	outpw(CHPTR->_CARRIER_DCO_INCR_LOW,(unsigned)carrdco_update);
    }

    CHPTR->IM1 = A->I_Prompt;  
    CHPTR->QM1 = A->Q_Prompt;  

    /* Release the accumulation buffer to the ProcAccum process. */

    if(++CHPTR->iNXTACCUM >= NACCUM)  /* A circular buffer */
	CHPTR->iNXTACCUM=0;
    ++CHPTR->AccumPending;            /* Another accumulation pending */
}