rtcm.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Task to parse RTCM-104 Version 2.0 differential corrections. These
* real-time pseudorange corrections may be obtained from an external DGPS
* beacon receiver.  Note: A revised RTCM specification is under development,
* which may significantly affect the required RTCM-104 message decoding.
****************************************************************************/

/* Two-word RTCM-104 assembly area. Each word contains d29*, d30*, and
   the 30 data bits. */

static unsigned long word1;                                     /* Word 1. */
static unsigned long word2;                                     /* Word 2. */

/* # bits in the two-word RTCM-104 assembly area. Don't count d29* and
   d30* of the second word twice. */

static int nword;                                                 /* 0-62. */

static double BEACX,BEACY,BEACZ;      /* Beacon position, XYZ coordinates. */

static double BEACT[3][3];

/* Number of RTCM-104 data words (non-header words) expected to follow
   before the next header. */

static int dwrdxpct;

/* Preamble and sequence number of the previous frame. */

static int ppreamb;        /* Preamble, to determine inverted/noninverted. */
static int psnum;                           /* Sequence number of message. */

/* Collection area for the RTCM-104 frame data. */

static int frpreamb;       /* Preamble, to determine inverted/noninverte.d */
static int frsnum=-1;                       /* Sequence number of message. */
static int frmtype;                                       /* Message type. */
static int frstid;                                          /* Station ID. */
static int frshlth;                                     /* Station health. */
static int frzcount;                                  /* Modified Z-count. */
static int frlen;                          /* Message length (data words). */
static int ndwd;        /* Current number of data words, excluding header. */

/* Frame data words, stored in uncomplemented form. */

static unsigned long dwd[50];

/****************************************************************************    
* Function: void TRTCM(void)
*
* Every second, monitor for presence of signal and data. Every 4 seconds,
* interrogate the external beacon receiver for its status.  Every 0.2
* seconds, take RTCM-104 characters from the serial interface port opened by
* a DC n command and call RTCM to process them.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void TRTCM(void)
{
    int i,gwk;
    unsigned long ctic,BeacStatTimer,OneSecTimer;
    double gsec,dt;

    CurrentTIC(&BeacStatTimer);
    OneSecTimer = 0UL;

    while(TRUE)
    {
	CurrentTIC(&ctic);

	/* Every 10 TICs, decrement status counters. When these counters
	   go to zero, it means that either signal or beacon data has been
	   absent for 60 seconds. Wipe out differential corrections if there
	   has been no data for 60 seconds, and also wipe out any correction
	   that is more than 120 seconds old. */

	if(ctic >= OneSecTimer)
	{
	    OneSecTimer += 10;
	    if(ctic >= OneSecTimer)
		OneSecTimer = ctic + 10;
      
	    TICToGpsTime(ctic,&gwk,&gsec);   /* Get current time estimate. */
	    PROTECT++;

	    /* Wipe out differential corrections more than
	       120 seconds old. */

	    for(i=0;i<MAXSATELLITES;i++)
	    {
		if(BEAC.DC[i].vflg==FALSE)
		    continue;
		dt = MZDifference(gsec,BEAC.DC[i].mzcount);
		if(fabs(dt) > 120.0)
		    BEAC.DC[i].vflg = FALSE;
	    }

	    for(i=0;i<MAXSATELLITES;i++)
	    {
		if(BEAC.DDC[i].vflg==FALSE)
		    continue;
		dt = MZDifference(gsec,BEAC.DDC[i].mzcount);
		if(fabs(dt) > 120.0)
		    BEAC.DDC[i].vflg = FALSE;
	    }

	    if(BEAC.data)
	    {
		BEAC.data--;
		if(BEAC.data==FALSE)
		{
		    /* No data for 60 seconds -- wipe out all corrections. */

		    for(i=0;i<MAXSATELLITES;i++)
			BEAC.DC[i].vflg = BEAC.DDC[i].vflg = FALSE;
		}
	    }

	    if(BEAC.sig)
		BEAC.sig--;
	    if(BEAC.sig && BEAC.data==FALSE)
		TCBComment("No RTCM frames");
	    if(BEAC.sig==FALSE)
		TCBComment("No beacon rcvr");
	    PROTECT--;
	}

	/* Every 40 TICs, interrogate the external beacon receiver
	   for status (if one is in use). */

       if(ctic >= BeacStatTimer)
       {
	   BeacStatTimer += 40;
	   if(ctic >= BeacStatTimer)
	       BeacStatTimer = ctic + 40;
	   if(BeacPort)
	   {
	       WriteSerial(0xC1);                /* Request configuration. */
	       WriteSerial(0xC5);                       /* Request status. */
	    }
	}

	/* Process serial input characters from the external beacon
	   receiver. */

	if(InputCharsReady())
	    while(InputCharsReady())
		RTCM(ReadSerial());

	Suspend(2);
    }
}

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

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

    /* Get current time estimate. */

    CurrentTIC(&ctic);
    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;
    PROTECT++;
    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++];
	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 Parse2(void)
