gpsfuncs.c

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

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

  if ((in = fopen( buf, "rt")) == NULL)
  {
    printf( "error opening '%s'.\n", buf);
    for ( id=1; id<=32; id++)
    {
      gps_alm[id].week = gps_week-1;
      gps_alm[id].inc  = 1.0;
    }
  }
  else
  {
    status = warm_start;
    while ( !feof( in))                // GB: replaced eof() by feof()
    {
      fscanf( in, "%45c", &header);
      fscanf( in, "%27c", &text);
      fscanf( in, "%d", &id);
      fscanf( in, "%27c", &text);
      fscanf( in, "%i", &health);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &eccen);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &toa);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &rinc);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &rras);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &sqra);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &ratoa);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &aopg);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &rma);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &af0);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &af1);
      fscanf( in, "%27c", &text);
      fscanf( in, "%i", &week);
      fscanf( in, "%c", &trailer);

      gps_alm[id].health   = health;
      gps_alm[id].week     = week;
      gps_alm[id].toa      = toa;
      gps_alm[id].ety      = eccen;
      gps_alm[id].toa      = toa;
      gps_alm[id].inc      = rinc;
      gps_alm[id].omegadot = rras;
      gps_alm[id].sqra     = sqra;
      gps_alm[id].omega0   = ratoa;
      gps_alm[id].w        = aopg;
      gps_alm[id].ma       = rma;
      gps_alm[id].af0      = af0;
      gps_alm[id].af1      = af1;
      gps_alm[id].sat_file = 0;

      if (gps_alm[id].sqra > 0.0) 
        gps_alm[id].w = 19964981.84/pow(gps_alm[id].sqra,3);
    }
    fclose( in);

    alm_gps_week = week;
    alm_toa      = toa;
  }
  if ( buf)
    free( buf);
  return;
}

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

PARAMETERS None.

PURPOSE
  This function 

WRITTEN BY
  Clifford Kelley

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

void read_ephemeris( void)
{
  int     id,health,week;
  double  toc,toe;
  double  crc, crs, cic, cis, cuc, cus, tgd, ety, inc0, omegadot, w0, w, ma;
  double  daf0, daf1, daf2, esqra;
  float   d_toe;
  char infile[] = "current.eph";
  char *buf;
  FILE *in;

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

  if (( in = fopen( buf, "rt")) == NULL)
  {
    printf( "error opening '%s'.\n", buf);
  }
  else
  {
    while ( !feof( in))                // GB: replaced eof() by feof()
    {

      fscanf( in, "%37c", &header);
      fscanf( in, "%27c", &text);
      fscanf( in, "%i", &id);
      fscanf( in, "%27c", &text);
      fscanf( in, "%i", &health);
      fscanf( in, "%27c", &text);
      fscanf( in, "%i", &week);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &toe);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &toc);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &tgd);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &daf0);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &daf1);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &daf2);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &ety);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &inc0);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &omegadot);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &esqra);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &w0);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &w);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &ma);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &cuc);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &cus);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &crc);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &crs);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &cic);
      fscanf( in, "%27c", &text);
      fscanf( in, "%le", &cis);
      fscanf( in, "%c", &trailer);

      d_toe = clock_tow-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 week is current and time is less than 2 hours old use for hot start
//    note: gps_week is computed from the PCs real time clock
//          and does not roll over at 1024
//
      if ( week == (gps_week % 1024) && fabs( d_toe) < 7200.0) 
        gps_eph[id].valid=1;
      gps_eph[id].health   = health;
      gps_eph[id].week     = week;
      gps_eph[id].toe      = toe;
      gps_eph[id].toc      = toc;
      gps_eph[id].tgd      = tgd;
      gps_eph[id].af0      = daf0;
      gps_eph[id].af1      = daf1;
      gps_eph[id].af2      = daf2;
      gps_eph[id].ety      = ety;
      gps_eph[id].inc0     = inc0;
      gps_eph[id].omegadot = omegadot;
      gps_eph[id].sqra     = esqra;
      gps_eph[id].omega0       = w0;
      gps_eph[id].w        = w;
      gps_eph[id].ma       = ma;
      gps_eph[id].cuc      = cuc;
      gps_eph[id].cus      = cus;
      gps_eph[id].crc      = crc;
      gps_eph[id].crs      = crs;
      gps_eph[id].cic      = cic;
      gps_eph[id].cis      = cis;

      if (gps_eph[id].sqra > 0.0) 
        gps_eph[id].wm = 19964981.84 / pow( gps_eph[id].sqra, 3);
    }
    fclose( in);
  }
  if ( buf)
    free( buf);
  return;
}

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

