rtcm.c

GPS导航定位程序

	BEAC.hgt = bhgt;
	BEAC.pos = 1;
	PROTECT--;
    }
}

/****************************************************************************    
* Function: void Parse7(void)
*
* Decode a Type 7 frame and emit the data to the global data structure BEAC.
* Type 7 is the beacon almanac message.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Parse7(void)
{
    int nbeacs,i;
    unsigned freq6msb,freq6lsb,freqdiv100,brcode,health,brate;
    double freqHz;

    Checkpoint("Parse7",1);

    if(frlen%3)                              /* Incorrect # of data words. */
	return;
  
    nbeacs = frlen/3;
    if(nbeacs)
	for(i=0;i<nbeacs;i++)
	{
	    freq6msb = (unsigned)((dwd[3*i+1]>>6)&0x3F);
	    freq6lsb = (unsigned)((dwd[3*i+2]>>24)&0x3F);
	    freqdiv100 = (freq6msb<<6) + freq6lsb;
	    freqHz = freqdiv100 * 100.0;
	    health = (unsigned)((dwd[3*i+2]>>22) & 3);
	    brcode = (unsigned)((dwd[3*i+2]>>9) & 7);
	    if(health==3)                 /* Health indicates: do not use. */
		continue;
	    brate = 0;                                         /* Unknown. */
	    if(brcode==0)
		brate=25;
	    if(brcode==1)
		brate=50;
	    if(brcode==2)
		brate=100;
	    if(brcode==5)
		brate=200;
	    if(brate)
		BDENTER(freqHz,brate);     /* Enter into beacon directory. */
      }
}

/****************************************************************************    
* Function: void Parse9(void)
*
* Decode a Type 9 frame and emit the data to the global data structure BEAC.
* Type 9 is a high rate corrections message.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Parse9(void)
{
    unsigned utemp[128];                      /* Room to parse 32 entries. */
    int i,j,nentries,sf,svid,iod,rrcbits,gwk;
    unsigned prcbits;
    unsigned long ctic,w;
    double prc,rrc,gsec;

#if (EnableDiffPropCorrections==1)
    double elv,azi,dopp,erc,iono,tropo;
#endif

    Checkpoint("Parse9",1);

  
    CurrentTIC(&ctic);                       /* Get current time estimate. */
    TICToGpsTime(ctic,&gwk,&gsec);

    /* Break the message up into its component bytes. */

    j = 0;
    for(i=0;i<frlen;i++)
    {
	w = dwd[i];
	utemp[j++] = (unsigned)((w>>22) & 0xFF);
	utemp[j++] = (unsigned)((w>>14) & 0xFF);
	utemp[j++] = (unsigned)((w>>6) & 0xFF);
    }

    nentries = (3*frlen) / 5;   /* How many SV entries are in the message. */
    if(nentries>MAXSATELLITES)
	return;

    j = 0;
    for(i=0;i<nentries;i++)
    {
	/* Parse the entry for each satellite. */

	sf = (utemp[j]>>7) & 1;
	svid = utemp[j++] & 0x1F;
	if(svid==0)
	    svid=MAXSATELLITES;
	prcbits = ((utemp[j]<<8)+utemp[j+1]) & 0xFFFF;
	prc = SignExtend(prcbits,15) * (sf? 0.32: 0.02);
	j++; j++;
	rrcbits = utemp[j++];
	rrc = SignExtend(rrcbits,7) * (sf? 0.032: 0.002);
	iod = utemp[j++];

	/* Update the differential corrections for the satellites contained
	   in the high-rate message. */

	PROTECT++;
	BEAC.DC[svid-1].prc = prc;
	BEAC.DC[svid-1].rrc = rrc;
	BEAC.DC[svid-1].iod = iod;
	BEAC.DC[svid-1].mzcount = frzcount;
	BEAC.DC[svid-1].dontuse = (prcbits==0x8000) || (rrcbits==0x80);

#if (EnableDiffPropCorrections==1)
	if(BEAC.pos && Ephs[svid-1].vflg && ((Ephs[svid-1].s1hlth&0x20)==0))
	{
	    /* Compute differential atmospheric corrections if ref station
	       position is known, we have an ephemeris for the satellite in
	       question, and the satellite is healthy. */

	    svelv(svid,gwk,gsec,BEACT,BEACX,BEACY,BEACZ,BEAC.lat,BEAC.lon,
		  BEAC.hgt,&elv,&azi,&dopp,&erc,&iono,&tropo);
	    BEAC.DC[svid-1].DiffPropCorr = sviono[svid-1] - iono
					   + svtropo[svid-1] - tropo;
	}
	else
	{
	    /* Otherwise the differential atmospheric corrections are 0.0.
	       If you never want to use differential atmospheric corrections
	       (they are minor and very computationally expensive) just make
	       sure the following quantity is always set to 0.0. */

	    BEAC.DC[svid-1].DiffPropCorr = 0.0;
	}
#endif
    
	BEAC.DC[svid-1].vflg = TRUE;
	PROTECT--;
    }
}

/****************************************************************************    
* Function: void Parse9(void)
*
* Decode a Type 9 frame and emit the data to the global data structure BEAC.
* Type 16 is a text message.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Parse16(void)
{
    int i,j;
    unsigned long w;

    Checkpoint("Parse16",1);

    j = 0;
    PROTECT++;
    for(i=0;i<frlen;i++)
    {
	w = dwd[i];
	BEAC.msg[j++] = (char)((w>>22) & 0xFF);
	BEAC.msg[j++] = (char)((w>>14) & 0xFF);
	BEAC.msg[j++] = (char)((w>>6) & 0xFF);
	if(j>=90)
	    break;
    }
    while(j<90)
	BEAC.msg[j++] = ' ';
    for(j=0;j<90;j++)
	if(BEAC.msg[j]==0)
	    BEAC.msg[j] = ' ';
    BEAC.msg[90] = 0;
    PROTECT--;
}

/****************************************************************************    
* Function: void BDENTER(double freqHz, unsigned brate)
*
* The GPS designer should maintain a directory of beacon stations if he or
* she wishes to automatically control beacon station selection.
*
* Input: freqHz - Beacon frequency in Hz.
*        brate - beacon bit rate in bits/second.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
#pragma warn -par
void BDENTER(double freqHz, unsigned brate)
{
}
#pragma warn .par

/****************************************************************************    
* Function: unsigned long Cmaybe(unsigned long w)
*
* Complement the bits according to d30*.
*
* Input: w - the data word.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
unsigned long Cmaybe(unsigned long w)
{
    if(w&0x40000000UL)
	w = w ^ 0x3FFFFFC0UL;

    return(w);
}

/****************************************************************************    
* Function: int odd(unsigned long w)
*
* Odd or even number of 1's?
*
* Input: w - the data word.
*
* Output: None.
*
* Return Value: TRUE if odd number of 1's.
****************************************************************************/
int odd(unsigned long w)
{
    int i,oddeven;

    oddeven = 0;

    for(i=0;i<32;i++)
    {
	oddeven = oddeven ^ ((int)w&1);
	w = w>>1;
    }
    
    return(oddeven);
}

/****************************************************************************    
* Function: int Parch(unsigned long w)
*
* Perform parity check on data word.
*
* Input: w - the data word.
*
* Output: None.
*
* Return Value: TRUE if parity is good.
****************************************************************************/
int Parch(unsigned long w)
{
    int D25,D26,D27,D28,D29,D30,par;

    w = Cmaybe(w);
    D25 = odd(w&0x0BB1F3480UL);
    D26 = odd(w&0x05D8F9A40UL);
    D27 = odd(w&0x0AEC7CD00UL);
    D28 = odd(w&0x05763E680UL);
    D29 = odd(w&0x06BB1F340UL);
    D30 = odd(w&0x08B7A89C0UL);
    par = ((D25<<5) + (D26<<4) + (D27<<3) + (D28<<2) + (D29<<1) + D30);
    return (par==(w&0x3F));
}

/****************************************************************************    
* Function: int Hdrchk(unsigned long w1, unsigned long w2)
*
* Check for correct header.
*
* Input: w1, w2 - last 2 data words.
*
* Output: None.
*
* Return Value: TRUE if header found.
****************************************************************************/
int hdrchk(unsigned long w1, unsigned long w2)
{
    unsigned long d30pre;

    d30pre = w1&0x7FC00000UL;           /* d30* and 8-bit preamble fields. */
    if(d30pre!=0x19800000UL && d30pre!=0x66400000UL)
	return(FALSE);
    if(Parch(w1)==0)                              /* Word 1 failed parity. */ 
	return(FALSE);                    
    if(Parch(w2)==0)                              /* Word 2 failed parity. */ 
	return(FALSE);                    
    return(TRUE);
}

/****************************************************************************    
* Function: void Emit(void)
*
* An RTCM-104 message frame has been assembled in the frame buffer and the
* next frame's header has been recognized. The frame must now be parsed
* according to its message type, and the resulting data is to be emitted into 
* global shared data structured, primarily for use by Task MAIN, module NAV,
* procedure omp.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Emit(void)
{
    Checkpoint("Emit",1);

    switch(frmtype)
    {
	case 1:          /* TYPE 1 MESSAGE - Differential GPS Corrections. */
	    Parse1();
	    BEAC.data = 60;
	    TCBComment("Type 1 Msg");
	    break;

	case 2:    /* TYPE 2 MESSAGE - Delta Differential GPS Corrections. */
	    Parse2();
	    BEAC.data = 60;
	    TCBComment("Type 2 Msg");
	    break;
 
	case 3:          /* TYPE 3 MESSAGE - Reference Station Parameters. */
	    Parse3();
	    BEAC.data = 60;
	    TCBComment("Type 3 Msg");
	    break;

	case 4:                  /* TYPE 4 MESSAGE - Surveying Parameters. */
	    BEAC.data = 60;
	    TCBComment("Type 4 Msg");
	    break;

	case 5:                  /* TYPE 5 MESSAGE - Constellation Health. */
	    BEAC.data = 60;
	    TCBComment("Type 5 Msg");
	    break;

	case 6:                            /* TYPE 6 MESSAGE - Null Frame. */
	    TCBComment("Type 6 Msg");
	    break;

	case 7:                   /* TYPE 7 MESSAGE - Radiobeacon Almanac. */
	    BEAC.data = 60;
	    TCBComment("Type 7 Msg");
	    break;

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