rtcm.c

GPS导航定位程序

	case 9:    /* TYPE 9 MESSAGE - High Rate Differential Corrections. */
	    Parse9();
	    BEAC.data = 60;
	    TCBComment("Type 9 Msg");
	    break; 
    
	case 10:     /* TYPE 10 MESSAGE - P Code Differential Corrections. */
	    TCBComment("Type 10 Msg");
	    break;

	case 11:             /* TYPE 11 MESSAGE - C/A Code L2 Corrections. */
	    TCBComment("Type 11 Msg");
	    break;

	case 12:       /* TYPE 12 MESSAGE - Pseudolite Station Parameters. */
	    TCBComment("Type 12 Msg");
	    break;

	case 13:       /* TYPE 13 MESSAGE - Ground Transmitter Parameters. */
	    TCBComment("Type 13 Msg");
	    break;

	case 14:        /* TYPE 14 MESSAGE - Surveying Auxiliary Message. */
	    TCBComment("Type 14 Msg");
	    break;

	case 15:    /* TYPE 15 MESSAGE - Ionosphere (Troposphere) Message. */
	    TCBComment("Type 15 Msg");
	    break;

	case 16:                     /* TYPE 16 MESSAGE - Special Message. */
	    Parse16();
	    TCBComment("Type 16 Msg");
	    break;

	case 17:                   /* TYPE 17 MESSAGE - Ephemeris Almanac. */
	    TCBComment("Type 17 Msg");
	    break;

	default:
	    break;
    }

    BEAC.typ = frmtype;
    BEAC.frm++;
    if(BEAC.frm>999)
	BEAC.frm = 0;

    Checkpoint("Emit",2);
}

/****************************************************************************    
* Function: void Rtcmproc(int bit)
*
* The bit is shifted into a 62-bit buffer. If frame sync has already been
* achieved, 30-bit RTCM words are collected into a buffer for processing at
* the end of the frame. Otherwise, the 62-bit buffer is examined to see
* whether it is the start of a frame. Frame sync is declared when certain
* consistency checks are passed. Frame sync can be lost if message headers
* stop passing the consistency checks.
*
* Input: bit - The data bit.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
int mtype,stid,snum,shlth,zcount,preamb,SpurDelLimit;
unsigned long cword1,cword2;

void Rtcmproc(int bit)
{
  
    Checkpoint("Rtcmproc",1);

    /* Shift the new bit into the two-word assembly area and increment the
       bit counter.  Word2 contains the newest bits. */

    word1 = (word1<<1) + ((word2&0x20000000UL)? 1:0);
    word2 = (word2<<1) + bit;
    nword++;
    if(nword>62)                          /* Buffer contains only 62 bits. */
	nword = 62;        

    /* The following option should only be enabled if your DGPS reference
       station inserts spurious pad characters between RTCM messages. */

#if (SpuriousPadChars==1)
    if(BEAC.sync && dwrdxpct==0 && nword==10 && SpurDelLimit)
    {
	/* Recognize and delete up to 100 spurious pad characters between
	   RTCM-104 messages. */

	if((word2&0x1FF)!=0x066 && (word2&0x1FF)!=0x199)
	{
	    SpurDelLimit--;
	    word2 = word2>>8;
	    nword = 2;
	}
    }
