📄 gpsfuncs.cpp
字号:
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 )
{
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);
chan[ch].page5=i5p;
if (i5page != i5p && i5data==1)
{
i5page=i5p;
if ( i5page == 51 )
{
iatoa=int((sf[5][3] & 0xFF00) >>8);
// atoa=iatoa*4096;
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
{
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*4096.0;
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;
}
}
}
}
}
/*******************************************************************************
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.
PURPOSE checks the parity of the 5 subframes of the nav message
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void parity_check(void)
{
long pb1=0x3b1f3480L,pb2=0x1d8f9a40L,pb3=0x2ec7cd00L;
long pb4=0x1763e680L,pb5=0x2bb1f340L,pb6=0x0b7a89c0L;
int parity,m_parity;
char d29=0,d30=0,sfm,word,b_1,b_2,b_3,b_4,b_5,b_6;
int err_bit;
for (sfm=1;sfm<=5;sfm++)
{
p_error[sfm]=0;
for (word=1;word<=10;word++)
{
m_parity=int(sf[sfm][word] &0x3f);
b_1=exor(d29,sf[sfm][word] & pb1) << 5;
b_2=exor(d30,sf[sfm][word] & pb2) << 4;
b_3=exor(d29,sf[sfm][word] & pb3) << 3;
b_4=exor(d30,sf[sfm][word] & pb4) << 2;
b_5=exor(0,sf[sfm][word] & pb5) << 1;
b_6=exor(d29^d30,sf[sfm][word] & pb6);
parity=b_1+b_2+b_3+b_4+b_5+b_6;
err_bit=0;
if (parity != m_parity)
{
err_bit=1;
}
p_error[sfm]=(p_error[sfm] << 1) + err_bit;
if (d30==1) sf[sfm][word]=0x03fffffc0L & ~sf[sfm][word];
sf[sfm][word]=sf[sfm][word]>>6;
d29=(m_parity & 0x2) >>1;
d30=m_parity & 0x1;
}
}
}
/*******************************************************************************
FUNCTION exor(char bit, long parity)
RETURNS None.
PARAMETERS
bit char
parity long
PURPOSE
count the number of bits set in the parameter parity and
do an exclusive or with the parameter bit
WRITTEN BY
Clifford Kelley
*******************************************************************************/
int exor(char bit, long parity)
{
char i;
int result;
result=0;
for (i=7;i<=30;i++)
{
if (bit_test_l(parity,i)) result++;
}
result=result%2;
result=(bit ^ result) & 0x1;
return(result);
}
/*******************************************************************************
FUNCTION ecef_to_llh(ecef pos)
RETURNS position in llh structure
PARAMETERS
pos ecef
PURPOSE Convert a position in in cartesian ecef coordinates to
Geodetic WGS 84 coordinates
Based on equations found in Hoffman-Wellinhoff
WRITTEN BY
Clifford Kelley
*******************************************************************************/
llh ecef_to_llh(ecef pos)
{
double p,n,thet,esq,epsq;
llh result;
// gotoxy(1,24);
// printf("->ecef_to_llh");
p=sqrt(pos.x*pos.x+pos.y*pos.y);
thet=atan(pos.z*a/(p*b));
esq =1.0-b*b/(a*a);
epsq=a*a/(b*b)-1.0;
result.lat=atan((pos.z+epsq*b*pow(sin(thet),3))/(p-esq*a*pow(cos(thet),3)));
result.lon=atan2(pos.y,pos.x);
n=a*a/sqrt(a*a*cos(result.lat)*cos(result.lat) +
b*b*sin(result.lat)*sin(result.lat));
result.hae=p/cos(result.lat)-n;
// gotoxy(1,24);
// printf("ecef_to_llh->");
return(result);
}
/*******************************************************************************
FUNCTION llh_to_ecef(llh pos)
RETURNS position in ecef structure
PARAMETERS
pos llh
PURPOSE Convert a position in Geodetic WGS 84 coordinates to cartesian
ecef coordinates
Based on equations found in Hoffman-Wellinhoff
WRITTEN BY
Clifford Kelley
*******************************************************************************/
ecef llh_to_ecef(llh pos)
{
double n;
// gotoxy(1,24);
// printf("->llh_to_ecef");
ecef result;
n=a*a/sqrt(a*a*cos(pos.lat)*cos(pos.lat)+b*b*sin(pos.lat)*sin(pos.lat));
result.x=(n+pos.hae)*cos(pos.lat)*cos(pos.lon);
result.y=(n+pos.hae)*cos(pos.lat)*sin(pos.lon);
result.z=(b*b/(a*a)*n+pos.hae)*sin(pos.lat);
// gotoxy(1,24);
// printf("llh_to_ecef->");
return(result);
}
/*******************************************************************************
FUNCTION pos_vel_time(int nsl)
RETURNS None.
PARAMETERS
nsl int
PURPOSE
This routine processes the all-in-view pseudorange to arrive
at a receiver position
INPUTS:
pseudo_range[nsl] Vector of measured range from satellites to the receiver
sat_location[nsl][3] Array of satellite locations in ECEF when the signal
was sent
nsl number of satellites used
OUTPUTS:
RP[3] VECTOR OF RECEIVER POSITION IN ECEF (X,Y,Z)
CBIAS RECEIVER CLOCK BIAS FROM GPS TIME
VARIABLES USED:
C SPEED OF LIGHT IN VACUUM IN M/S
S[6][5] MATRIX USED FOR SOLVING FOR RECEIVER POSITION CORRECTION
B[5] RESULTING RECEIVER CLOCK BIAS & POSITION CORRECTIONS
X,Y,Z TEMPORARY RECEIVER POSITION
T TEMPORARY RECEIVER CLOCK BIAS
R[5] RANGE FROM RECEIVER TO SATELLITES
IF THE POSITION CANNOT BE DETERMINED THE RESULT OF RP
WILL BE (0,0,0) THE CENTER OF THE EARTH
WRITTEN BY
Clifford Kelley
*******************************************************************************/
pvt pos_vel_time( int nsl)
{
double dd[5][5],r,ms[5][13],pm[5][13],bm[13],br[5],correct_mag,x,y,z,t;
double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom,alpha;
int i,j,k,nits;
pvt result;
//
// USE ITERATIVE APPROACH TO SOLVING FOR THE POSITION OF
// THE RECEIVER
//
// gotoxy(1,24);
// printf("->pos_vel_time");
nits=0;
t=0.0;
x=rec_pos_xyz.x;
y=rec_pos_xyz.y;
z=rec_pos_xyz.z;
//printf("a %lf,%lf,%lf\n",x,y,z);
do
{
for (i=1;i<=nsl;i++)
{
//
// Compute range in ECI at the time of arrival at the receiver
//
alpha=(t-dt[i])*omegae;// was dt[i]-t
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x,2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y,2)+
pow(track_sat[i].z-z,2));
bm[i]=r-(dt[i]-t)*c;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -