ogsnavascii.c

OpenGPSSim是一个gps模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          GPS Simulation                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogsnavascii.cpp                                         *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   24.05.02                                                *
   *                                                                      *
   *    Author:   G. Beyerle                                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * Copyright (C) 2002-2006  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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <assert.h>

#include "ogsdefine.h"
#include "ogsstructs.h"
#include "ogsextern.h"
#include "ogsprototypes.h"
#include "ogslibrary.h"

/* ------------------------------- defines -------------------------------- */

#define SPSIZE 256

/* ------------------------------- globals -------------------------------- */

/* -------------------------- prototypen (global) ------------------------- */

/* ------------------------------ procedures ------------------------------ */

static double read_key_value_double( char *sp)
{
  char *token;

  if ( !sp)
  {
    printf( "Error reading ASCII file.");
    exit(-1);
  }

  token = strtok( sp, "=");
  if ( !token)
  {
    printf( "Error reading ASCII file.");
    printf( "Value: >%s<", sp);
    exit(-1);
  }
  
  token = strtok( NULL, "=");
  if ( !token)
  {
    printf( "Error reading ASCII file.");
    printf( "Value: >%s<", sp);
    exit(-1);
  }

  return ( atof( token));
}

static int read_key_value_int( char *sp)
{
  char *token;

  if ( !sp)
  {
    printf( "Error reading ASCII file.");
    exit(-1);
  }

  token = strtok( sp, "=");
  if ( !token)
  {
    printf( "Error reading ASCII file.");
    printf( "Value: >%s<", sp);
    exit(-1);
  }
  
  token = strtok( NULL, "=");
  if ( !token)
  {
    printf( "Error reading ASCII file.");
    printf( "Value: >%s<", sp);
    exit(-1);
  }

  return ( atoi( token));
}

static long read_key_value_long( char *sp)
{
  char *token;

  if ( !sp)
  {
    printf( "Error reading ASCII file.");
    exit(-1);
  }

  token = strtok( sp, "=");
  if ( !token)
  {
    printf( "Error reading ASCII file.");
    printf( "Value: >%s<", sp);
    exit(-1);
  }
  
  token = strtok( NULL, "=");
  if ( !token)
  {
    printf( "Error reading ASCII file.");
    printf( "Value: >%s<", sp);
    exit(-1);
  }

  return ( atol( token));
}

//
//  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;
}

//
//  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);
  }

  fgets( sp, SPSIZE, fp); nav->sf1how       = read_key_value_long( sp);
  fgets( sp, SPSIZE, fp); nav->sf2how       = read_key_value_long( sp);
  fgets( sp, SPSIZE, fp); nav->sf3how       = read_key_value_long( sp);
  fgets( sp, SPSIZE, fp); nav->eph.ura      = read_key_value_int( sp);
  fgets( sp, SPSIZE, fp); nav->eph.iodc     = read_key_value_int( sp);
  fgets( sp, SPSIZE, fp); nav->eph.iode     = read_key_value_int( sp);
  fgets( sp, SPSIZE, fp); nav->eph.tgd      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.toc      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.toe      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.af2      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.af1      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.af0      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.dn       = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.omegadot = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.idot     = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.cuc      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.cus      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.crc      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.crs      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.cic      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.cis      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.ma       = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.ety      = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.sqra     = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.omega0   = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.inc0     = read_key_value_double( sp);
  fgets( sp, SPSIZE, fp); nav->eph.w        = read_key_value_double( sp);

#if 0
  printf( "nav->sf1how   = %d\n", nav->sf1how);
  printf( "nav->sf2how   = %d\n", nav->sf2how);
  printf( "nav->sf3how   = %d\n", nav->sf3how);
  printf( "nav->eph.ura  = %d\n", nav->eph.ura);
  printf( "nav->eph.iodc = %d\n", nav->eph.iodc);
  printf( "nav->eph.iode = %d\n", nav->eph.iode);
  printf( "nav->eph.tgd  = %e\n", nav->eph.tgd);
  printf( "nav->eph.toc  = %e\n", nav->eph.toc);
  printf( "nav->eph.toe  = %e\n", nav->eph.toe);
  getchar();
#endif

//
//  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)
{
  int i;

  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;