#endif

    /* If we're synchronized and looking for a data word, process word2
       as soon as we have 32 bits. */

    if(BEAC.sync && dwrdxpct && nword==32)
    {
	/* Process a new data word. */

	if(Parch(word2))
	{
	    /* The new word passed the parity check.  Store it and reduce
	       the bit count. */

	    dwrdxpct--;                /* Decrement # expected data words. */
	    dwd[ndwd] = Cmaybe(word2);
	    ndwd++; 
	    if(ndwd>=50)
		ndwd=49;                 /* Prevent input buffer overflow. */
	    nword = 2;               /* Leave d29* and d30* in the buffer. */

#if (RTCMDecodingRule==0)
	    /* Emit message frames as soon as they are received. */

	    if(dwrdxpct==0)
	    {
		frmtype = mtype;
		BEAC.id = frstid = stid;
		BEAC.hlth = frshlth = shlth;
		frzcount = zcount;
		psnum = frsnum;
		frsnum = snum;
		Emit();
		ndwd = 0;
	    }
#endif

	    return;
	}
	else
	{
	    /* The new word failed the parity check. We aren't synchronized
	       anymore. Throw out any partially collected frame data and
	       immediately start looking for a new header. */

	    if(BEAC.sync)
		BEAC.sync=0;

#if (SpuriousPadChars==1)
	    SpurDelLimit = 0;            /* No spurious chars allowed now. */
#endif

	    frsnum = -1;                /* Invalidate previous frame data. */
	    psnum = -1;            /* Invalidate next previous frame data. */
	    frpreamb = 0;
	    ppreamb = 0;
	    dwrdxpct = 0;
	    nword = 2;               /* Leave d29* and d30* in the buffer. */
	    ndwd = 0;
	    return;
	}
    }

    /* When there are 62 bits in words 1 and 2, check for a valid header.
       If it isn't found, we aren't synchronized. */

    if(nword==62)
    {
	if(hdrchk(word1,word2))
	{
	    /* Emit the previously collected frame data if its sequence
	       number is consistent with the current frame header and the
	       previous frame header. */

	    if(BEAC.sync==0)
		BEAC.sync=1;
	    cword1 = Cmaybe(word1);          /* d30*=1: complement d1-d24. */
	    cword2 = Cmaybe(word2);
	    preamb = (int)((cword1>>22) & 0xFF);
	    snum = (int)((cword2>>14) & 0x7);          /* Sequence number. */
	    frlen = dwrdxpct = (int)((cword2>>9) & 0x1F); /* # data words. */
	    mtype = (int)((cword1>>16) & 0x3F);           /* Message type. */
	    stid = (int)((cword1>>6) & 0x3FF);              /* Station ID. */
	    shlth = (int)((cword2>>6) & 0x7);           /* Station health. */
	    zcount = (int)((cword2>>17) & 0x1FFF);    /* Modified Z-count. */
	    nword = 2;               /* Leave d29* and d30* in the buffer. */

#if (SpuriousPadChars==1)
	    SpurDelLimit = 100; /* Max spur chars allowed between headers. */
#endif

	    /* The following applies when you have selected enforcement of
	       the RTCM-104 "Complete Message Decoding Rule" by setting
	       RTCMDecodingRule=1.  */

#if (RTCMDecodingRule==1)
	    if(frsnum>=0 && psnum>=0 && snum==((frsnum+1)%8) 
							 && preamb==frpreamb)
	    {
		Emit();                       /* Emit previous frame data. */
		ndwd = 0;
		frmtype = mtype;
		BEAC.id = frstid = stid;
		BEAC.hlth = frshlth = shlth;
		frzcount = zcount;
		frlen = dwrdxpct;
		psnum = frsnum;
		frsnum = snum;
	    }
	    else
	    {

		/* Sequence numbers aren't incrementing as expected, or the
		   sense (inverted/noninverted bits) changed. */
	
		ndwd = 0;
		frmtype = mtype;
		frstid = stid;
		frshlth = shlth;
		frzcount = zcount;
		frlen = dwrdxpct;
		psnum = frsnum;
		frsnum = snum;
		ppreamb = frpreamb;
		frpreamb = preamb;
	    }
#endif

	    /* The following applies when you have not selected enforcement
	       of the RTCM-104 "Complete Message Decoding Rule".  In this
	       case you want to emit messages as soon as they are received,
	       without waiting to decode the header of the next message.
	       Messages consisting of headers only (ex: Type 6) are emitted
	       here. */

#if (RTCMDecodingRule==0)
	    if(dwrdxpct==0)
	    {
		/* Emit header-only message frames as soon as they are
		   received. */

		if(dwrdxpct==0)
		{
		    frmtype = mtype;
		    BEAC.id = frstid = stid;
		    BEAC.hlth = frshlth = shlth;
		    frzcount = zcount;
		    psnum = frsnum;
		    frsnum = snum;
		    Emit();
		    ndwd = 0;
		}
	    }
#endif

	}
	else
	{
	    /* The header isn't valid.  Throw out partially collected frame
	       data. */

	    if(BEAC.sync)
		BEAC.sync=0;

#if (SpuriousPadChars==1)
	    SpurDelLimit = 0;            /* No spurious chars allowed now. */
#endif

	    ndwd = 0;
	    frsnum = -1;                /* Invalidate previous frame data. */
	    frpreamb = 0;
	    psnum = -1;            /* Invalidate next previous frame data. */
	    ppreamb = 0;
	    return;
	}
    }

   return;
}

