gpsfuncs.c

C写的用软件无线电实现的GPS模拟程序

          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.

PURPOSE

WRITTEN 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.

PURPOSE

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()
RETURNS  None.

PARAMETERS None.

PURPOSE

WRITTEN 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.

PURPOSE

WRITTEN 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 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
//
  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.;