sync.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Function: void ProcessDataBit(register chanstruc _ds *CHPTR, unsigned dbit,
*                               unsigned _20ms_epoch)
*
* Stores the message data bits from each satellite and performs frame sync
* if this has not already been achieved. If frame sync has been achieved
* then monitors when a complete data frame has been received and dispatches
* it for processing.
*
* Input: CHPTR - pointer to the channel structure for the SV concerned.
*        dbit - the data bit.
*        _20ms_epoch - the 20ms epoch count.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void ProcessDataBit(register chanstruc _ds *CHPTR, unsigned dbit,
		    unsigned _20ms_epoch)
{
    unsigned long dataTIC;
    unsigned long subframe[FRAME_SIZE_PLUS_2];

    dataTIC = TIC-12;  /* SF being parsed ended 1.2 seconds (60 bits) ago. */

    StoreData(CHPTR,dbit);

    /* If frame sync has been achieved then see if a complete frame has
       been logged. */

    if(CHPTR->FrameSync==TRUE) 
    {
	CHPTR->BitNumber = (CHPTR->BitNumber+1)%300; 
	if(CHPTR->BitNumber==0)
	{
	    if(ValidFrameFormat(CHPTR,subframe))
	    {
		Checkpoint("ProcessDataBit",1);
		EmitSubframe(CHPTR,subframe,dataTIC); 
	    }
	}
    }

    /* Else attempt frame sync. */

    if(CHPTR->bitlk==TRUE && CHPTR->FrameSync==FALSE)
	FrameSync(CHPTR,_20ms_epoch);
}

/****************************************************************************    
* Function: void StoreData(register chanstruc _ds *CHPTR, unsigned dbit)
*
* Stores message data bits for each satellite into separate arrays.
*
* Input: CHPTR - pointer to the channel structure for the SV concerned.
*        dbit - the data bit.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void StoreData(register chanstruc _ds *CHPTR, unsigned dbit)
{
    int dataword;
    int feedbit;

    /* Each word in wordbuff is composed of: 
       Bits 0 to 29 = the GPS data word
       Bits 30 to 31 = 2 LSBs of the GPS word ahead. */ 

    for(dataword=0;dataword<=(FRAME_SIZE_PLUS_2-2);dataword++)
    {
	/* Shift the data word 1 place to the left. */

	CHPTR->wordbuff[dataword] <<= 1;

	/* Add the MSB of the following GPS word (bit 29 of a wordbuff
	   word). */

	feedbit = (CHPTR->wordbuff[dataword+1]>>29)&0x0001;
	CHPTR->wordbuff[dataword] += feedbit;
    }

    /* The bit added for the 12th word is the new data bit. */

    CHPTR->wordbuff[FRAME_SIZE_PLUS_2-1] <<= 1;
    CHPTR->wordbuff[FRAME_SIZE_PLUS_2-1] += dbit;
}

/****************************************************************************    
* Function: void BitSync(register chanstruc _ds *CHPTR, int _1ms_epoch)
*
* Performs data bit synchronisation.This occurs when the 1ms epoch counter
* is zero at the start of each data bit. A running sum of the inphase
* correlation at each of the possible 20 1ms epoch settings is maintained.
* The best is selected and the 1ms epoch is slewed so that an epoch count of
* zero corresponds to this position. If the best position is constant for
* a fixed period then bit lock is declared.
*
* Input: CHPTR - pointer to the channel structure for the SV concerned.
*        _1ms_epoch - the 1ms epoch count.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void BitSync(register chanstruc _ds *CHPTR, int _1ms_epoch)
{
    int epoch_counter;

    /* Update the corresponding bit integral. */

    CHPTR->DBINTEG[_1ms_epoch]    
      += (long)(labs(CHPTR->IPSUM20)-CHPTR->DBINTEG[_1ms_epoch])>>6;

    /* If a 1ms epoch slew is pending then don't bother attempting sync. */

    if(CHPTR->_1MS_EPOCH_SLEW==0)
    {
	/* If the 1ms epoch counter is zero and 2000 (2s) worth of data bit
	   integrations have occurred. */

	if((_1ms_epoch==0) && (CHPTR->nDBINTEG>=2000))
	{
	    /* If the best epoch position is non-zero then a slew of the
	       1ms epoch counter is needed. */

	    if(CHPTR->BESTEPOCH!=0)
	    {
		/* The slew so that the best epoch position will 
		   correspond to the 1ms epoch counter equal to zero. */

		CHPTR->_1MS_EPOCH_SLEW = 20 - CHPTR->BESTEPOCH;

		/* Restart the data bit integrations for epoch setting. */

		for(epoch_counter=0;epoch_counter<20;++epoch_counter)
		    CHPTR->DBINTEG[epoch_counter]=0;

		CHPTR->nDBINTEG=0;                 /* Integration counter. */
		CHPTR->bitlk=0;                            /* No bit lock. */
		CHPTR->FrameSync = FALSE;                /* No frame sync. */
		CHPTR->FrameSyncAndTIC = FALSE;
	    }

	    /* The best epoch position is now zero and the best sum the
	       current integration value. */

	    CHPTR->BESTEPOCH = 0;
	    CHPTR->BESTSUM = CHPTR->DBINTEG[_1ms_epoch];
	}
	else                             /* The 1ms epoch counter is != 0. */
	{
	    /* If the current integrated data bit value is better than the
	       current best then change the best epoch position and the sum
	       at this epoch. */

	    if(labs(CHPTR->DBINTEG[_1ms_epoch]) > CHPTR->BESTSUM)
	    {
		CHPTR->BESTEPOCH = _1ms_epoch;
		CHPTR->BESTSUM = CHPTR->DBINTEG[_1ms_epoch];
	    }
	}

	/* If the best epoch position has not changed for 1000 readings (1s)
	   then declare bit lock at this position. */
	
	if(CHPTR->nDBINTEG < 2000L)
	    CHPTR->nDBINTEG++;
	else
	    CHPTR->bitlk=TRUE;

    }                                           /* If 1ms epoch slew == 0. */
}

/****************************************************************************    
* Function: void FrameSync(register chanstruc _ds *CHPTR,
*                          unsigned _20ms_epoch)
*
* Performs sync to the message frame. Frame sync is declared when (i) the
* first 2 words pass the parity check, (ii) the preamble is detected,
* (iii) the subframe ID is valid and (iv) the zero bits are present. When the 
* above conditions are valid then the first 2 words of a frame have been
* received - the 20ms epoch counter at this point should 10 (last bit of
* word 2).
*
* Input: CHPTR - pointer to the channel structure for the SV concerned.
*        _20ms_epoch - the 20ms epoch count.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void FrameSync(register chanstruc _ds *CHPTR, unsigned _20ms_epoch)
{
    int preamble;              /* The TLM word preamble sequence 10001011. */
    int sid;                                  /* The subframe ID (1 to 5). */
    int zerobits;            /* The zero bits (the last 2 bits of word 2). */

    unsigned long word0,word1;

    word0 = CHPTR->wordbuff[FRAME_SIZE_PLUS_2-2];
    word1 = CHPTR->wordbuff[FRAME_SIZE_PLUS_2-1];

    /* Extract the preamble. */

    preamble = (int)(word0>>22)&0x00FF;

    /* Extract the subframe ID. */

    sid = (int)(word1>>8)&0x0007;

    /* Extract the zero bits. */

    zerobits = (int)word1&0x0003;

    /* Invert the extracted data according to bit 30 of the previous word. */

    if(word0&0x40000000L)
    {
	preamble ^= 0x00FF;
	zerobits ^= 0x0003;
    }
    if(word1&0x40000000L)
	sid ^= 0x0007;

    /* Check that the preamble, subframe and zero bits are ok. */

    if(preamble!=PREAMBLE)
	return;

    /* Check if the subframe ID is ok. */

    if(sid<1 || sid>5)
	return;

    /* Check that the zero bits are ok. */

    if(zerobits!=0)
	return;

    /* Check that the 2 most recently logged words pass parity. Have to first 
       invert the data bits according to bit 30 of the previous word. */

    if(word0&0x40000000L)
	word0 ^= 0x3FFFFFC0L;

    if(word1&0x40000000L)
	word1 ^= 0x3FFFFFC0L;

    if(ParityCheck(word0)==FALSE || ParityCheck(word1)==FALSE) 
	return;

    /* All the data matches up, therefore all that needs to be done is
       to set the 20ms epoch counter to its correct value. */

    if(_20ms_epoch!=10)
	CHPTR->_20MS_EPOCH_SLEW = 50 - ((40+_20ms_epoch)%50);
    else if(CHPTR->_20MS_EPOCH_SLEW==0)
    {
	CHPTR->FrameSync = TRUE;
	CHPTR->BitNumber = 0; 
    }
}

/****************************************************************************    
* Function: int ValidFrameFormat(register chanstruc _ds *CHPTR,
*                                unsigned long *subframe)
*
* Validates that the frame format is ok.
*
* Input: CHPTR - pointer to the channel structure for the SV concerned.
*        subframe - pointer to the subframe data.
*
* Output: None.
*
* Return Value: TRUE if format is good.
****************************************************************************/
int ValidFrameFormat(register chanstruc _ds *CHPTR, unsigned long *subframe)
{
    int preamble,next_preamble;
    int zerobits,next_zerobits;
    int sid,next_sid;
    int wordcounter;
    int parity_errors;

    for(wordcounter=0;wordcounter<FRAME_SIZE_PLUS_2;wordcounter++)
	subframe[wordcounter] = CHPTR->wordbuff[wordcounter];
    
    /* Extract the preamble. */

    preamble = (int)(subframe[0]>>22)&0x00FF;

    /* Extract the subframe ID. */

    sid = (int)(subframe[1]>>8)&0x0007;

    /* Extract the zero bits. */

    zerobits = (int)subframe[1]&0x0003;

    /* Invert the extracted data according to bit 30 of the previous word. */

    if(subframe[0]&0x40000000L)
    {
	preamble ^= 0x00FF;
	zerobits ^= 0x0003;
    }
    if(subframe[1]&0x40000000L)
	sid ^= 0x0007;

    /* Check that the preamble, subframe and zero bits are ok. */

    if(preamble!=PREAMBLE)
	return(FALSE);

    if(sid<1 || sid>5)
	return(FALSE);

    if(zerobits!=0)
	return(FALSE);

    /* Extract the next preamble (10001011). */

    next_preamble = (int)(subframe[FRAME_SIZE_PLUS_2-2]>>22)&0x00FF;
    
    /* Extract the next subframe ID. */

    next_sid = (int)(subframe[FRAME_SIZE_PLUS_2-1]>>8)&0x0007;

    /* Extract the next zero bits. */

    next_zerobits = (int)(subframe[FRAME_SIZE_PLUS_2-1]&0x0003);

    /* Invert the extracted data according to bit 30 of the previous word. */

    if(subframe[FRAME_SIZE_PLUS_2-2]&0x40000000L)
    {
	next_preamble ^= 0x00FF;
	next_zerobits ^= 0x0003;
    }
    if(subframe[FRAME_SIZE_PLUS_2-1]&0x40000000L)
	next_sid ^= 0x0007;

    /* Check that the preamble, subframe and zero bits are ok. */

    if(next_preamble!=PREAMBLE)       
	return(FALSE);

    if(next_sid<1 || next_sid>5)
	return(FALSE);

    if(next_zerobits!=0)
	return(FALSE);

    /* Check that the subframe IDs are consistent. */

    if((next_sid-sid)!=1 && (next_sid-sid)!=-4)
	return(FALSE);

    /* Check that all 12 words have correct parity. Have to first 
       invert the data bits according to bit 30 of the previous word. */

    parity_errors = 0;

    for(wordcounter=0;wordcounter<FRAME_SIZE_PLUS_2;wordcounter++)
    {
	if(subframe[wordcounter]&0x40000000L)
	    subframe[wordcounter] ^= 0x3FFFFFC0L;
    }
    for(wordcounter=0;wordcounter<FRAME_SIZE_PLUS_2;wordcounter++)
    {
	if(ParityCheck(subframe[wordcounter])==FALSE)
	    parity_errors++;
    }

    if(parity_errors!=0)
	return(FALSE);
    
    return(TRUE);
}

