www.pudn.com > opengpssim.zip > ogsnavascii.c
/* ************************************************************************ * * * 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#include #include #include #include #include #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; 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: 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); } fgets( sp, SPSIZE, fp); nav->sf4pageno = read_key_value_int( sp); nav->sf4svid = page4sf2page[nav->sf4pageno]; fgets( sp, SPSIZE, fp); nav->sf4how = read_key_value_long( sp); // 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); fgets( sp, SPSIZE, fp); if ( strlen( sp) < 85) { printf( "Error reading ASCII file."); printf( "Line: >%s<", sp); exit(-1); } strncpy( nav->text_msg, sp + 62, 22); // printf( "sp:%s\n", sp); // printf( ">%s<\n", nav->text_msg); // getchar(); break; case 18: // ionospheric & UTC fgets( sp, SPSIZE, fp); nav->iono.b0 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->iono.b1 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->iono.b2 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->iono.b3 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->iono.al0 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->iono.al1 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->iono.al2 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->iono.al3 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.a0 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.a1 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.tot = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.WNt = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.dtls = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.WNlsf = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.DN = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->utc.dtlsf = read_key_value_double( sp); break; case 25: for (i=1; i<=32; i++) { // fscanf( fp, "%*60c%d\n", &nav->ASV[i]); fgets( sp, SPSIZE, fp); nav->ASV[i] = read_key_value_int( sp); } // 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]); fgets( sp, SPSIZE, fp); nav->SVh[i] = read_key_value_int( sp); } 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) { int i; 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: 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); } fgets( sp, SPSIZE, fp); nav->sf5pageno = read_key_value_int( sp); nav->sf5svid = page5sf2page[nav->sf5pageno]; fgets( sp, SPSIZE, fp); nav->sf5how = read_key_value_long( sp); 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: fgets( sp, SPSIZE, fp); nav->alm.health = read_key_value_int( sp); fgets( sp, SPSIZE, fp); nav->alm.prn = read_key_value_int( sp); fgets( sp, SPSIZE, fp); nav->alm.ety = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.toa = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.inc = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.omegadot = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.sqra = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.omega0 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.w = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.ma = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.af1 = read_key_value_double( sp); fgets( sp, SPSIZE, fp); nav->alm.af0 = read_key_value_double( sp); break; case 25: for (i=1; i<=32; i++) { // fscanf( fp, "%*60c%d\n", &nav->ASV[i]); fgets( sp, SPSIZE, fp); nav->ASV[i] = read_key_value_int( sp); } // 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]); fgets( sp, SPSIZE, fp); nav->SVh[i] = read_key_value_int( sp); } 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 ----------------------------- */