gpsfuncs.c

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

*******************************************************************************/

ECEFT satpos_ephemeris(double t, char n)
{
  double ei,ea,diff,ta,aol,delr,delal,delinc,r,inc;
  double la,xp,yp,bclk,tc,d_toc,d_toe;
  double xn,yn,zn,xe,ye,xls,yls,zls,range1,ralt,tdot,satang,xaz,yaz;
  double b,az;
  ECEFT result;
//
//     MA IS THE ANGLE FROM PERIGEE AT TOA
//
  d_toc = t-gps_eph[n].toc;
  if ( d_toc>302400.0) 
    d_toc = d_toc-604800.0;
  else if (d_toc<-302400.0)
    d_toc=d_toc+604800.0;
  bclk = gps_eph[n].af0 + gps_eph[n].af1*d_toc + 
         gps_eph[n].af2*d_toc*d_toc - gps_eph[n].tgd;
  tc    = t - bclk;
  d_toe = tc - gps_eph[n].toe;
  if ( d_toe > 302400.0) 
    d_toe = d_toe-604800.0;
  else if (d_toe < -302400.0)
    d_toe = d_toe + 604800.0;
  ei = gps_eph[n].ma + d_toe*gps_eph[n].wm + gps_eph[n].dn;
  ea = ei;
  do
  {
    diff = (ei-(ea-gps_eph[n].ety*sin(ea)))/(1.0E0-gps_eph[n].ety*cos(ea));
    ea   = ea+diff;
  } while ( fabs(diff) > 1.0e-9 );
  bclk      = bclk-4.442807633E-10*gps_eph[n].ety*gps_eph[n].sqra*sin(ea);
  result.tb = bclk;
//
//     ea is the eccentric anomaly
//
  ta = atan2(sqrt(1.00-pow(gps_eph[n].ety,2))*sin(ea),cos(ea)-gps_eph[n].ety);
//
//     TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
//
  aol = ta + gps_eph[n].w;
//
//     AOL IS THE ARGUMENT OF LATITUDE OF THE SATELLITE
//
//     calculate the second harmonic perturbations of the orbit
//
  delr   = gps_eph[n].crc*cos( 2.0*aol) + gps_eph[n].crs*sin( 2.0*aol);
  delal  = gps_eph[n].cuc*cos( 2.0*aol) + gps_eph[n].cus*sin( 2.0*aol);
  delinc = gps_eph[n].cic*cos( 2.0*aol) + gps_eph[n].cis*sin( 2.0*aol);
//
//     R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
//
  r   = pow(gps_eph[n].sqra,2)*(1.00-gps_eph[n].ety*cos(ea))+delr;
  aol = aol+delal;
  inc = gps_eph[n].inc0+delinc+gps_eph[n].idot*d_toe;
//      WRITE(6,*)T-TOE(N)
//
//     LA IS THE CORRECTED LONGITUDE OF THE ASCENDING NODE
//
  la = gps_eph[n].omega0 + (gps_eph[n].omegadot-omegae)*d_toe -
       omegae*gps_eph[n].toe;
  xp = r*cos(aol);
  yp = r*sin(aol);
  result.x  = xp*cos(la)-yp*cos(inc)*sin(la);
  result.y  = xp*sin(la)+yp*cos(inc)*cos(la);
  result.z  = yp*sin(inc);
  result.az = 0.0;
  result.el = 0.0;
  if ( rec_pos_xyz.x != 0.0 || rec_pos_xyz.y != 0.0 || rec_pos_xyz.z != 0.0)
  {
//               fprintf(stream,"x=%20.10lf,y=%20.10lf,z=%20.10lf\n",rec_pos_xyz.x,
//               rec_pos_xyz.y,rec_pos_xyz.z);
/*
      CALCULATE THE POSITION OF THE RECEIVER
*/
    xn  = -cos(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
    yn  = -sin(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
    zn  =  cos(rec_pos_llh.lat);
    xe  = -sin(rec_pos_llh.lon);
    ye  =  cos(rec_pos_llh.lon);
/*
     DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
*/
    xls = result.x-rec_pos_xyz.x;
    yls = result.y-rec_pos_xyz.y;
    zls = result.z-rec_pos_xyz.z;
    range1 = sqrt(xls*xls+yls*yls+zls*zls);
    ralt = sqrt(rec_pos_xyz.x*rec_pos_xyz.x+rec_pos_xyz.y*rec_pos_xyz.y+rec_pos_xyz.z*rec_pos_xyz.z);
    tdot = (rec_pos_xyz.x*xls+rec_pos_xyz.y*yls+rec_pos_xyz.z*zls)/range1/ralt;
    xls = xls/range1;
    yls = yls/range1;
    zls = zls/range1;
    if ( tdot >= 1.00 )
       b=0.0;
    else if ( tdot <= -1.00 )
       b=pi;
    else
    {
       b=acos(tdot);
    }
    satang=pi/2.0-b;
    xaz=xe*xls+ye*yls;
    yaz=xn*xls+yn*yls+zn*zls;
    if (xaz !=0.0 || yaz !=0.0) 
      az = atan2(xaz,yaz);
    else 
      az = 0.0;
    result.el = satang;
    result.az = az;
  }
  return result;
}

/*******************************************************************************
FUNCTION read_initial_data()
RETURNS  None.

PARAMETERS None.

PURPOSE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

void read_initial_data(void)
{
  int   id;
  SATVIS dummy;
  
  for (id=1; id<=32; id++) 
    gps_alm[id].inc = 0.0;

//
//  READ THE INPUT DATA FILE(s)
//
// for initialization, if we have data we will switch to warm or hot start
  status = cold_start;       

  read_ion_utc();
  read_almanac();
  dummy = satfind( 0);
  read_ephemeris();
  thetime = time( NULL);

//  printf( "thetime = %d\n", thetime);
//  getchar();
  return;
}

/*******************************************************************************
FUNCTION receiver_loc()
RETURNS  None.

PARAMETERS None.

PURPOSE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
LLH receiver_loc( void)
{
  float latitude, longitude, height;
  char  text[10];
  LLH   result;
  FILE  *in;

  result.lat = 0.0;
  result.lon = 0.0;
  result.hae = 0.0;

/*
 *    READ THE CURRENT LOCATION DATA FILE
 */
  char infile[] = "curloc.dat";
  char *tmpstr;

  tmpstr = conmalloc( strlen( OGSDataDir) + strlen( infile) + 1);
  strcpy( tmpstr, OGSDataDir);
  strcat( tmpstr, infile);

  if ((in = fopen( tmpstr, "rt")) == NULL)
  {
    printf( "error opening %s.\n", tmpstr);
    status = cold_start;
  }
  else
  {
    fscanf( in, "%10s", text);
    fscanf( in, "%f", &latitude);
    fscanf( in, "%10s", text);
    fscanf( in, "%f", &longitude);
    fscanf( in, "%10s", text);
    fscanf( in, "%f", &height);
    result.lat = latitude / 57.296;
    result.lon = longitude / 57.296;
    result.hae = height;
  }
  fclose( in);
  if ( tmpstr)
    free( tmpstr);
  return (result);
}

/*******************************************************************************
FUNCTION navmess()
RETURNS  None.

PARAMETERS None.

PURPOSE
  This function assembles and decodes the 1500 bit nav message
  into almanac and ephmeris messages

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

void decode_navmess( char prn, char ch)
{
  int i,j,k;
  unsigned long isqra, ie, iomega0;
  long iaf0, iomegadot;
  char itgd, iaf2;
  int iweek, icapl2, iura, ihealth, iodc, iodct, iaf1;
  unsigned int itoe, itoc;
  int iode, icrs, idn, icuc, icus, fif, icic, iomegad;
  int icis, icrc, idoe, idot;
  unsigned int iae, iatoa;
//  static i4page, i5page;
  static int i4page, i5page;   // GB: int inserted
  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;
  float d_toe;

//
//    assemble the bits into subframes and words
//
  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;

  if ( gps_eph[prn].valid==0 || 
       (almanac_valid==0 && almanac_flag==0) || 
       fabs( d_toe) > 7200.0)
  {
    j=0;
    for ( sfr=1; sfr<=5; sfr++)
    {
      for ( word=1;word<=10;word++)
      {
        scale         = 536870912L;  // 2^29
        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

//
//   EPHEMERIS DECODE subframes 1, 2, 3
//
//  subframe 1
//
    if ((p_error[1]==0 || p_error[1]==0x200) && p_error[2]==0 && p_error[3]==0)
    {
      gps_eph[prn].valid=0;
      iodc = (int)(((sf[1][3] & 0x3) <<8 ) | ((sf[1][8] & 0xFF0000L) >> 16));
//    iodct=iodc & 0xff;
//    iode=sf[2][3]  >> 16;
//    idoe=sf[3][10] >> 16;
//    if ( iodct != iode || iodct!= idoe || iode != idoe || gps_eph[prn].sqra==0.0)
//    
      iweek               = (int)(sf[1][3] >> 14);
      gps_eph[prn].week   = iweek;
//      icapl2              = ( sf[1][3] & 0x3000 ) >> 12;
      iura                = (int)(( sf[1][3] & 0xF00 ) >> 8);
      gps_eph[prn].ura    = iura;
      ihealth             = (int)(( sf[1][3] & 0xFC ) >> 2);
      gps_eph[prn].health = ihealth;
      gps_eph[prn].iodc   = iodc;
      itgd                = (int)( sf[1][7] & 0xFF);
      gps_eph[prn].tgd    = itgd*4.656612873e-10;
      itoc                = (int)( sf[1][8] & 0xFFFF);
      gps_eph[prn].toc    = itoc*16.0;
      iaf2                = (int)( sf[1][9] >> 16);
      gps_eph[prn].af2    = iaf2*2.775557562e-17;
      iaf1                = (int)( sf[1][9] & 0xFFFF);
      gps_eph[prn].af1    = iaf1*1.136868377e-13;
      iaf0                = sf[1][10] >> 2;
      if ( bit_test_l( iaf0, 22)) 
        iaf0 = iaf0 | 0xFFC00000L;
      gps_eph[prn].af0    = iaf0*4.656612873e-10;
//
//   subframe 2
//
      icrs = (int)(sf[2][3] & 0xFFFF);
      gps_eph[prn].crs      = icrs*.03125;
      idn                   = (int)(sf[2][4] >> 8);
      gps_eph[prn].dn       = idn*1.136868377e-13*pi;
      im0                   = ((sf[2][4] & 0xFF) << 24) | sf[2][5];
      gps_eph[prn].ma       = im0*4.656612873e-10*pi;
      icuc                  = (int)(sf[2][6] >>8);
      gps_eph[prn].cuc      = icuc*1.862645149e-9;
      ie                    = ((sf[2][6] & 0xFF) << 24) | sf[2][7];
      gps_eph[prn].ety      = ie*1.164153218e-10;
      icus                  = (int)(sf[2][8] >> 8);
      gps_eph[prn].cus      = icus*1.862645149e-9;
      isqra                 = (((sf[2][8] & 0xFF) << 24) | sf[2][9]);
      gps_eph[prn].sqra     = isqra*1.907348633e-6;
      if (gps_eph[prn].sqra>0.0) 
        gps_eph[prn].wm     = 19964981.84/pow(gps_eph[prn].sqra,3);
      itoe                  = (int)(sf[2][10] >> 8);
      gps_eph[prn].toe      = itoe*16.;
//      fif=(sf[2][10] & 0x80) >> 7;
//
// subframe 3
//
      icic                  = (int)(sf[3][3] >> 8);
      gps_eph[prn].cic      = icic*1.862645149e-9;
      icis                  = (int)(sf[3][5] >> 8);
      gps_eph[prn].cis      = icis*1.862645149e-9;
      inc0                  = ((sf[3][5] & 0xFF) << 24) | sf[3][6];
      gps_eph[prn].inc0     = inc0*4.656612873e-10*pi;
      iomega0               = ((sf[3][3] & 0xFF) << 24) | sf[3][4];
      gps_eph[prn].omega0=iomega0*4.656612873e-10*pi;
      icrc=(int)(sf[3][7] >> 8);
      gps_eph[prn].crc=icrc*.03125;
      iw=((sf[3][7] & 0xFF) << 24) | sf[3][8];
      gps_eph[prn].w=iw*4.656612873e-10*pi;
      iomegadot=sf[3][9];
      if (bit_test_l(iomegadot,24)) 
        iomegadot=iomegadot | 0xFF000000L;
      gps_eph[prn].omegadot=iomegadot*1.136868377e-13*pi;
      idot=(int)((sf[3][10] & 0xFFFC) >> 2);
      if (bit_test_l(idot,14)) 
        idot=idot | 0xC000;
      gps_eph[prn].idot=idot*1.136868377e-13*pi;
      if ( gps_eph[prn].inc0 != 0.0 && 
           gps_eph[prn].sqra > 5000.0 &&
           gps_eph[prn].ety < .05) 
        gps_eph[prn].valid = 1;
    }
//
//    ALMANAC DECODE  subframes 4 and 5
//
//    SUBFRAME 4
//
    if (p_error[4]==0 && p_error[5]==0 && almanac_valid==0 && almanac_flag==0)
    {
      almanac_flag=1;
//      i4data= sf[4][3] >> 22;
      i4p = (int)((sf[4][3] & 0x3F0000L) >> 16);
      if ( i4p != i4page)
      {
        i4page=i4p;
        if (i4page > 24 && i4page < 33)
        {
          isv=i4page ;
          gps_alm[isv].week=gps_week;
          iae=(int)(sf[4][3] & 0x00FFFFL);
          gps_alm[isv].ety=iae*4.768371582E-7;
          iatoa=(int)(sf[4][4] >> 16);
          gps_alm[isv].toa=iatoa*4096.0;
          iadeli=sf[4][4] & 0x00FFFFL;
          if (bit_test_l(iadeli,16)) 
            iadeli=iadeli | 0xFFFF0000L;
          gps_alm[isv].inc=(iadeli*1.907348633E-6+0.3)*pi;
          iomegad=(int)(sf[4][5] >> 8);
          gps_alm[isv].omegadot=iomegad*3.637978807E-12*pi;
          gps_alm[isv].health=(int)(sf[4][5] & 0x0000FF);
          iasqr=sf[4][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[4][7];
          if (bit_test_l(iaomega0,24)) 
            iaomega0=iaomega0 | 0xFF000000L;
          gps_alm[isv].omega0=iaomega0*1.192092896E-7*pi;
          iaw=sf[4][8];
          if (bit_test_l(iaw,24)) 
            iaw=iaw | 0xFF000000L;
          gps_alm[isv].w=iaw*1.192092896E-7*pi;
          iam0=sf[4][9];
          if (bit_test_l(iam0,24)) 
            iam0=iam0 | 0xFF000000L;  
          gps_alm[isv].ma=iam0*1.192092896E-7*pi;
//          iaaf0=(sf[4][10] >> 13) | (sf[4][10] & 0x1C);  
          iaaf0=(sf[4][10] >> 13) | ((sf[4][10] & 0x1C) >> 2);  // fixed (GB-020217)  
          if (bit_test_l(iaaf0,11)) 
            iaaf0=iaaf0 | 0xFFFFF800L;
          gps_alm[isv].af0=iaaf0*9.536743164E-7;
          iaaf1=(sf[4][10] | 0xFFE0) >> 5;
          if (bit_test_l(iaaf1,11)) 
            iaaf1=iaaf1 | 0xFFFFF800L;
          gps_alm[isv].af1=iaaf1*3.637978807E-12;
        }
        else if ( i4page == 55 )
        {