procsbf.c

GPS导航定位程序

#include "includes.h"

/* This is the order in which the subframe 4 page ID's are subcommutated. */

int pg2svid4[] = {57,25,26,27,28,57,29,30,31,32,57,62,52,53,54,57,55,56,58,
		  59,57,60,61,62,63};

/* This is the order in which the subframe 5 page ID's are subcommutated. */

int pg2svid5[] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,
		  23,24,51};

struct
{
    int vflg1;                                   /* 1=valid data for SF 1. */
    int vflg2;                                   /* 1=valid data for SF 2. */
    int vflg3;                                   /* 1=valid data for SF 3. */
    int iodc;                         /* Issue of SF 1 data in SF1words[]. */
    int iode2;                        /* Issue of SF 2 data in SF2words[]. */
    int iode3;                        /* Issue of SF # data in SF3words[]. */
    unsigned long SF1words[10];            /* Unparsed SF 1 message words. */
    unsigned long SF2words[10];            /* Unparsed SF 2 message words. */
    unsigned long SF3words[10];            /* Unparsed SF 3 message words. */
} newsf123[32];

/****************************************************************************    
* Function: void TProcSbf(void)
*
* Activated at 1 second intervals. Retrieves GPS navigation message subframes 
* from the subframe queue, process them, and suspends awaiting reactivation.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void TProcSbf(void)
{
    while(TRUE)
    {
	if(SfPending==0)
	{
	    /* No subframes to process.  Check back in 1 second. */

	    Suspend(10);
	}
	else
	{
	    if(SfPending > MaxSfPending)
	    MaxSfPending = SfPending;
	    ProcessSubframe(sfbuff[psf].SV,sfbuff[psf].subframe,
			    sfbuff[psf].dataTIC);
	    psf = (psf+1) % MAXCHANNELS;  SfPending--;
	}
    }
}

/****************************************************************************    
* Function: void ProcessSubframe(int sv, unsigned long *g,
*                                unsigned long dataTIC)
*
* Processes GPS data subfarmes. Each 30-bit GPS word is packed into the 30
* LSB's of each 32-bit longword, and the two MSB's are don't-cares.
*
* Input: sv - the SV concerned.
*        g - pointer to the subframe buffer to be processed. 
*        dataTIC - TIC value for subframe end.
*
* Output: Update global data.
*
* Return Value: None.
****************************************************************************/
void ProcessSubframe(int sv, unsigned long *g, unsigned long dataTIC)
{
    char buff[90],spmsg[23];

    int i;
    int sid;
    int wkn;
    int almhlth;
    int pageid;
    int almsv;
    int dtls;
    int wnt;
    int wnlsf;
    int dn;
    int dtlsf;
    int chlth[32];
    int gwk;
    int refweek;
    int HaveEph123;
    int NoOldData;
    int NewDataSet;
    int SVDataOK;
    int DiffWeeks;
    int DiffDays;

    unsigned long tow;
    unsigned long ctic;
    unsigned long tot;
    unsigned long *u;

    float toa;
    float ainclin;
    float arora;
    float aratoa;
    float aaf0;
    float aaf1;
    float asqrta;
    float aecc;
    float aargpg;
    float amanom;

    double a;
    double alpha0;
    double alpha1;
    double alpha2;
    double alpha3;
    double beta0;
    double beta1;
    double beta2;
    double beta3;
    double A0;
    double A1;
    double gsec;
    double dtemp;
    double DiffSecs;
    double DiffTime;

    ephstruc *e;

    Checkpoint("ProcessSubframe",1);

    /* The constants used in this procedure are, unless otherwise noted,
       scale factors to convert from units used in the GPS navigation
       message subframes to units used in the GPS Builder's data base.
       See ICD-GPS-200 for a specification of GPS transmitted data. */

    sid = (int)(g[1]>>8)&7;
    tow = (g[1]>>13)&0x1FFFFL;
    SVDataOK = ephs[sv-1].vflg && ((ephs[sv-1].s1hlth&0x20)==0);

    /* If the receiver's estimate of GPS time is not within one second of
       the time implied by the subframe, update the RCO estimate. */

    PROTECT++;
    if(SVDataOK && CurClkModel.RCOtic)
    {
	TICToGpsTime(dataTIC,&gwk,&gsec);
	dtemp = tow*6.0 - gsec;
	if(dtemp<-302400.0)
	    dtemp += SECONDS_IN_WEEK;
	else if(dtemp>302400.0)
	    dtemp -= SECONDS_IN_WEEK;
	if(dtemp>0.5 || dtemp<-0.5)
	{
	    if(dtemp>0.5)
		CurClkModel.RCOsec -= 1.0;
	    else
		CurClkModel.RCOsec += 1.0;
	}
    }
    PROTECT--;

    /* Report the last subframe processed for this SV. */
    
    lastsf[sv-1] = sid;

    switch(sid)
    {
	case 1:
	    newsf123[sv-1].iodc = ((g[2]<<2)&0x300) + ((g[7]>>22)&0xFF);
	    for(i=0;i<10;i++)
		newsf123[sv-1].SF1words[i] = g[i];
	    newsf123[sv-1].vflg1 = 1;
	    SVDataOK = ((g[2]>>8)&0x20) == 0;      /* Updated health code. */
	    wkn = (g[2]>>20)&0x3FF;                /* Updated week number. */

	    /* Note: the week number transmitted by the SV is modulo 1024.
	       Unwrap it by requiring that the week number be within 512
	       weeks of the PC's clock. */

	    while(wkn-CurClkModel.Zwk<-512)
		wkn += 1024;

	    PROTECT++;
	    if(SVDataOK && CurClkModel.RCOtic==0)
	    {
		/* 0.075 is the typical value for SV signal path delay. */

		DiffTime = fabs((CurClkModel.Zwk - wkn)*SECONDS_IN_WEEK
			   + CurClkModel.Zsec - (tow*6.0+0.075));
		if(DiffTime>300.0)
		{
		    /* If user's original time estimate was wrong by more
		       than 5 minutes. */

		    DiffWeeks = floor(DiffTime/SECONDS_IN_WEEK);
		    DiffSecs = DiffTime - SECONDS_IN_WEEK*DiffWeeks;
		    DiffDays = floor(DiffSecs/SECONDS_IN_DAY);
		    DiffSecs -= DiffDays*SECONDS_IN_DAY;
		    sprintf(buff,"WARNING: PC clock/time zone wrong by "
				 "%d weeks %d days %9.1lf secs",
				 DiffWeeks,DiffDays,DiffSecs);
		    WarningMessage(buff);
		}

		CurClkModel.RCOwk = CurClkModel.Zwk - wkn;
		CurClkModel.RCOsec = CurClkModel.Zsec + dataTIC*TIC_PERIOD
				     - (tow*6.0+0.075);
		while(CurClkModel.RCOsec>=SECONDS_IN_WEEK)
		{
		    CurClkModel.RCOsec-=SECONDS_IN_WEEK;
		    CurClkModel.RCOwk++;
		}
		while(CurClkModel.RCOsec<0.0)
		{
		    CurClkModel.RCOsec+=SECONDS_IN_WEEK;
		    CurClkModel.RCOwk--;
		}
		CurClkModel.RCOtic = dataTIC;
	    }                    /* if(SVDataOK && CurClkModel.RCOtic==0). */
	    PROTECT--;

	    sprintf(buff,"SV %2d SF 1",sv);
	    TCBComment(buff);
	    break;

	case 2:
	    newsf123[sv-1].iode2 = (g[2]>>22)&0xFF;
	    for(i=0;i<10;i++)
		newsf123[sv-1].SF2words[i] = g[i];
	    newsf123[sv-1].vflg2 = 1;
	    sprintf(buff,"SV %2d SF 2",sv);
	    TCBComment(buff);
	    break;

	case 3:
	    newsf123[sv-1].iode3 = (g[9]>>22)&0xFF;
	    for(i=0; i<10; ++i)
		newsf123[sv-1].SF3words[i] = g[i];
	    newsf123[sv-1].vflg3 = 1;
	    sprintf(buff,"SV %2d SF 3",sv);
	    TCBComment(buff);
	    break;

	case 4:
	    if(SVDataOK==0)
		break;
	    pageid = (g[2]>>22)&0x3F;
	    if(pageid==0)  /* There is no almanac in the slot for some SV. */
	    {
		almsv = pg2svid4[((tow-4)/5)%25];
		if(almsv>0 && almsv<=32)
		{
		    /* No almanac for almsv (pageid is 0, nonexistent SV). */

		    alms[almsv-1].vflg = 2;
		    LastAlm = almsv;
		}
	    }
	    else if(pageid>=25 && pageid<=32)       /* This is an almanac. */
	    {
		almsv = pageid;
		LastAlm = almsv;
		aecc = (unsigned long)((g[2]>>6)&0xFFFF)*4.768371582e-7;
		toa = (unsigned long)((g[3]>>22)&0xFF)*4096.0;
		ainclin = (0.3 + SignExtend(g[3]>>6,15)*1.907348633e-6)*PI;
		arora = SignExtend(g[4]>>14,15)*3.637978807e-12*PI;
		asqrta = (unsigned long)((g[5]>>6)&0xFFFFFFL)*4.8828125e-4;
		if(asqrta==0.0)
		{
		    WarningMessage("ILLEGAL SUBFRAME: sqrt(A) was zero.");
		    Beep(1000,100);
		    return;
		}
		aratoa = SignExtend(g[6]>>6,23)*1.192092896e-7*PI;
		aargpg = SignExtend(g[7]>>6,23)*1.192092896e-7*PI;
		amanom = SignExtend(g[8]>>6,23)*1.192092896e-7*PI;
		aaf0 = SignExtend(((g[9]>>19)&0x7F8) + ((g[9]>>8)&3),10)
		       *9.536743164e-7;
		aaf1 = SignExtend((g[9]>>11)&0x7FF,10)*3.637978807e-12;
		almhlth = (g[4]>>6)&0xFF;

		/* Derive the reference week. */