/****************************************************************************    
* Function: int ParityCheck(unsigned long gpsword)
*
* Checks the parity of a GPS data word.
*
* Input: gpsword - the word to be checked.
*
* Output: None.
*
* Return Value: TRUE if parity is good.
****************************************************************************/
int ParityCheck(unsigned long gpsword)
{
    unsigned long d1,d2,d3,d4,d5,d6,d7,t,parity;

    /* XOR as many bits in parallel as possible.  The magic constants pick
       up bits which are to be XOR'ed together to implement the GPS parity
       check algorithm described in ICD-GPS-200.  This avoids lengthy shift-
       and-xor loops. */

    d1 = gpsword & 0xFBFFBF00L;
    d2 = _lrotl(gpsword,1) & 0x07FFBF01L;
    d3 = _lrotl(gpsword,2) & 0xFC0F8100L;
    d4 = _lrotl(gpsword,3) & 0xF81FFE02L;
    d5 = _lrotl(gpsword,4) & 0xFC00000EL;
    d6 = _lrotl(gpsword,5) & 0x07F00001L;
    d7 = _lrotl(gpsword,6) & 0x00003000L;

    t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;

    /* Now XOR the 5 6-bit fields together to produce the 6-bit
       final result. */

    parity = t ^ _lrotl(t,6) ^ _lrotl(t,12) ^ _lrotl(t,18) ^ _lrotl(t,24);
    parity = parity & 0x3F;
    if(parity == (gpsword&0x3F))
	return(TRUE);
    else
	return(FALSE);
}

/****************************************************************************
*
* Function: void EmitSubframe(register chanstruc _ds *CHPTR,
*                             unsigned long *subframe,unsigned long dataTIC)
*
* Emits the valid subframe for processing by the subframe processing
* functions.
*
* Input: CHPTR - A pointer to the channel structure of the channel in
*                question. This data structure must reside in DGROUP.
*        subframe - Pointer to 12 subframe words.      
*        dataTIC - The TIC at the end of the last subframe.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void EmitSubframe(register chanstruc _ds *CHPTR, unsigned long *subframe,
		  unsigned long dataTIC)
{
    int i;

    /* Emit the current subframe (not including the TLM and HOW from the next
       subframe) if we have frame sync. */

    CHPTR->BadDataCntr = 0;       /* Reset errors-since-good-data counter. */

    /* Check to make sure that the subframe for this satellite hasn't
       already been sent because the satellite is being tracked on more than
       one channel. */

    if(dataTIC - TimeOfLastSubframe[CHPTR->SV] < 10)
	return;

    if(SfPending<MAXCHANNELS)
    {
      for(i=0; i<10; ++i)
	  sfbuff[csf].subframe[i] = subframe[i];
      sfbuff[csf].SV = CHPTR->SV;
      sfbuff[csf].dataTIC = dataTIC;
      if(++csf >= MAXCHANNELS)
	  csf=0;
      SfPending++;
      TimeOfLastSubframe[CHPTR->SV] = dataTIC;
    }
    else
    {
	/* No room in the buffer, increment the missed subframe count. */

	misssf++;
    }
}