www.pudn.com > opengpssim.zip > ogsgar2rnx.c
/* ************************************************************************
* *
* GPS Simulation *
* *
* -------------------------------------------------------------------- *
* *
* Module: ogsgar2rnx.cpp *
* *
* Version: 0.1.0 *
* *
* Date: 17.05.02 *
* *
* Authors: Antonio Tabernero, modifications by Georg Beyerle *
* *
* -------------------------------------------------------------------- *
* *
* Copyright (C) 2000-2002 Antonio Tabernero *
* *
* 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' and 'gp2021.cpp' are modi- *
* fied 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 G*RM*N binary data and construct 1500 bit frames *
* *
************************************************************************ */
/********************************************************************
** @source
**
** @author Copyright (C) 2000,2001 Antonio Tabernero
** @version 1.48
** @last modified Ago 28 2001 Antonio Tabernero.
** @last modified Mar 22 2002 Antonio Tabernero.
** @last modified Apr 3 2002 Antonio Tabernero.
** @@
**
** This program is free software; you can redistribute it and/or
** modify it under the terms of the GNU Library 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
** Library General Public License for more details.
**
** You should have received a copy of the GNU Library General Public
** License along with this library; if not, write to the
** Free Software Foundation, Inc., 59 Temple Place - Suite 330,
** Boston, MA 02111-1307, USA.
********************************************************************/
/* ******************************* changes ********************************
1.3 Can use records 0x0e and 0x11 to get aprox time and position
instead of requiring record 0x33 (which seems to be missing
in some models/firmwares)
Output Doppler data (using the flag +doppler) when applied to
binary files logged with the 1.2 (or newer) version of async.
1.4 Added generation of RINEX ephemeris files (-nav option)
Added decoding of the navigation message
Added option to get input from stdin
Some bugs corrected
1.45 -monitor, -V, -sf options
1.48 version published. Minor cosmetic changes from 1.45
************************************************************************ */
/* ------------------------------- includes ------------------------------- */
#include
#include
#include
#include
#include
#include
/* ------------------------------- defines -------------------------------- */
// data bits: bits 30 -> 7 within ULONG NAV_WORD
#define d1 ((NAV_WORD>>29) & 1)
#define d2 ((NAV_WORD>>28) & 1)
#define d3 ((NAV_WORD>>27) & 1)
#define d4 ((NAV_WORD>>26) & 1)
#define d5 ((NAV_WORD>>25) & 1)
#define d6 ((NAV_WORD>>24) & 1)
#define d7 ((NAV_WORD>>23) & 1)
#define d8 ((NAV_WORD>>22) & 1)
#define d9 ((NAV_WORD>>21) & 1)
#define d10 ((NAV_WORD>>20) & 1)
#define d11 ((NAV_WORD>>19) & 1)
#define d12 ((NAV_WORD>>18) & 1)
#define d13 ((NAV_WORD>>17) & 1)
#define d14 ((NAV_WORD>>16) & 1)
#define d15 ((NAV_WORD>>15) & 1)
#define d16 ((NAV_WORD>>14) & 1)
#define d17 ((NAV_WORD>>13) & 1)
#define d18 ((NAV_WORD>>12) & 1)
#define d19 ((NAV_WORD>>11) & 1)
#define d20 ((NAV_WORD>>10) & 1)
#define d21 ((NAV_WORD>> 9) & 1)
#define d22 ((NAV_WORD>> 8) & 1)
#define d23 ((NAV_WORD>> 7) & 1)
#define d24 ((NAV_WORD>> 6) & 1)
// Parity bits: bits 6 -> 1 within ULONG NAV_WORD
#define D25 ((NAV_WORD>> 5) & 1)
#define D26 ((NAV_WORD>> 4) & 1)
#define D27 ((NAV_WORD>> 3) & 1)
#define D28 ((NAV_WORD>> 2) & 1)
#define D29 ((NAV_WORD>> 1) & 1)
#define D30 ((NAV_WORD>> 0) & 1)
// Previous word parity bits 29-30: bits 32 and 31 of ULONG NAV_WORD
#define P29 ((NAV_WORD>>31) & 1)
#define P30 ((NAV_WORD>>30) & 1)
#define RECLEN 256
/* ------------------------------- typedefs ------------------------------- */
typedef unsigned char BOOLEAN;
typedef unsigned char BYTE;
typedef unsigned long ULONG;
typedef unsigned short int UINT;
typedef short int INT;
typedef struct
{
ULONG c50;
BYTE uk[4];
BYTE sv;
} type_rec0x36;
/* ------------------------------- globals -------------------------------- */
static BYTE LSB[32] = { 7, 6, 5, 4, 3, 2, 1, 0,
15,14,13,12,11,10, 9, 8,
23,22,21,20,19,18,17,16,
31,30,29,28,27,26,25,24};
static ULONG NAV_WORD;
static BYTE Frame[32][30]; // 30 bytes = 240 bits = 10 GPS words a 24 bits
static BOOLEAN ChkFrame[32][30];
static int pages[64] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12,13,14,15,
16,17,18,19,20,21,22,23,24, 2, 3, 4, 5, 7, 8, 9,
10,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
-1,-1,-1,25,13,14,15,17,18, 1,19,20,22,23,12,25};
static BYTE CurSat;
extern int ListOnlyPRN;
/* ------------------------------- procedures ----------------------------- */
static type_rec0x36 process_0x36( BYTE *record)
{
type_rec0x36 rec;
int i;
// 50 Hz counter
rec.c50 = *((ULONG*)(record));
// data : 30 bits + 2 parity bits from previous word
for ( i=0; i<4; i++)
rec.uk[i] = record[4+i];
// SV PRN
rec.sv = record[8];
return rec;
}
static void reset_frame( int cursat)
{
// memcpy( frame[CurSat], RESET, 30);
// memcpy( ChkFrame[CurSat], RESET, 30);
memset( Frame[cursat], 0, 30);
memset( ChkFrame[cursat], 0, 30);
}
// Move bits 2 to 25 from org to Frame[] array
static void strip_parity_and_copy_to_frame( BYTE* org, ULONG word, int cursat)
{
BYTE *dest;
int k, index;
BYTE d30;
d30 = (org[3] >> 6) & 0x1;
index = word * 3;
dest = Frame[cursat] + index;
for ( k=0; k<3; k++)
{
dest[k] = ((org[3-k] & 0x3f) << 2) + ((org[2-k] & 0xc0) >> 6);
if ( d30 == 1)
dest[k] = dest[k]^0xff;
ChkFrame[cursat][index+k] = 1;
}
return;
}
//
// Extract bit number nbit from byte stream ptr
//
static BYTE peak_bit(int nbit, BYTE *ptr)
{
int byte_pos = (nbit>>3);
int bit_pos = 7 - (nbit & 0x07);
BYTE res = (ptr[byte_pos] & (1 << bit_pos)) >> bit_pos;
return res;
}
//
// Place bit (0/1) in position nbit of byte stream dest
//
static void poke_bit( BYTE bit, int nbit, BYTE *dest)
{
int byte_pos = (nbit >> 3);
int bit_pos = (nbit & 0x07);
BYTE mask;
BYTE org = dest[byte_pos];
// printf("ORG %d byte %d bit %d\n",org,byte_pos,bit_pos);
bit_pos = 7 - bit_pos;
mask = 0xff - (1<=25) && (sv_id<=32))
ptr+=sprintf( ptr, "Almanac (sat %d)\n", sv_id);
else
ptr+=sprintf( ptr, "Other function\n");
break;
}
}
else
{
switch(page)
{
case 0:
ptr+=sprintf( ptr, "Dummy sat (no data)\n");
break;
case -1:
ptr+=sprintf( ptr, "\n");
break;
case 25:
ptr+=sprintf( ptr, "Sat 1-24 health data\n");
break;
default:
ptr+=sprintf( ptr, "Almanac (sat %d)\n",sv_id);
break;
}
}
return fail;
}
static void process_frame( ULONG nfr)
{
BOOLEAN fail;
BYTE sf_id;
char info[256];
fail = info_frame( nfr, info);
if ( fail)
printf(" ERROR:");
printf( "%s", info);
return;
}
#define MASK_DATA 0x3fffffc0
#define MASK_PARITY 0x0000003f
//
// calc parity bits
//
static BOOLEAN parity( BYTE *ptr)
{
BYTE P,T;
// dest = (BYTE*)&NAV_WORD;
memcpy( (void*)&NAV_WORD, (void*)ptr, 4);
if ( P30)
NAV_WORD ^= MASK_DATA; // source bits complement
P = (BYTE)(P29^d1^d2^d3^d5^d6^d10^d11^d12^d13^d14^d17^d18^d20^d23);
P = (P<<1) + (BYTE)(P30^d2^d3^d4^d6^d7^d11^d12^d13^d14^d15^d18^d19^d21^d24);
P = (P<<1) + (BYTE)(P29^d1^d3^d4^d5^d7^d8^d12^d13^d14^d15^d16^d19^d20^d22);
P = (P<<1) + (BYTE)(P30^d2^d4^d5^d6^d8^d9^d13^d14^d15^d16^d17^d20^d21^d23);
P = (P<<1) + (BYTE)(P30^d1^d3^d5^d6^d7^d9^d10^d14^d15^d16^d17^d18^d21^d22^d24);
P = (P<<1) + (BYTE)(P29^d3^d5^d6^d8^d9^d10^d11^d13^d15^d19^d22^d23^d24);
T = (BYTE)(NAV_WORD&MASK_PARITY);
return (P==T);
}
int copy_to_navbuf( FILE *fd, int sel_sv, char *buf,
long *c50_ptr, int *svprnlist)
{
int j, k, sfid, nof;
static BOOLEAN par, all_par = 1;
static BYTE record[RECLEN], id, len;
static type_rec0x36 rec;
static ULONG nsf, word, // GPS word (0,...,9 !)
current_frame[32];
static long last_word = -1;
static int outputnavbits = 0;
int bufpos = 0, loop = 1;
static int first = 1;
if ( first)
{
first = 0;
if ( !ListOnlyPRN)
{
printf( "-------------------------------------------------------------------------\n");
printf( " ***** Tracking navigation message from PRN %2d *****\n",sel_sv+1);
printf( "-------------------------------------------------------------------------\n");
printf( "Subframe HOW Words in SF Parity/Subframe ID/page Contents\n");
printf( "-------------------------------------------------------------------------\n");
}
for ( k=0; k<32; k++)
{
reset_frame( k);
current_frame[k] = 0xffffffff;
}
}
while ( !feof( fd) && loop)
{
// read records from data file
fread( &id, 1, 1, fd);
fread( &len, 1, 1, fd);
if ( len > RECLEN)
{
printf( "inkonsistent data file\n");
printf( "id = 0x%x, len = %d\n", id, len);
exit(-1);
}
fread( record, 1, len, fd);
if ( id == 0x36)
{
rec = process_0x36( record);
#if 0
printf("rec.uk[0] = %x\n", rec.uk[0]);
printf("rec.uk[1] = %x\n", rec.uk[1]);
printf("rec.uk[2] = %x\n", rec.uk[2]);
printf("rec.uk[3] = %x\n", rec.uk[3]);
printf("rec.sv = %d\n", rec.sv);
printf("rec.c50 = %d\n", rec.c50);
getchar();
#endif
CurSat = rec.sv;
if ( svprnlist)
svprnlist[CurSat+1] += 1;
if ( CurSat == sel_sv)
{
word = ((rec.c50-30) % 300) / 30;
// nsf increases by 1 every subframe (10 GPS words)
nsf = (rec.c50-30) / 300;
if ( current_frame[CurSat] == 0xffffffff)
{
current_frame[CurSat] = nsf;
printf( " HOW %6d [", 6 * nsf);
// printf(" c50 %8d [", rec.c50-9*30);
}
// printf( "\n");
// printf( "nsf = %d\n", nsf);
// printf( "current_frame[CurSat] = %d\n", current_frame[CurSat]);
// getchar();
if ( nsf != current_frame[CurSat])
{
// read first word of next subframe; thus, previous subframe is complete
printf( "] ");
if ( all_par)
process_frame( nsf);
else
printf( " BAD PARITY\n");
reset_frame( CurSat);
current_frame[CurSat] = nsf;
printf( " HOW %6d [", 6 * nsf);
all_par = 1;
last_word = -1;
}
// printf( "\nsfid=%d, word=%d\n", sf_id, word+1);
// getchar();
// if ( sf_id == 1 && word == 0)
// {
// outputnavbits = 1;
// getchar();
// }
for ( k=1; k < (word-last_word); k++)
printf(" ");
par = parity( rec.uk);
if ( !par)
{
outputnavbits = 0;
bufpos = 0;
}
if ( par)
printf("O");
else
printf("X");
all_par &= par;
last_word = word;
// printf( "\n");
// printf( "sfid=%d, word=%d\n", sf_id, word+1);
// for ( k=0; k < 30; k++)
// printf( " %02x", Frame[CurSat][k]);
// printf( "\n");
if ( outputnavbits)
{
for ( k=5; k>=0; k--)
buf[bufpos++] = ((rec.uk[3] >> k) & 0x1) ? 1 : -1;
for ( k=7; k>=0; k--)
buf[bufpos++] = ((rec.uk[2] >> k) & 0x1) ? 1 : -1;
for ( k=7; k>=0; k--)
buf[bufpos++] = ((rec.uk[1] >> k) & 0x1) ? 1 : -1;
for ( k=7; k>=0; k--)
buf[bufpos++] = ((rec.uk[0] >> k) & 0x1) ? 1 : -1;
if ( bufpos >= 1500)
loop = 0;
} // --- if ( outputnavbits) ---
fflush( stdout);
strip_parity_and_copy_to_frame( rec.uk, word, CurSat);
// printf( "sfid=%d, word=%d\n", sf_id, word+1);
// for ( k=0; k < 30; k++)
// printf( " %02x", Frame[CurSat][k]);
// printf( "\n");
// getchar();
// get subframe id from 2nd GPS word
sfid = (Frame[CurSat][5] >> 2) & 0x7;
// printf( "\n");
// printf( "sfid=%d, word=%d\n", sfid, word+1);
// getchar();
// next word is GPS word 1 / subframe 1 -> switch on output
if ( sfid == 5 && word == 9)
outputnavbits = 1;
} // --- if ( CurSat == sel_sv) ---
} // --- if ( id == 0x36) ---
} // --- while ( !feof(fd)) ---
// printf("\n");
*c50_ptr = rec.c50;
return (bufpos);
}
#if 0
//
//
//
int main( int argc, char *argv[])
{
int k, sel_sv, buflen;
long c50;
FILE *fd, *fpout;
fd = fopen( argv[1], "rb");
if ( !fd)
{
printf("error opening file %s.\n", argv[1]);
exit(0);
}
sel_sv = 10; // PRN-1
char buf[1500];
char infile[256];
k = 1;
do
{
buflen = copy_to_navbuf( fd, sel_sv, buf, &c50);
if ( buflen == 1500)
{
sprintf( infile, "nav-%08x.dat", c50);
fpout = fopen( infile, "wb");
if ( !fpout)
{
printf( "error opening file %s\n", infile);
exit(-1);
}
fwrite( buf, buflen, 1, fpout);
fclose( fpout);
}
} while( buflen>0);
fclose( fd);
exit( 0);
return (0);
}
#endif