procsbf.c

GPS导航定位程序

		CurrentTIC(&ctic);
		TICToGpsTime(ctic,&refweek,&gsec);
		if(gsec-toa>302400.0)
		    refweek++;
		else if(gsec-toa<-302400.0)
		    refweek--;

		PROTECT++;
		alms[almsv-1].almhlth = almhlth;
		alms[almsv-1].refweek = refweek;
		alms[almsv-1].toa = toa;
		alms[almsv-1].ecc = aecc;
		alms[almsv-1].inclin = ainclin;
		alms[almsv-1].rora = arora;
		alms[almsv-1].sqrta = asqrta;
		alms[almsv-1].ratoa = aratoa;
		alms[almsv-1].argpg = aargpg;
		alms[almsv-1].manom = amanom;
		alms[almsv-1].af0 = aaf0;
		alms[almsv-1].af1 = aaf1;
		alms[almsv-1].vflg = 1;
		PROTECT--;
	    }
	    else if(pageid==55)               /* Special message subframe. */
	    {
		spmsg[0] = (g[2]>>14)&0xFF;
		spmsg[1] = (g[2]>>6)&0xFF;
		for(i=0;i<6;i++)
		{
		    spmsg[2+3*i] = (g[3+i]>>22)&0xFF;
		    spmsg[3+3*i] = (g[3+i]>>14)&0xFF;
		    spmsg[4+3*i] = (g[3+i]>>6)&0xFF;
		}
		spmsg[20] = (g[9]>>22)&0xFF;
		spmsg[21] = (g[9]>>14)&0xFF;
		spmsg[22] = 0;
	    }
	    else if(pageid==56)       /* The iono/UTC correction subframe. */
	    {
		alpha0 = SignExtend(g[2]>>14,7) * ldexp(1.0,-30);
		alpha1 = SignExtend(g[2]>>6,7) * ldexp(1.0,-27);
		alpha2 = SignExtend(g[3]>>22,7) * ldexp(1.0,-24);
		alpha3 = SignExtend(g[3]>>14,7) * ldexp(1.0,-24);
		beta0 = SignExtend(g[3]>>6,7) * ldexp(1.0,11);
		beta1 = SignExtend(g[4]>>22,7) * ldexp(1.0,14);
		beta2 = SignExtend(g[4]>>14,7) * ldexp(1.0,16);
		beta3 = SignExtend(g[4]>>6,7) * ldexp(1.0,16);
		A0 = (long)(((g[6]<<2)&0xFFFFFF00L)+((g[7]>>22)&0xFF))
		     * 9.313225746e-10;
		A1 = SignExtend(g[5]>>6,23) * 8.881784197e-16;
		dtls = (unsigned long)((g[8]>>22)&0xFF);
		tot = (unsigned long)((g[7]>>14)&0xFF) * 4096;
		wnt = (unsigned long)((g[7]>>6)&0xFF);
		wnlsf = (unsigned long)((g[8]>>14)&0xFF);
		dn = (unsigned long)((g[8]>>6)&0xFF);
		dtlsf = SignExtend(g[9]>>22,7);

		PROTECT++;
		NoOldData = (ionoutc.vflg==0);
		NewDataSet = (ionoutc.a0!=alpha0) || (ionoutc.a1!=alpha1)
			     || (ionoutc.a2!=alpha2) || (ionoutc.a3!=alpha3)
			     || (ionoutc.b0!=beta0)  || (ionoutc.b1!=beta1)
			     || (ionoutc.b2!=beta2) || (ionoutc.b3!=beta3)
			     || (ionoutc.A0!=A0) || (ionoutc.A1!=A1)
			     || (ionoutc.dtls!=dtls) || (ionoutc.tot!=tot)
			     || (ionoutc.wnt!=wnt) || (ionoutc.wnlsf!=wnlsf)
			     || (ionoutc.dn!=dn) || (ionoutc.dtlsf!=dtlsf);

		if(NoOldData || NewDataSet)
		{
		    ionoutc.a0 = alpha0;  /* Ionospheric model parameters. */
		    ionoutc.a1 = alpha1;
		    ionoutc.a2 = alpha2;
		    ionoutc.a3 = alpha3;
		    ionoutc.b0 = beta0;
		    ionoutc.b1 = beta1;
		    ionoutc.b2 = beta2;
		    ionoutc.b3 = beta3;

		    ionoutc.A0 = A0;         /* UTC conversion parameters. */
		    ionoutc.A1 = A1;
		    ionoutc.dtls = dtls;
		    ionoutc.tot = tot;
		    ionoutc.wnt = wnt;
		    ionoutc.wnlsf = wnlsf;
		    ionoutc.dn = dn;
		    ionoutc.dtlsf = dtlsf;
		    ionoutc.vflg = 1;
		}
		PROTECT--;
	    }
	    else if(pageid==63)             /* Constellation health words. */
	    {
		chlth[24] = (g[7]>>6)&0x3F;
		chlth[25] = (g[8]>>24)&0x3F;
		chlth[26] = (g[8]>>18)&0x3F;
		chlth[27] = (g[8]>>12)&0x3F;
		chlth[28] = (g[8]>>6)&0x3F;
		chlth[29] = (g[9]>>24)&0x3F;
		chlth[30] = (g[9]>>18)&0x3F;
		chlth[31] = (g[9]>>12)&0x3F;
	    }
	    sprintf(buff,"SV %2d SF 4",sv);
	    TCBComment(buff);
	    break;

	case 5:
	    if(SVDataOK==0)
		break;
	    pageid = (g[2]>>22)&0x3F;
	    if(pageid==0)
	    {
		/* There is no almanac in the designated slot for some SV. */

		almsv = pg2svid5[((tow-5)/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>0 && pageid<=24)         /* 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. */

		CurrentTIC(&ctic);
		TICToGpsTime(ctic,&refweek,&gsec);
		if(gsec-toa>302400.0)
		    refweek++;
		else if(gsec-toa<-302400.0)
		    refweek--;

		PROTECT++;
		alms[almsv-1].almhlth = almhlth;
		alms[almsv-1].refweek = refweek;
		alms[almsv-1].toa = toa;
		alms[almsv-1].ecc = aecc;
		alms[almsv-1].inclin = ainclin;
		alms[almsv-1].rora = arora;
		alms[almsv-1].sqrta = asqrta;
		alms[almsv-1].ratoa = aratoa;
		alms[almsv-1].argpg = aargpg;
		alms[almsv-1].manom = amanom;
		alms[almsv-1].af0 = aaf0;
		alms[almsv-1].af1 = aaf1;
		alms[almsv-1].vflg = 1;
		PROTECT--;
	    }
	    else if(pageid==51)             /* Constellation health words. */
	    {
		for(i=0;i<6;i++)
		{
		    chlth[4*i+0] = (g[i+3]>>24)&0x3F;
		    chlth[4*i+1] = (g[i+3]>>18)&0x3F;
		    chlth[4*i+2] = (g[i+3]>>12)&0x3F;
		    chlth[4*i+3] = (g[i+3]>>6)&0x3F;
		}
	    }
	    sprintf(buff,"SV %2d SF 5",sv);
	    TCBComment(buff);
	    break;
    }                                                        /* Of switch. */

    /* Update ephemeris and clock correction data only when all three
       subframes (1,2,3) are from the same data set. */

    PROTECT++;
    HaveEph123 = newsf123[sv-1].vflg1 && newsf123[sv-1].vflg2
		 && newsf123[sv-1].vflg3
		 && (newsf123[sv-1].iode2==newsf123[sv-1].iode3)
		 && ((newsf123[sv-1].iodc&0xFF)==newsf123[sv-1].iode2);
    NoOldData = (ephs[sv-1].vflg==0);
    NewDataSet = (ephs[sv-1].iode!=newsf123[sv-1].iode2)
		 || (ephs[sv-1].iodc!=newsf123[sv-1].iodc);
    PROTECT--;

    if(HaveEph123 && (NoOldData || NewDataSet) )
    {
	PROTECT++;
	e = &ephs[sv-1];
	u = &newsf123[sv-1].SF1words[0];
	e->TofXmission = (((u[1]>>13)&0x1FFFFL)-1)*6;  /* TOW of the SF 1. */
	e->iodc = newsf123[sv-1].iodc;
	e->codeL2 = (u[2]>>8)&0x3;
	e->wkn = (u[2]>>20)&0x3FF;
	e->L2Pdata = (u[3]>>29)&0x1;
	e->ura = (u[2]>>14)&0xF;
	e->s1hlth = (u[2]>>8)&0x3F;
	e->tgd = SignExtend( (u[6]>>6)&0xFF, 7)*4.656612873e-10;
	e->toc = (unsigned long)((u[7]>>6)&0xFFFFul)*16.0;
	e->tocwk = e->wkn;
	dtemp = e->toc - tow*6;
	if(dtemp>302400.0)
	    e->tocwk--;
	if(dtemp<-302400.0)
	    e->tocwk++;
	e->af2 = SignExtend(u[8]>>22,7)*2.775557562e-17;
	e->af1 = SignExtend(u[8]>>6,15)*1.136868377e-13;
	e->af0 = SignExtend(u[9]>>8,21)*4.656612873e-10;

	u = &newsf123[sv-1].SF2words[0];
	e->iode = newsf123[sv-1].iode2;
	e->crs = SignExtend(u[2]>>6,15)*0.03125;
	e->deltan = SignExtend(u[3]>>14,15)*1.136868377e-13*PI;
	e->m0 = (long)( ((u[3]<<18)&0xFF000000L)+((u[4]>>6)&0xFFFFFFL) )
		 *4.656612873e-10*PI;
	e->cuc = SignExtend(u[5]>>14,15)*1.862645149e-9;
	e->ecc = (unsigned long)(((u[5]<<18)&0xFF000000L)
		 + ((u[6]>>6)&0xFFFFFFL))*1.164153218e-10;
	e->cus = SignExtend(u[7]>>14,15)*1.862645149e-9;
	e->sqrta = (unsigned long)(((u[7]<<18)&0xFF000000L)
		   + ((u[8]>>6)&0xFFFFFFL))*1.907348633e-6;
	if(e->sqrta==0.0)
	{
	    WarningMessage("ILLEGAL SUBFRAME: sqrt(A) was zero.");
	    Beep(1000,100);
	    PROTECT--;
	    return;
	}
	e->toe = (unsigned long)((u[9]>>14)&0xFFFFul)*16.0;
	e->toewk = e->wkn;
	dtemp = e->toe - tow*6;
	if(dtemp>302400.0)
	    e->toewk--;
	if(dtemp<-302400.0)
	    e->toewk++;
	e->fti = (u[9]>>13)&1;
    
	u = &newsf123[sv-1].SF3words[0];
	e->cic = SignExtend(u[2]>>14,15)*1.862645149e-9;
	e->om0 = (long)(((u[2]<<18)&0xFF000000L)+((u[3]>>6)&0xFFFFFFL))
		 *4.656612873e-10*PI;
	e->cis = SignExtend(u[4]>>14,15)*1.862645149e-9;
	e->in0 = (long)(((u[4]<<18)&0xFF000000L)+((u[5]>>6)&0xFFFFFFL))
		 *4.656612873e-10*PI;
	e->crc = SignExtend(u[6]>>14,15)*0.03125;
	e->olc = (long)(((u[6]<<18)&0xFF000000L)+((u[7]>>6)&0xFFFFFFL))
		 *4.656612873e-10*PI;
	e->omd = SignExtend(u[8]>>6,23)*1.136868377e-13*PI;
	e->idot = SignExtend(u[9]>>8,13)*1.136868377e-13*PI;

	/* Compute a derived quantity: semimajor axis (m). */

	e->a = a = DSquare(e->sqrta);

	/* Compute a derived quantity: mean motion (rad/sec). */

	e->n0 = sqrt(GravConstant/(a*a*a));
    
	e->vflg = 1;
	newsf123[sv-1].vflg1 = newsf123[sv-1].vflg2 
			     = newsf123[sv-1].vflg3
			     = 0;
	Rx2EPH(sv);                 /* Record the new broadcast ephemeris. */
	PROTECT--;
    }
    Checkpoint("ProcessSubframe",2);
}