📄 gpsfuncs.cpp
字号:
// static int i4page,i5page;
int i4data,i5data,isv,iaomegad;
long iaaf0,iaaf1,iadeli,iaomega0,im0,inc0,iw;
unsigned long iasqr;
long iaw,iam0,scale,ia0,ia1;
char ial0,ial1,ial2,ial3,ibt0,ibt1,ibt2,ibt3;
int itot,iWNt,idtls,iWNlsf,iDN,idtlsf;//WNa
int sfr,word,i4p,i5p;
double r_sqra,r_inc0,r_ety;
float d_toe;
//
// assemble the 1500 data bits into subframes and words
//
// gotoxy(1,24);
// printf("->navmess prn %d ch %d",prn,ch);
d_toe=clock_tow-gps_eph[prn].toe;
if (d_toe>302400.0) d_toe=d_toe-604800.0;
else if (d_toe<-302400.0)d_toe=d_toe+604800.0;
j=0;
for (sfr=1;sfr<=5;sfr++)
{
for (word=1;word<=10;word++)
{
scale=536870912L;
sf[sfr][word]=0;
for (i=0;i<=29;i++)
{
if (chan[ch].message[(j+chan[ch].offset)%1500]==1)
sf[sfr][word]+=scale;
scale=scale>>1;
j++;
}
}
}
parity_check(); // check the parity of the 1500 bit message
chan[ch].word_error[0]=p_error[1];
chan[ch].word_error[1]=p_error[2];
chan[ch].word_error[2]=p_error[3];
chan[ch].word_error[3]=p_error[4];
chan[ch].word_error[4]=p_error[5];
//
// EPHEMERIS DECODE subframes 1, 2, 3
//
// subframe 1
//
// check parity of first 3 subframes, since it is a circular register
// we may have over written the first few bits so allow for word 1 of
// subframe 1 to have a problem
//
if ((p_error[1]==0 || p_error[1]==0x200) && p_error[2]==0 && p_error[3]==0)
{
iodc=int(((sf[1][3] & 0x3) <<8 ) | ((sf[1][8] & 0xFF0000L) >>16));
iode=int(sf[2][3] >> 16);
idoe=int(sf[3][10] >> 16);
// fprintf(kalm,"prn=%d iodc=%d, iode=%d,idoe=%d\n",prn,iodc,iode,idoe);
// fprintf(kalm," eph.iode=%d, eph.iodc=%d\n",gps_eph[prn].iode,gps_eph[prn].iodc);
// if both copies of iode agree and we have a new iode or new iodc then process the ephemeris
if (iode==idoe && ((iode!=gps_eph[prn].iode) || (iodc!=gps_eph[prn].iodc)))
{
iweek= int(sf[1][3] >> 14);
// icapl2=( sf[1][3] & 0x3000 ) >> 12;
iura=int(( sf[1][3] & 0xF00 ) >> 8);
ihealth=int(( sf[1][3] & 0xFC ) >> 2);
itgd=int(sf[1][7] & 0xFF);
itoc=int(sf[1][8] & 0xFFFF);
iaf2=int(sf[1][9] >> 16);
iaf1=int(sf[1][9] & 0xFFFF);
iaf0=sf[1][10] >> 2;
if (bit_test_l(iaf0,22)) iaf0=iaf0 | 0xFFC00000L;
//
// subframe 2
//
icrs=int(sf[2][3] & 0xFFFF);
idn=int(sf[2][4] >> 8);
im0=((sf[2][4] & 0xFF) << 24) | sf[2][5];
icuc=int(sf[2][6] >>8);
ie=((sf[2][6] & 0xFF) << 24) | sf[2][7];
icus=int(sf[2][8] >> 8);
isqra=(((sf[2][8] & 0xFF) << 24) | sf[2][9]);
itoe=int(sf[2][10] >> 8);
// fif=int((sf[2][10] & 0x80) >> 7);
//
// subframe 3
//
icic=int(sf[3][3] >> 8);
icis=int(sf[3][5] >> 8);
inc0=((sf[3][5] & 0xFF) << 24) | sf[3][6];
iomega0=((sf[3][3] & 0xFF) << 24) | sf[3][4];
icrc=int(sf[3][7] >> 8);
iw=((sf[3][7] & 0xFF) << 24) | sf[3][8];
iomegadot=sf[3][9];
if (bit_test_l(iomegadot,24)) iomegadot=iomegadot | 0xFF000000L;
idot=int((sf[3][10] & 0xFFFC) >> 2);
if (bit_test_l(idot,14)) idot=idot | 0xC000;
r_sqra=isqra*c_2m19;
r_inc0=inc0*c_2m31*pi;
r_ety=ie*c_2m33;
// fprintf(kalm,"sqra=%lf, inc=%lf,ety=%lf\n",r_sqra,r_inc0,r_ety);
//
// Does this ephemeris make sense?
//
if ((r_inc0<1.25 && r_inc0>0.873) && (r_sqra>5100.0 && r_sqra<5200.0) &&
(r_ety <.05 && r_ety>0.0))
{
gps_eph[prn].valid=1;
gps_eph[prn].iode=iode;
gps_eph[prn].iodc=iodc;
gps_eph[prn].week=iweek;
gps_eph[prn].ura=iura;
gps_eph[prn].health=ihealth;
gps_eph[prn].tgd=itgd*c_2m31;
gps_eph[prn].toc=itoc*16.0;
gps_eph[prn].af2=iaf2*c_2m55;
gps_eph[prn].af1=iaf1*c_2m43;
gps_eph[prn].af0=iaf0*c_2m31;
gps_eph[prn].crs=icrs*c_2m5;
gps_eph[prn].dn=idn*c_2m43*pi;
gps_eph[prn].ma=im0*c_2m31*pi;
gps_eph[prn].cuc=icuc*c_2m29;
gps_eph[prn].ety=r_ety;
gps_eph[prn].cus=icus*c_2m29;
gps_eph[prn].sqra=r_sqra;
gps_eph[prn].wm=19964981.84/pow(r_sqra,3);
gps_eph[prn].toe=itoe*c_2p4;
gps_eph[prn].cic=icic*c_2m29;
gps_eph[prn].cis=icis*c_2m29;
gps_eph[prn].inc0=r_inc0;
gps_eph[prn].w0=iomega0*c_2m31*pi;
gps_eph[prn].crc=icrc*c_2m5;
gps_eph[prn].w=iw*c_2m31*pi;
gps_eph[prn].omegadot=iomegadot*c_2m43*pi;
gps_eph[prn].idot=idot*c_2m43*pi;
if (out_debug) write_Debug_ephemeris(prn);
}
else if (gps_eph[prn].valid==1 && d_toe>7200.0) gps_eph[prn].valid=0;
}
}
//!
//! ALMANAC DECODE subframes 4 and 5
//!
//! SUBFRAME 4
//!
//! check parity of subframes 4 and 5 and don't bother if we already
//! have the almanac
//!
// fprintf(kalm,"p_error[4]=%d p_error[5]=%d almanac_valid=%d\n",
// p_error[4],p_error[5],almanac_valid);
if (p_error[4]==0 && p_error[5]==0 )// if no parity errors on subframe 4 & 5
{
i5data=int(sf[5][3] >> 22);
i5p=int((sf[5][3] & 0x3F0000L) >> 16);
alm_page=i5p;
if (i5page != i5p && i5data==1)
{
if ( i5p != 51 && i5p !=0 )
{
isv=i5p;
iatoa=int(sf[5][4] >> 16);
if (gps_alm[isv].week == gps_week%1024 && gps_alm[isv].toa == float(iatoa*c_2p12))
{
almanac_valid=1; // if they are the same as what we already have
i5page=i5p; // then almanac is up to date for that satellite
}
else almanac_valid=0;
// fprintf(kalm,"isv=%d gps_week=%d gps_alm[isv].week=%d gps_alm[isv].toa=%f iatoa*c_2p12=%f\n valid=%d i5page=%d\n",
// isv,gps_week%1024,gps_alm[isv].week,gps_alm[isv].toa,float(iatoa*c_2p12),almanac_valid,i5page);
}
}
}
if (p_error[4]==0 && p_error[5]==0 && almanac_valid==0 ) // if no parity errors on subframe 4 & 5
{ // and data not valid for this page
i4data= int(sf[4][3] >> 22);
i4p= int((sf[4][3] & 0x3F0000L) >> 16);
if (i4p != i4page && i4data==1) //! i4p all we need is a page to be
{ //! read from 1 satellite
i4page=i4p;
if (i4page > 24 && i4page < 33) // almanacs for PRN 25 -> 32
{
isv=i4page ;
gps_alm[isv].week=gps_week%1024;
iae=int(sf[4][3] & 0x00FFFFL);
gps_alm[isv].ety=iae*c_2m21;
iatoa=int(sf[4][4] >> 16);
gps_alm[isv].toa=iatoa*c_2p12;
iadeli=sf[4][4] & 0x00FFFFL;
if (bit_test_l(iadeli,16)) iadeli=iadeli | 0xFFFF0000L;
gps_alm[isv].inc=(iadeli*c_2m19+0.3)*pi;
iomegad=int(sf[4][5] >> 8);
gps_alm[isv].rra=iomegad*c_2m38*pi;
gps_alm[isv].health=int(sf[4][5] & 0x0000FF);
iasqr=sf[4][6];
gps_alm[isv].sqa=iasqr*c_2m11;
if (gps_alm[isv].sqa>0.0) gps_alm[isv].w=19964981.84/
pow(gps_alm[isv].sqa,3);
iaomega0=sf[4][7];
if (bit_test_l(iaomega0,24)) iaomega0=iaomega0 | 0xFF000000L;
gps_alm[isv].lan=iaomega0*c_2m23*pi;
iaw=sf[4][8];
if (bit_test_l(iaw,24)) iaw=iaw | 0xFF000000L;
gps_alm[isv].aop=iaw*c_2m23*pi;
iam0=sf[4][9];
if (bit_test_l(iam0,24)) iam0=iam0 | 0xFF000000L;
gps_alm[isv].ma=iam0*c_2m23*pi;
iaaf0=(sf[4][10] >> 13) | ((sf[4][10] & 0x1C)>>2);
if (bit_test_l(iaaf0,11)) iaaf0=iaaf0 | 0xFFFFF800L;
gps_alm[isv].af0=iaaf0*c_2m20;
iaaf1=(sf[4][10] | 0xFFE0) >> 5;
if (bit_test_l(iaaf1,11)) iaaf1=iaaf1 | 0xFFFFF800L;
gps_alm[isv].af1=iaaf1*c_2m38;
}
else if ( i4page == 55 ) // page for text message
{
gps_alm[prn].text_message[0]=char((sf[4][3] & 0x00FF00) >> 8);
gps_alm[prn].text_message[1]=char( sf[4][3] & 0x0000FF);
for ( k=1;k<=7;k++)
{
gps_alm[prn].text_message[3*k-1]= char(sf[4][k+3] >> 16);
gps_alm[prn].text_message[3*k ]= char((sf[4][k+3] & 0x00FF00) >> 8);
gps_alm[prn].text_message[3*k+1]= char(sf[4][k+3] & 0x0000FF);
}
}
else if ( i4page == 56 ) // page for broadcast Ionosphere Model & UTC Parameters
{
ial0=int((sf[4][3] & 0x00FF00) >> 8);
al0=ial0*c_2m30;
ial1= int(sf[4][3] & 0x0000FF);
al1=ial1*c_2m27;
ial2= int(sf[4][4] >> 16);
al2=ial2*c_2m24;
ial3=int((sf[4][4] & 0x00FF00) >> 8);
al3=ial3*c_2m24;
ibt0= int(sf[4][4] & 0x0000FF);
b0=ibt0*2048.;
ibt1= int(sf[4][5] >> 16);
b1=ibt1*16384.;
ibt2=int((sf[4][5] & 0x00FF00) >> 8);
b2=ibt2*65536.;
ibt3= int(sf[4][5] & 0x00FF);
b3=ibt3*65536.;
ia1= sf[4][6];
if (bit_test_l(ia1,24)) ia1=ia1 | 0xFF000000L;
a1=ia1*c_2m50;
ia0= (sf[4][7] << 8) | (sf[4][8] >> 16);
a0=ia0*c_2m30;
itot= int((sf[4][8] & 0x00FF00) >> 8);
tot=itot*4096;
iWNt= int(sf[4][8] & 0xFF);
WNt=iWNt;
idtls= int(sf[4][10] >> 16);
if (idtls >128) idtls=idtls |0xFF00;
dtls=idtls;
iWNlsf=int((sf[4][9] & 0x00FF00) >> 8);
WNlsf=iWNlsf;
iDN = int(sf[4][9] & 0x0000FF);
DN=iDN;
idtlsf= int(sf[4][9] >> 16);
if (idtlsf >128) idtlsf=idtlsf |0xFF00;
dtlsf=idtlsf;
}
else if ( i4page == 63 ) // page for constellation health
{
ASV[1]= int((sf[4][3] & 0x00F000) >>12);
ASV[2]= int((sf[4][3] & 0x000F00) >>8);
ASV[3]= int((sf[4][3] & 0x0000F0) >>4);
ASV[4]= int( sf[4][3] & 0x00000F);
ASV[5]= int( sf[4][4] >>20);
ASV[6]= int((sf[4][4] & 0x0F0000L) >>16);
ASV[7]= int((sf[4][4] & 0x00F000) >>12);
ASV[8]= int((sf[4][4] & 0x000F00) >> 8);
ASV[9]= int((sf[4][4] & 0x0000F0) >> 4);
ASV[10]=int(sf[4][4] & 0x00000F);
ASV[11]=int(sf[4][5] >>20);
ASV[12]=int((sf[4][5] & 0x0F0000L) >>16);
ASV[13]=int((sf[4][5] & 0x00F000) >>12);
ASV[14]=int((sf[4][5] & 0x000F00) >> 8);
ASV[15]=int((sf[4][5] & 0x0000F0) >> 4);
ASV[16]=int(sf[4][5] & 0x00000F);
ASV[17]=int(sf[4][6] >>20);
ASV[18]=int((sf[4][6] & 0x0F0000L) >>16);
ASV[19]=int((sf[4][6] & 0x00F000) >>12);
ASV[20]=int((sf[4][6] & 0x000F00) >> 8);
ASV[21]=int((sf[4][6] & 0x0000F0) >> 4);
ASV[22]=int(sf[4][6] & 0x00000F);
ASV[23]=int(sf[4][7] >>20);
ASV[24]=int((sf[4][7] & 0x0F0000L) >>16);
ASV[25]=int((sf[4][7] & 0x00F000) >>12);
ASV[26]=int((sf[4][7] & 0x000F00) >> 8);
ASV[27]=int((sf[4][7] & 0x0000F0) >> 4);
ASV[28]=int( sf[4][7] & 0x00000F);
ASV[29]=int( sf[4][8] >>20);
ASV[30]=int((sf[4][8] & 0x0F0000L) >>16);
ASV[31]=int((sf[4][8] & 0x00F000) >>12);
ASV[32]=int((sf[4][8] & 0x000F00) >> 8);
SVh[25]=int(sf[4][8] & 0x00003F);
if( SVh[25]==0x3f) gps_alm[25].inc=0.0;
SVh[26]=int(sf[4][9] >>18);
if( SVh[26]==0x3f) gps_alm[26].inc=0.0;
SVh[27]=int((sf[4][9] & 0x03F000L) >>12);
if( SVh[27]==0x3f) gps_alm[27].inc=0.0;
SVh[28]=int((sf[4][9] & 0x000FC0) >>6);
if( SVh[28]==0x3f) gps_alm[28].inc=0.0;
SVh[29]= int(sf[4][9] & 0x00003F);
if( SVh[29]==0x3f) gps_alm[29].inc=0.0;
SVh[30]= int(sf[4][10] >>18);
if( SVh[30]==0x3f) gps_alm[30].inc=0.0;
SVh[31]=int((sf[4][10]& 0x03F000L) >>12);
if( SVh[31]==0x3f) gps_alm[31].inc=0.0;
SVh[32]=int((sf[4][10]& 0x000FC0) >>6);
if( SVh[32]==0x3f) gps_alm[32].inc=0.0;
}
}
i5data=int(sf[5][3] >> 22);
i5p=int((sf[5][3] & 0x3F0000L) >> 16);
if (i5page != i5p && i5data==1)
{
chan[ch].page5=i5p;
i5page=i5p;
if ( i5page == 51 ) // page for constellation health
{
iatoa=int((sf[5][3] & 0xFF00) >>8);
SVh[1]=int(sf[5][4] >>18);
if( SVh[1]==0x3f) gps_alm[1].inc=0.0;
SVh[2]=int((sf[5][4] & 0x03F000L)>>12);
if( SVh[2]==0x3f) gps_alm[2].inc=0.0;
SVh[3]=int((sf[5][4] & 0x000FC0)>>6);
if( SVh[3]==0x3f) gps_alm[3].inc=0.0;
SVh[4]= int(sf[5][4] & 0x00003F);
if( SVh[4]==0x3f) gps_alm[4].inc=0.0;
SVh[5]= int(sf[5][5] >>18);
if( SVh[5]==0x3f) gps_alm[5].inc=0.0;
SVh[6]=int((sf[5][5] & 0x03F000L)>>12);
if( SVh[6]==0x3f) gps_alm[6].inc=0.0;
SVh[7]=int((sf[5][5] & 0x000FC0)>>6);
if( SVh[7]==0x3f) gps_alm[7].inc=0.0;
SVh[8]= int(sf[5][5] & 0x00003F);
if( SVh[8]==0x3f) gps_alm[8].inc=0.0;
SVh[9]= int(sf[5][6] >>18);
if( SVh[9]==0x3f) gps_alm[9].inc=0.0;
SVh[10]=int((sf[5][6] & 0x03F000L)>>12);
if( SVh[10]==0x3f) gps_alm[10].inc=0.0;
SVh[11]=int((sf[5][6] & 0x000FC0)>>6);
if( SVh[11]==0x3f) gps_alm[11].inc=0.0;
SVh[12]= int(sf[5][6] & 0x00003F);
if( SVh[12]==0x3f) gps_alm[12].inc=0.0;
SVh[13]= int(sf[5][7] >>18);
if( SVh[13]==0x3f) gps_alm[13].inc=0.0;
SVh[14]=int((sf[5][7] & 0x03F000L)>>12);
if( SVh[14]==0x3f) gps_alm[14].inc=0.0;
SVh[15]=int((sf[5][7] & 0x000FC0)>>6);
if( SVh[15]==0x3f) gps_alm[15].inc=0.0;
SVh[16]= int(sf[5][7] & 0x00003F);
if( SVh[16]==0x3f) gps_alm[16].inc=0.0;
SVh[17]= int(sf[5][8] >>18);
if( SVh[17]==0x3f) gps_alm[17].inc=0.0;
SVh[18]=int((sf[5][8] & 0x03F000L)>>12);
if( SVh[18]==0x3f) gps_alm[18].inc=0.0;
SVh[19]=int((sf[5][8] & 0x000FC0)>>6);
if( SVh[19]==0x3f) gps_alm[19].inc=0.0;
SVh[20]= int(sf[5][8] & 0x00003F);
if( SVh[20]==0x3f) gps_alm[20].inc=0.0;
SVh[21]= int(sf[5][9] >>18);
if( SVh[21]==0x3f) gps_alm[21].inc=0.0;
SVh[22]=int((sf[5][9] & 0x03F000L)>>12);
if( SVh[22]==0x3f) gps_alm[22].inc=0.0;
SVh[23]=int((sf[5][9] & 0x000FC0)>>6);
if( SVh[23]==0x3f) gps_alm[23].inc=0.0;
SVh[24]= int(sf[5][9] & 0x00003F);
if( SVh[24]==0x3f) gps_alm[24].inc=0.0;
}
else
{ // pages for almanacs of PRN 1 -> 24
isv=i5page;
gps_alm[isv].week=gps_week%1024;
iae=int(sf[5][3] & 0xFFFF);
gps_alm[isv].ety=iae*c_2m21;
iatoa=int(sf[5][4] >> 16);
gps_alm[isv].toa=iatoa*c_2p12;
iadeli=int(sf[5][4] & 0xFFFF);
gps_alm[isv].inc=(iadeli*c_2m19+0.3)*pi;
iaomegad=int(sf[5][5] >> 8);
gps_alm[isv].rra=iaomegad*c_2m38*pi;
gps_alm[isv].health=int(sf[5][5] & 0xFF);
iasqr=sf[5][6];
gps_alm[isv].sqa=iasqr*c_2m11;
if (gps_alm[isv].sqa>0.0) gps_alm[isv].w=19964981.84/pow(gps_alm[isv].sqa,3);
iaomega0=sf[5][7];
if (bit_test_l(iaomega0,24)) iaomega0=iaomega0 | 0xFF000000L;
gps_alm[isv].lan=iaomega0*c_2m23*pi;
iaw=sf[5][8];
if (bit_test_l(iaw,24)) iaw=iaw | 0xFF000000L;
gps_alm[isv].aop=iaw*c_2m23*pi;
iam0=sf[5][9];
if (bit_test_l(iam0,24)) iam0=iam0 | 0xFF000000L;
gps_alm[isv].ma=iam0*c_2m23*pi;
iaaf0=int((sf[5][10] >> 13) | ((sf[5][10] & 0x1C)>>2));
if (bit_test_l(iaaf0,11)) iaaf0=iaaf0 | 0xF800;
gps_alm[isv].af0=iaaf0*c_2m20;
iaaf1=int((sf[5][10] & 0xFFE0) >> 5);
if (bit_test_l(iaaf1,11)) iaaf1=iaaf1 | 0xF800;
gps_alm[isv].af1=iaaf1*c_2m38;
}
}
}
// gotoxy(1,24);
// printf("navmess->");
}
/*******************************************************************************
FUNCTION bit_test_l(unsigned long data, char bit_n)
RETURNS int
PARAMETERS
data unsigned long
bit_n char
PURPOSE
This function returns a 1 if bit number bit_n of data is 1
else it returns a 0
WRITTEN BY
Clifford Kelley
*******************************************************************************/
inline int bit_test_l(unsigned long data,char bit_n)
{
int result;
result=0;
if (data & test_l[bit_n])result=1;
return(result);
}
/*******************************************************************************
FUNCTION parity_check(void)
RETURNS None.
PARAMETERS None.
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -