ogsionavasc.c

OpenGPSSim是一个gps模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          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 <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 -------------------------------- */

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