PARAMETERS None.

PURPOSE
  This function writes the broadcast almanac data to a file for later
      use.  In particular to support a warm or hot start.

WRITTEN BY
  Clifford Kelley

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

void write_almanac( void)
{
  int i;

  out = fopen( "current.alm", "w+");
  if ( !out)
  {
    printf( "write_almanac: error opening file 'current.alm'.\n");
    exit(-1);
  }


  for (i=1;i<=32;i++)
  {
    if ( gps_alm[i].inc > 0.0)
    {
//******** Week  131 almanac for PRN-01 ********
//ID:                         01
//Health:                     000
//Eccentricity:               0.5362033844E-002
//Time of Applicability(s):  589824.0000
//Orbital Inclination(rad):   0.9676266920
//Rate of Right Ascen(r/s):  -0.8126052768E-008
//SQRT(A)  (m 1/2):           5153.620605
//Right Ascen at Week(rad):  -0.1178026838E+001
//Argument of Perigee(rad):  -1.701721362
//Mean Anom(rad):             0.2097295909E+001
//Af0(s):                     0.2183914185E-003
//Af1(s/s):                   0.3637978807E-011
//week:                        131
      fprintf( out, "******** Week %4d almanac for PRN-%02d ********\n",
        gps_alm[i].week % 1024, i);
      fprintf( out, "ID:                         %02d\n", i);
      fprintf( out, "Health:                     %03d\n", gps_alm[i].health);
      fprintf( out, "Eccentricity:               %10.9e\n", gps_alm[i].ety);
      fprintf( out, "Time of Applicability(s):   %10.9e\n", gps_alm[i].toa);
      fprintf( out, "Orbital Inclination(rad):   %10.9e\n", gps_alm[i].inc);
      fprintf( out, "Rate of Right Ascen(R/s):   %10.9e\n", gps_alm[i].omegadot);
      fprintf( out, "SQRT(A) (m^1/2):            %10.9e\n", gps_alm[i].sqra);
      fprintf( out, "Right Ascen at TOA(rad):    %10.9e\n", gps_alm[i].omega0);
      fprintf( out, "Argument of Perigee(rad):   %10.9e\n", gps_alm[i].w);
      fprintf( out, "Mean Anom(rad):             %10.9e\n", gps_alm[i].ma);
      fprintf( out, "Af0(s):                     %10.9e\n", gps_alm[i].af0);
      fprintf( out, "Af1(s/s):                   %10.9e\n", gps_alm[i].af1);
      fprintf( out, "week:                       %4d   \n", gps_alm[i].week % 1024);
      fprintf( out, "\n");
    }
  }
  fclose(out);
}

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

PARAMETERS None.

PURPOSE
  This function writes the broadcast ephemeris data to a file for later
  use.  In particular to support a hot start.

WRITTEN BY
  Clifford Kelley

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

void write_ephemeris()
{
  int i;
  out = fopen( "current.eph", "w+");
  if ( !out)
  {
    printf( "write_ephemeris: error opening file 'current.eph'\n");
    exit(-1);
  }
  for ( i=1; i<=32; i++)
  {
    if ( gps_eph[i].inc0 >0.0)
    {

      fprintf( out, "**** Ephemeris for PRN-%02d ***********\n", i);
      fprintf( out, "ID:                         %3d\n",    i);
      fprintf( out, "Health:                     %3d\n",    gps_eph[i].health);
      fprintf( out, "Week:                       %4d\n",    gps_eph[i].week);
      fprintf( out, "E Time of Applic(s):        %10.9e\n", gps_eph[i].toe);
      fprintf( out, "C Time of Applic(s):        %10.9e\n", gps_eph[i].toc);
      fprintf( out, "Tgd(s):                     %10.9e\n", gps_eph[i].tgd);
      fprintf( out, "Af0(s):                     %10.9e\n", gps_eph[i].af0);
      fprintf( out, "Af1(s/s):                   %10.9e\n", gps_eph[i].af1);
      fprintf( out, "Af2(s/s/s):                 %10.9e\n", gps_eph[i].af2);
      fprintf( out, "Eccentricity:               %10.9e\n", gps_eph[i].ety);
      fprintf( out, "Orbital Inclination(rad):   %10.9e\n", gps_eph[i].inc0);
      fprintf( out, "Rate of Right Ascen(R/s):   %10.9e\n", gps_eph[i].omegadot);
      fprintf( out, "SQRT(A) (m^1/2):            %10.9e\n", gps_eph[i].sqra);
      fprintf( out, "Right Ascen at TOE(rad):    %10.9e\n", gps_eph[i].omega0);
      fprintf( out, "Argument of Perigee(rad):   %10.9e\n", gps_eph[i].w);
      fprintf( out, "Mean Anom(rad):             %10.9e\n", gps_eph[i].ma);
      fprintf( out, "Cuc(rad):                   %10.9e\n", gps_eph[i].cuc);
      fprintf( out, "Cus(rad):                   %10.9e\n", gps_eph[i].cus);
      fprintf( out, "Crc(m):                     %10.9e\n", gps_eph[i].crc);
      fprintf( out, "Crs(m):                     %10.9e\n", gps_eph[i].crs);
      fprintf( out, "Cic(rad):                   %10.9e\n", gps_eph[i].cic);
      fprintf( out, "Cis(rad):                   %10.9e\n", gps_eph[i].cis);
      fprintf( out, "\n");
    }
  }
  fclose(out);
}

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

PARAMETERS None.

PURPOSE
  This function writes the broadcast ionospheric correction data and the
      parameters to tie GPS time to UTC to a file for later use.

WRITTEN BY
  Clifford Kelley

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

void write_ion_utc()
{
  FILE *out;

  out = fopen( "ion_utc.dat", "w+");
  if ( !out)
  {
    printf( "write_ion_utc: error opening file 'ion_utc.dat'\n");
    exit(-1);
  }
  fprintf( out, "al0:                        %e\n", Iono.al0);
  fprintf( out, "al1:                        %e\n", Iono.al1);
  fprintf( out, "al2:                        %e\n", Iono.al2);
  fprintf( out, "al3:                        %e\n", Iono.al3);
  fprintf( out, "b0:                         %f\n", Iono.b0);
  fprintf( out, "b1:                         %f\n", Iono.b1);
  fprintf( out, "b2:                         %f\n", Iono.b2);
  fprintf( out, "b3:                         %f\n", Iono.b3);
  fprintf( out, "a0:                         %f\n", Utc.a0);
  fprintf( out, "a1:                         %f\n", Utc.a1);
  fprintf( out, "dtls:                       %f\n", Utc.dtls);
  fprintf( out, "tot:                        %f\n", Utc.tot);
  fprintf( out, "WNt:                        %f\n", Utc.WNt);
  fprintf( out, "WNlsf:                      %f\n", Utc.WNlsf);
  fprintf( out, "DN:                         %f\n", Utc.DN);
  fprintf( out, "dtlsf:                      %f\n", Utc.dtlsf);
  fclose( out);

  return;
}


/*******************************************************************************
FUNCTION fix_sqrt( long x)
RETURNS  long integer

PARAMETERS
      x long integer

PURPOSE
  This function finds the fixed point square root of a long integer

WRITTEN BY
  Clifford Kelley

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

long fix_sqrt( long x)
{
  long xt, scr;
  int i;

// --- take care of sqrt(0) --- (G.B. 02.04.08)
  if ( !x)
    return (0);

  i  = 0;
  xt = x;

  do
  {
    xt = xt >> 1;
    i++;
  } while( xt>0);

  i  = (i >> 1) + 1;
  xt = x >> i;

  do
  {
     scr = xt * xt;
     scr = x - scr;
     scr = scr >> 1;
     scr = scr / xt;