?? gpsfuncs.c
字號:
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 ) {// broadcast ionospheric data ial0 = (int)((sf[4][3] & 0x00FF00) >> 8); Iono.al0 = ial0*9.313225746e-10; ial1 = (int)(sf[4][3] & 0x0000FF); Iono.al1 = ial1*7.4505806e-9; ial2 = (int)(sf[4][4] >> 16); Iono.al2 = ial2*5.960464478e-8; ial3 = (int)((sf[4][4] & 0x00FF00) >> 8); Iono.al3 = ial3*5.960464478e-8; ibt0 = (int)(sf[4][4] & 0x0000FF); Iono.b0 = ibt0*2048.; ibt1 = (int)(sf[4][5] >> 16); Iono.b1 = ibt1*16384.; ibt2 = (int)((sf[4][5] & 0x00FF00) >> 8); Iono.b2 = ibt2*65536.; ibt3 = (int)(sf[4][5] & 0x00FF); Iono.b3 = ibt3*65536.; ia1 = sf[4][6]; if (bit_test_l( ia1, 24)) ia1 = ia1 | 0xFF000000L; Utc.a1 = ia1*8.881784197e-16; ia0 = (sf[4][7] << 8) | (sf[4][8] >> 16); Utc.a0 = ia0*9.31322574615e-10; itot = (int)((sf[4][8] & 0x00FF00) >> 8); Utc.tot = itot*4096; iWNt = (int)(sf[4][8] & 0xFF); Utc.WNt = iWNt; idtls = (int)(sf[4][9] >> 16); if (idtls > 128) idtls = idtls | 0xFF00; Utc.dtls = idtls; iWNlsf = (int)((sf[4][9] & 0x00FF00) >> 8); Utc.WNlsf = iWNlsf; iDN = (int)(sf[4][9] & 0x0000FF); Utc.DN = iDN;// idtlsf = (int)( sf[4][9] >> 16); idtlsf = (int)( sf[4][10] >> 16); // fixed (GB-020217) if (idtlsf > 128) idtlsf = idtlsf | 0xFF00; Utc.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; } }//// SUBFRAME 5//// i5data= sf[5][3] >> 22; i5p=(int)((sf[5][3] & 0x3F0000L) >> 16); chan[ch].page5=i5p; if ( i5page != i5p) { 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; iae=(int)(sf[5][3] & 0xFFFF); gps_alm[isv].ety=iae*4.768371582E-7; iatoa=(int)(sf[5][4] >> 16); gps_alm[isv].toa=iatoa*4096.0; iadeli=(int)(sf[5][4] & 0xFFFF); gps_alm[isv].inc=(iadeli*1.907348633E-6+0.3)*pi; iaomegad=(int)(sf[5][5] >> 8); gps_alm[isv].omegadot=iaomegad*3.637978807E-12*pi; gps_alm[isv].health=(int)(sf[5][5] & 0xFF); iasqr=sf[5][6]; gps_alm[isv].sqra=iasqr*4.8828125E-4; if (gps_alm[isv].sqra>0.0) gps_alm[isv].w=19964981.84/pow(gps_alm[isv].sqra,3); iaomega0=sf[5][7]; if (bit_test_l(iaomega0,24)) iaomega0=iaomega0 | 0xFF000000L; gps_alm[isv].omega0=iaomega0*1.192092896E-7*pi; iaw=sf[5][8]; if (bit_test_l(iaw,24)) iaw=iaw | 0xFF000000L; gps_alm[isv].w =iaw*1.192092896E-7*pi; iam0 = sf[5][9]; if ( bit_test_l( iam0, 24)) iam0 = iam0 | 0xFF000000L; gps_alm[isv].ma = iam0*1.192092896E-7*pi; iaaf0 = (int)((sf[5][10] >> 13) | (sf[5][10] & 0x1C)); if ( bit_test_l(iaaf0,11)) iaaf0 = iaaf0 | 0xF800; gps_alm[isv].af0 = iaaf0*9.536743164E-7; iaaf1 = (int)((sf[5][10] & 0xFFE0) >> 5); if ( bit_test_l( iaaf1, 11)) iaaf1 = iaaf1 | 0xF800; gps_alm[isv].af1 = iaaf1*3.637978807E-12; } } } } return;}/*******************************************************************************FUNCTION parity_check()RETURNS None.PARAMETERS None.PURPOSEWRITTEN BY Clifford Kelley*******************************************************************************/void parity_check(void){ long pb1=0x3b1f3480L, pb2=0x1d8f9a40L, pb3=0x2ec7cd00L, 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; // remove 6 parity bits d29 = (m_parity & 0x2) >> 1; d30 = m_parity & 0x1; } }}/*******************************************************************************FUNCTION exor()RETURNS None.PARAMETERS None.PURPOSEWRITTEN 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()RETURNS None.PARAMETERS None.PURPOSEWRITTEN BY Clifford Kelley*******************************************************************************/LLH ecef_to_llh( ECEF pos){ double p,n,thet,esq,epsq; LLH result; 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; return result;}/*******************************************************************************FUNCTION llh_to_ecef()RETURNS None.PARAMETERS None.PURPOSEWRITTEN BY Clifford Kelley*******************************************************************************/ECEF llh_to_ecef(LLH pos){ double n; 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); return result;}/*******************************************************************************FUNCTION pos_vel_time(int)RETURNS None.PARAMETERS None.PURPOSE This routine processes the all-in-view pseudorange to arrive at a receiver positionINPUTS: 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 usedOUTPUTS: RP[3] VECTOR OF RECEIVER POSITION IN ECEF (X,Y,Z) CBIAS RECEIVER CLOCK BIAS FROM GPS TIMEVARIABLES 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 SATELLITESIF THE POSITION CANNOT BE DETERMINED THE RESULT OF RPWILL BE (0,0,0) THE CENTER OF THE EARTHWRITTEN 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// nits = 0; t = 0.0; x = rec_pos_xyz.x; y = rec_pos_xyz.y; z = rec_pos_xyz.z; do { for (i=1;i<=nsl;i++) {//// Compute range in ECI at the time of arrival at the receiver// alpha=(dt[i]-t)*omegae; 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; ms[1][i]=(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x)/r; ms[2][i]=(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y)/r; ms[3][i]=(track_sat[i].z-z)/r; ms[4][i]=1.0; } a1=0.;b1=0.;c1=0.;d1=0.;// e1=0.;?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -