www.pudn.com > opengpssim.zip > ogsionavasc.c

/* ************************************************************************  
   *                                                                      * 
   *                          GPS Simulation                              * 
   *                                                                      * 
   * -------------------------------------------------------------------- * 
   *                                                                      * 
   *    Module:   ogsionavasc.cpp                                         * 
   *                                                                      * 
   *   Version:   0.1                                                     * 
   *                                                                      * 
   *      Date:   24.05.02                                                * 
   *                                                                      * 
   *    Author:   G. Beyerle                                              * 
   *                                                                      * 
   * -------------------------------------------------------------------- * 
   *                                                                      * 
   * Copyright (C) 2002  Georg Beyerle                                    * 
   *                                                                      * 
   * This program is free software; you can redistribute it and/or modify * 
   * it under the terms of the GNU General Public License as published by * 
   * the Free Software Foundation; either version 2 of the License, or    * 
   * (at your option) any later version.                                  * 
   *                                                                      * 
   * This program is distributed in the hope that it will be useful,      * 
   * but WITHOUT ANY WARRANTY; without even the implied warranty of       * 
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        * 
   * GNU General Public License for more details.                         * 
   *                                                                      * 
   * You should have received a copy of the GNU General Public License    * 
   * along with this program; if not, write to the Free Software          * 
   * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.            * 
   *                                                                      * 
   * -------------------------------------------------------------------- * 
   *                                                                      * 
   * The files 'gpsfuncs.cpp', 'gpsrcvr.cpp' are modified versions of     * 
   * the files with the same name from Clifford Kelley's OpenSourceGPS    * 
   * distribution. The unmodified files can be obtained from              * 
   *   http://www.home.earthlink.net/~cwkelley                            * 
   *                                                                      * 
   * -------------------------------------------------------------------- * 
   *                                                                      * 
   *                 Read & write nav data in ASCII format                * 
   *                                                                      * 
   ************************************************************************ */ 
 
/* ******************************* changes ******************************** 
 
   dd.mm.yy - 
 
   ************************************************************************ */ 
 
/* ------------------------------- includes ------------------------------- */ 
 
#include  
#include  
#include  
#include  
#include  
#include  
 
#include "ogsdefine.h" 
#include "ogsstructs.h" 
#include "ogsextern.h" 
#include "ogsprototypes.h" 
#include "ogslibrary.h" 
 
/* ------------------------------- defines -------------------------------- */ 
 
/* ------------------------------- globals -------------------------------- */ 
 
/* -------------------------- prototypen (global) ------------------------- */ 
 
/* ------------------------------ procedures ------------------------------ */ 
 
 
// 
//  write nav data subframe 1 to 3 to ASCII file 
// 
void write_sf1to3( NAVDATA *nav, FILE *fp) 
{ 
  fprintf( fp, "[subframe 1-3 : satellite clock, health data and ephemeris data]\n"); 
  fprintf( fp, "hand over word subframe 1                              how = %d\n", nav->sf1how); 
  fprintf( fp, "hand over word subframe 2                              how = %d\n", nav->sf2how); 
  fprintf( fp, "hand over word subframe 3                              how = %d\n", nav->sf3how); 
  fprintf( fp, "user range accuracy index                              ura = %d\n", nav->eph.ura); 
  fprintf( fp, "issue of date, clock                                  iodc = %d\n", nav->eph.iodc); 
  fprintf( fp, "issue of date, ephemeris                              iode = %d\n", nav->eph.iode); 
  fprintf( fp, "satellite group delay differential [sec]               tgd = %.4e\n", nav->eph.tgd); 
  fprintf( fp, "clock data reference time [sec]                        toc = %.6e\n", nav->eph.toc); 
  fprintf( fp, "reference time ephemeris [sec]                         toe = %.6e\n", nav->eph.toe); 
  fprintf( fp, "clock polynomial correction parameter 2 [sec/sec^2]    af2 = %.4e\n", nav->eph.af2); 
  fprintf( fp, "clock polynomial correction parameter 1 [sec/sec]      af1 = %.6e\n", nav->eph.af1); 
  fprintf( fp, "clock polynomial correction parameter 0 [sec]          af0 = %.8e\n", nav->eph.af0); 
  fprintf( fp, "mean motion correction [semi-circles/sec]               dn = %.6e\n", nav->eph.dn); 
  fprintf( fp, "rate of right ascension [semi-circles/sec]        omegadot = %.6e\n", nav->eph.omegadot); 
  fprintf( fp, "rate of inclination angle [semi-circles/sec]          idot = %.6e\n", nav->eph.idot); 
  fprintf( fp, "cosine harm. corr. to arg. of latitude [rad]           cuc = %.6e\n", nav->eph.cuc); 
  fprintf( fp, "sine harm. corr. to arg. of latitude [rad]             cus = %.6e\n", nav->eph.cus); 
  fprintf( fp, "cosine harm. corr. to orbit radius [m]                 crc = %.6e\n", nav->eph.crc); 
  fprintf( fp, "sine harm. corr. to orbit radius [m]                   crs = %.6e\n", nav->eph.crs); 
  fprintf( fp, "cosine harm. corr. to angle of incl. [rad]             cic = %.6e\n", nav->eph.cic); 
  fprintf( fp, "sine harm. corr. to angle of inclination [rad]         cis = %.6e\n", nav->eph.cis); 
  fprintf( fp, "mean anomaly at reference time [semi-circles]           ma = %.10e\n", nav->eph.ma); 
  fprintf( fp, "eccentricity                                             e = %.10e\n", nav->eph.ety); 
  fprintf( fp, "square root of semi-major axis [m^1/2]                sqra = %.10e\n", nav->eph.sqra); 
  fprintf( fp, "lon. asc. node of orbit plane at weekly epoch       omega0 = %.9e\n", nav->eph.omega0); 
  fprintf( fp, "inclination angle at reference time [semi-circles]    inc0 = %.10e\n", nav->eph.inc0); 
  fprintf( fp, "argument of perigee [semi-circles]                       w = %.10e\n", nav->eph.w); 
  fprintf( fp, "[end subframe 1-3]\n"); 
 
  return; 
} 
 
#define SPSIZE 256 
 
// 
//  read nav data subframe 1 to 3 from ASCII file 
// 
void read_sf1to3( NAVDATA *nav, FILE *fp) 
{ 
  char sp[SPSIZE], *cp; 
 
  cp = fgets( sp, SPSIZE, fp); 
  if ( !cp) 
  { 
    printf( "error reading nav file\n"); 
    exit(-1); 
  } 
 
  fscanf( fp, "%*60c%d\n", &nav->sf1how); 
  fscanf( fp, "%*60c%d\n", &nav->sf2how); 
  fscanf( fp, "%*60c%d\n", &nav->sf3how); 
  fscanf( fp, "%*60c%d\n", &nav->eph.ura); 
  fscanf( fp, "%*60c%d\n", &nav->eph.iodc); 
  fscanf( fp, "%*60c%d\n", &nav->eph.iode); 
  fscanf( fp, "%*60c%e\n", &nav->eph.tgd); 
  fscanf( fp, "%*60c%e\n", &nav->eph.toc); 
  fscanf( fp, "%*60c%e\n", &nav->eph.toe); 
  fscanf( fp, "%*60c%e\n", &nav->eph.af2); 
  fscanf( fp, "%*60c%e\n", &nav->eph.af1); 
  fscanf( fp, "%*60c%e\n", &nav->eph.af0); 
  fscanf( fp, "%*60c%e\n", &nav->eph.dn); 
  fscanf( fp, "%*60c%e\n", &nav->eph.omegadot); 
  fscanf( fp, "%*60c%e\n", &nav->eph.idot); 
  fscanf( fp, "%*60c%e\n", &nav->eph.cuc); 
  fscanf( fp, "%*60c%e\n", &nav->eph.cus); 
  fscanf( fp, "%*60c%e\n", &nav->eph.crc); 
  fscanf( fp, "%*60c%e\n", &nav->eph.crs); 
  fscanf( fp, "%*60c%e\n", &nav->eph.cic); 
  fscanf( fp, "%*60c%e\n", &nav->eph.cis); 
  fscanf( fp, "%*60c%e\n", &nav->eph.ma); 
  fscanf( fp, "%*60c%e\n", &nav->eph.ety); 
  fscanf( fp, "%*60c%e\n", &nav->eph.sqra); 
  fscanf( fp, "%*60c%e\n", &nav->eph.omega0); 
  fscanf( fp, "%*60c%e\n", &nav->eph.inc0); 
  fscanf( fp, "%*60c%e\n", &nav->eph.w); 
 
// 
//  simple data file consistency check 
// 
  char pattern[] = "[end subframe 1-3]"; 
 
  cp = fgets( sp, SPSIZE, fp); 
 
  if ( !cp || strncmp( sp, pattern, strlen( pattern))) 
  { 
    printf( "error reading nav file\n"); 
    printf( "expected string >%s<, read >%s<\n", pattern, sp); 
    exit(-1); 
  } 
 
  return; 
} 
 