/****************************************************************************    
* Function: void RTCM(int newbyte)
*
* Any received byte resets the beacon signal presence down-counter, so that
* signal presence will be indicated for at least 30 more seconds. Beacon
* receiver status messages are recognized and parsed. RTCM-104 6-of-8 message 
* bytes are unpacked into 6 bits. The subroutine rtcmproc is called six
* times, so as to process the RTCM-104 data stream one bit at a time.
*
* Input: newbyte - the new byte.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void RTCM(int newbyte)
{
    Checkpoint("RTCM",1);

    BEAC.sig=30;
    newbyte = newbyte & 0xFF;

    /* Pick up Request Configuration and Request Status messages that may
       already have been initiated. */

    if(BEAC.ReqCfgCt)
    {
	PROTECT++;
	BEAC.ReqCfgMsg[6-BEAC.ReqCfgCt] = newbyte;
	if(--BEAC.ReqCfgCt==0)
	{
	    BEAC.RptFreq = ( ((BEAC.ReqCfgMsg[0]&0x3F)<<6) +
					   (BEAC.ReqCfgMsg[1]&0x3F) ) / 10.0;
	    BEAC.RptBrate = 0;
	    if(BEAC.ReqCfgMsg[2]==0xD4)
		BEAC.RptBrate=25;
	    else if(BEAC.ReqCfgMsg[2]==0xD5)
		BEAC.RptBrate=50;
	    else if(BEAC.ReqCfgMsg[2]==0xD6)
		BEAC.RptBrate=100;
	    else if(BEAC.ReqCfgMsg[2]==0xD7)
		BEAC.RptBrate=200;
	}
	PROTECT--;
	return;
    }

    if(BEAC.ReqStatCt)
    {
	PROTECT++;
	BEAC.ReqStatMsg[3-BEAC.ReqStatCt] = newbyte;
	if(--BEAC.ReqStatCt==0)
	{
	    BEAC.RptSS = ((BEAC.ReqStatMsg[0]&0x3F)<<6)
						 + (BEAC.ReqStatMsg[1]&0x3F);
	    BEAC.RptSNR = BEAC.ReqStatMsg[2]&0x3F;
	}
	PROTECT--;
	return;
    }

    if(newbyte==0xC1)
    {
    BEAC.ReqCfgCt=6;   /* The next 6 bytes are Request Configuration info. */
    return;
    }

    if(newbyte==0xC5)
    {
	BEAC.ReqStatCt=3;     /* The next 3 bytes are Request Status info. */
	return;
    }

    if(newbyte<64 || newbyte>127)            /* Not an RTCM-104 data byte. */
	return; 

    Rtcmproc(newbyte&1);
    Rtcmproc((newbyte>>1)&1);
    Rtcmproc((newbyte>>2)&1);
    Rtcmproc((newbyte>>3)&1);
    Rtcmproc((newbyte>>4)&1);
    Rtcmproc((newbyte>>5)&1);

    Checkpoint("RTCM",2);
}