*
* Decode a Type 2 frame and emit the data to the global data structure BEAC.
* Type 2 is the delta-differential corrections for a previous IODE.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Parse2(void)
{
    unsigned utemp[128];                      /* Room to parse 32 entries. */
    int i,j,nentries,sf,svid,iod,drrcbits;
    unsigned dprcbits;
    unsigned long w;
    double dprc,drrc;

    Checkpoint("Parse2",1);

    /* 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;
    PROTECT++;
    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;
	dprcbits = ((utemp[j]<<8)+utemp[j+1]) & 0xFFFF;
	dprc = SignExtend(dprcbits,15) * (sf? 0.32: 0.02);
	j++;
	j++;
	drrcbits = utemp[j++];
	drrc = SignExtend(drrcbits,7) * (sf? 0.032: 0.002);
	iod = utemp[j++];
	BEAC.DDC[svid-1].dprc = dprc;
	BEAC.DDC[svid-1].drrc = drrc;
	BEAC.DDC[svid-1].iod = iod;
	BEAC.DDC[svid-1].mzcount = frzcount;
	BEAC.DDC[svid-1].dontuse = (dprcbits==0x8000) || (drrcbits==0x80);
	BEAC.DDC[svid-1].vflg = TRUE;
    }
    PROTECT--;
}

/****************************************************************************    
* Function: void Parse3(void)
*
* Decode a Type 3 frame and emit the data to the global data structure BEAC.
* Type 3 is the WGS-84 XYZ position of the beacon's differential reference
* station.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Parse3(void)
{
    long lw;
    double x,y,z,rvec,blat,blon,bhgt;

    Checkpoint("Parse3",1);

    lw = ((dwd[0]<<2)&0xFFFFFF00UL) + ((dwd[1]>>22)&0xFFUL);
    x = lw * 0.01;
    lw = ((dwd[1]<<10)&0xFFFF0000UL) + ((dwd[2]>>14)&0xFFFFUL);
    y = lw * 0.01;
    lw = ((dwd[2]<<18)&0xFF000000UL) + ((dwd[3]>>6)&0xFFFFFFUL);
    z = lw * 0.01;

    rvec = x*x + y*y + z*z;             /* Check validity of beacon's XYZ. */
    if(rvec>1.0)
	rvec = sqrt(rvec);
    else
       rvec = 0.0;
    if(fabs(rvec-REARTH) > 20000.0)
    {
	BEAC.pos=0; /* Beacon's XYZ is invalid, not within 20 km of Earth. */
	return;
    }
    else
    {
	XYZToLatLonHgt(x,y,z,&blat,&blon,&bhgt);

	/* Compute the matrix to transform XYZ vectors to NEU (North-East-Up)
	   vectors at the reference station position. */

	PROTECT++;
	LatLonToNorthEastUp(blat,blon,BEACT);
	BEACX = x;
	BEACY = y;
	BEACZ = z;
	BEAC.lat = blat;
	BEAC.lon = blon;