// 
//  write nav data subframe 4 to ASCII file 
// 
void write_sf4( NAVDATA *nav, FILE *fp) 
{ 
  fprintf( fp, "[subframe 4 : support data]\n"); 
  fprintf( fp, "page number                                         pageno = %d\n", nav->sf4pageno); 
  fprintf( fp, "hand over word                                         how = %d\n", nav->sf4how); 
 
  switch ( nav->sf4pageno) 
  { 
  case 0: 
    break; 
 
  case  2:  
  case  3:  
  case  4:  
  case  5:  
  case  7:  
  case  8:  
  case  9:  
  case 10:  
    fprintf( fp, "satellite health                                    health = %d\n", nav->alm.health); 
    fprintf( fp, "satellite id number                                     id = %d\n", nav->alm.prn); 
    fprintf( fp, "eccentricity                                             e = %.10e\n", nav->alm.ety); 
    fprintf( fp, "almanac reference time                                 toa = %.4e\n", nav->alm.toa); 
    fprintf( fp, "incl. angle at ref. time rel. to 0.3 of RA             inc = %.10e\n", nav->alm.inc); 
    fprintf( fp, "rate of right ascension [semi-circles/sec]        omegadot = %.6e\n", nav->alm.omegadot); 
    fprintf( fp, "square root of semi-major axis [m^1/2]                sqra = %.9e\n", nav->alm.sqra); 
    fprintf( fp, "lon. asc. node of orbit plane at weekly epoch       omega0 = %.9e\n", nav->alm.omega0); 
    fprintf( fp, "argument of perigee [semi-circles]                       w = %.9e\n", nav->alm.w); 
    fprintf( fp, "mean anomaly at reference time [semi-circles]           ma = %.9e\n", nav->alm.ma); 
    fprintf( fp, "clock polyn. corr. parameter [sec/sec]                 af1 = %.5e\n", nav->alm.af1); 
    fprintf( fp, "clock polyn. corr. parameter [sec]                     af0 = %.5e\n", nav->alm.af0); 
    break; 
 
  case  1: 
  case  6: 
  case 11: 
  case 12: 
  case 16: 
  case 19: 
  case 20: 
  case 21: 
  case 22: 
  case 23: 
  case 24: 
    fprintf( fp, "reserved\n"); 
    break; 
 
  case 13: 
  case 14: 
  case 15: 
    fprintf( fp, "spare satellite\n"); 
    break; 
 
  case 17: 
    fprintf( fp, "special message                                            = '%22s'\n", nav->text_msg); 
    break; 
 
  case 18: 
//  ionospheric & UTC 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.b0); 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.b1); 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.b2); 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.b3); 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.al0); 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.al1); 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.al2); 
    fprintf( fp, "                                                           = %.4f\n", nav->iono.al3); 
    fprintf( fp, "                                                           = %.10e\n", nav->utc.a0); 
    fprintf( fp, "                                                           = %.8e\n", nav->utc.a1); 
    fprintf( fp, "reference time for UTC data                            tot = %.4e\n", nav->utc.tot); 
    fprintf( fp, "UTC reference week number                              WNt = %.4e\n", nav->utc.WNt); 
    fprintf( fp, "delta time due to leap seconds                        dtls = %.4e\n", nav->utc.dtls); 
    fprintf( fp, "week number                                          WNlsf = %.4e\n", nav->utc.WNlsf); 
    fprintf( fp, "day number                                              DN = %.4e\n", nav->utc.DN); 
    fprintf( fp, "delta time due to leap seconds                       dtlsf = %.4e\n", nav->utc.dtlsf); 
    break; 
 
  case 25: 
    int i; 
    for ( i=1; i<=32; i++) 
      fprintf( fp, "anti-spoofing and SV config %2d                      as_cfg = %04d\n", i, nav->ASV[i]); 
 
//  SV PRN 1-24 are written in subframe 5 / page 25 
    for ( i=25; i<=32; i++) 
      fprintf( fp, "SV %2d health bits                                   health = %06d\n", i, nav->SVh[i]); 
    break; 
 
  default: 
    printf( "\n"); 
    printf( "unknown page number %d in subframe 4\n", nav->sf4pageno); 
    exit(-1); 
    break; 
  }   
  fprintf( fp, "[end subframe 4]\n"); 
  return; 
} 
 
void read_sf4( NAVDATA *nav, FILE *fp) 
{ 
  int i; 
  char sp[SPSIZE], *cp; 
 
  cp = fgets( sp, SPSIZE, fp); 
  if ( !cp) 
  { 
    printf( "error reading nav file\n"); 
    exit(-1); 
  } 
//  printf( "sp = >%s<\n", sp); 
//  cp = fgets( sp, SPSIZE, fp); 
//  printf( "sp = >%s<\n", sp); 
 
  fscanf( fp, "%*60c%d\n", &nav->sf4pageno); 
  fscanf( fp, "%*60c%d\n", &nav->sf4how); 
 
//  printf( "nav->sf4pageno = %d\n", nav->sf4pageno); 
//  printf( "nav->sf4how    = %d\n", nav->sf4how); 
//  getchar(); 
 
  switch ( nav->sf4pageno) 
  { 
  case 0: 
    break; 
 
  case  1: 
  case  6: 
  case 11: 
  case 12: 
  case 16: 
  case 19: 
  case 20: 
  case 21: 
  case 22: 
  case 23: 
  case 24: 
    fgets( sp, SPSIZE, fp); 
    break; 
 
  case 13: 
  case 14: 
  case 15: 
    fgets( sp, SPSIZE, fp); 
    break; 
 
  case 17: 
    fscanf( fp, "%*60c%22s\n", nav->text_msg); 
    break; 
 
  case 18: 
//  ionospheric & UTC 
    fscanf( fp, "%*60c%e\n", &nav->iono.b0); 
    fscanf( fp, "%*60c%e\n", &nav->iono.b1); 
    fscanf( fp, "%*60c%e\n", &nav->iono.b2); 
    fscanf( fp, "%*60c%e\n", &nav->iono.b3); 
    fscanf( fp, "%*60c%e\n", &nav->iono.al0); 
    fscanf( fp, "%*60c%e\n", &nav->iono.al1); 
    fscanf( fp, "%*60c%e\n", &nav->iono.al2); 
    fscanf( fp, "%*60c%e\n", &nav->iono.al3); 
    fscanf( fp, "%*60c%e\n", &nav->utc.a0); 
    fscanf( fp, "%*60c%e\n", &nav->utc.a1); 
    fscanf( fp, "%*60c%e\n", &nav->utc.tot); 
    fscanf( fp, "%*60c%e\n", &nav->utc.WNt); 
    fscanf( fp, "%*60c%e\n", &nav->utc.dtls); 
    fscanf( fp, "%*60c%e\n", &nav->utc.WNlsf); 
    fscanf( fp, "%*60c%e\n", &nav->utc.DN); 
    fscanf( fp, "%*60c%e\n", &nav->utc.dtlsf); 
    break; 
 
  case 25: 
    int i; 
    for (i=1; i<=32; i++) 
      fscanf( fp, "%*60c%d\n", &nav->ASV[i]); 
//  SV PRN 1-24 are written in subframe 5 / page 25 
    for (i=25; i<=32; i++) 
      fscanf( fp, "%*60c%d\n", &nav->SVh[i]); 
    break; 
 
  default: 
    printf( "\n"); 
    printf( "unknown page number %d in subframe 4\n", nav->sf4pageno); 
    exit(-1); 
    break; 
  }   
 
// 
//  simple data file consistency check 
// 
  char pattern[] = "[end subframe 4]"; 
 
  cp = fgets( sp, SPSIZE, fp); 
 
  if ( !cp || strncmp( sp, pattern, strlen( pattern))) 
  { 
    printf( "error reading nav file\n"); 
    printf( "expected string >%s<, read >%s<\n", pattern, sp); 
    exit(-1); 
  } 
 
  return; 
} 
 
// 
//  write nav data subframe 5 to ASCII file 
// 
void write_sf5( NAVDATA *nav, FILE *fp) 
{ 
 
  fprintf( fp, "[subframe 5 : support data]\n"); 
  fprintf( fp, "page number                                         pageno = %d\n", nav->sf5pageno); 
  fprintf( fp, "hand over word                                         how = %d\n", nav->sf5how); 
 
  switch ( nav->sf5pageno) 
  { 
  case 0: 
    break; 
  case  1: case  2: case  3: case  4: case  5: case  6: case  7: case  8: case  9: case 10: 
  case 11: case 12: case 13: case 14: case 15: case 16: case 17: case 18: case 19: case 20: 
  case 21: case 22: case 23: case 24: 
    fprintf( fp, "satellite health                                    health = %d\n", nav->alm.health); 
    fprintf( fp, "satellite id number                                     id = %d\n", nav->alm.prn); 
    fprintf( fp, "eccentricity                                             e = %.10e\n", nav->alm.ety); 
    fprintf( fp, "almanac reference time                                 toa = %.4e\n", nav->alm.toa); 
    fprintf( fp, "incl. angle at ref. time rel. to 0.3 of RA             inc = %.10e\n", nav->alm.inc); 
    fprintf( fp, "rate of right ascension [semi-circles/sec]        omegadot = %.6e\n", nav->alm.omegadot); 
    fprintf( fp, "square root of semi-major axis [m^1/2]                sqra = %.9e\n", nav->alm.sqra); 
    fprintf( fp, "lon. asc. node of orbit plane at weekly epoch       omega0 = %.9e\n", nav->alm.omega0); 
    fprintf( fp, "argument of perigee [semi-circles]                       w = %.9e\n", nav->alm.w); 
    fprintf( fp, "mean anomaly at reference time [semi-circles]           ma = %.9e\n", nav->alm.ma); 
    fprintf( fp, "clock polyn. corr. parameter [sec/sec]                 af1 = %.5e\n", nav->alm.af1); 
    fprintf( fp, "clock polyn. corr. parameter [sec]                     af0 = %.5e\n", nav->alm.af0); 
    break; 
 
  case 25: 
    int i; 
    for (i=1; i<=24; i++) 
      fprintf( fp, "SV %2d health bits                                   health = %06d\n", i, nav->SVh[i]); 
    break; 
 
  default: 
    printf( "\n"); 
    printf( "unknown page number %d in subframe 5\n", nav->sf5pageno); 
    exit(-1); 
    break; 
  }   
  fprintf( fp, "[end subframe 5]\n"); 
  return; 
} 
 
void read_sf5( NAVDATA *nav, FILE *fp) 
{ 
  int i; 
  char sp[SPSIZE], *cp; 
 
  cp = fgets( sp, SPSIZE, fp); 
  if ( !cp) 
  { 
    printf( "error reading nav file\n"); 
    exit(-1); 
  } 
 
  fscanf( fp, "%*60c%d\n", &nav->sf5pageno); 
  fscanf( fp, "%*60c%d\n", &nav->sf5how); 
 
  switch ( nav->sf5pageno) 
  { 
  case  1: case  2: case  3: case  4: case  5: case  6: case  7: case  8: case  9: case 10: 
  case 11: case 12: case 13: case 14: case 15: case 16: case 17: case 18: case 19: case 20: 
  case 21: case 22: case 23: case 24: 
    fscanf( fp, "%*60c%d\n", &nav->alm.health); 
    fscanf( fp, "%*60c%d\n", &nav->alm.prn); 
    fscanf( fp, "%*60c%e\n", &nav->alm.ety); 
    fscanf( fp, "%*60c%e\n", &nav->alm.toa); 
    fscanf( fp, "%*60c%e\n", &nav->alm.inc); 
    fscanf( fp, "%*60c%e\n", &nav->alm.omegadot); 
    fscanf( fp, "%*60c%e\n", &nav->alm.sqra); 
    fscanf( fp, "%*60c%e\n", &nav->alm.omega0); 
    fscanf( fp, "%*60c%e\n", &nav->alm.w); 
    fscanf( fp, "%*60c%e\n", &nav->alm.ma); 
    fscanf( fp, "%*60c%e\n", &nav->alm.af1); 
    fscanf( fp, "%*60c%e\n", &nav->alm.af0); 
    break; 
 
  case 25: 
    int i; 
    for (i=1; i<=32; i++) 
      fscanf( fp, "%*60c%d\n", &nav->ASV[i]); 
//  SV PRN 1-24 are written in subframe 5 / page 25 
    for (i=25; i<=32; i++) 
      fscanf( fp, "%*60c%d\n", &nav->SVh[i]); 
    break; 
 
  default: 
    printf( "\n"); 
    printf( "unknown page number %d in subframe 4\n", nav->sf5pageno); 
    exit(-1); 
    break; 
  }   
 
// 
//  simple data file consistency check 
// 
  char pattern[] = "[end subframe 5]"; 
 
  cp = fgets( sp, SPSIZE, fp); 
 
  if ( !cp || strncmp( sp, pattern, strlen( pattern))) 
  { 
    printf( "error reading nav file\n"); 
    printf( "expected string >%s<, read >%s<\n", pattern, sp); 
    exit(-1); 
  } 
 
  return; 
} 
 
/* ------------------------------ end of file ----------------------------- */