ogsgar2rnx.c

OpenGPSSim是一个gps模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          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 <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#include <time.h>
#include <sys/types.h>

/* ------------------------------- 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<<bit_pos);
  org &= mask;  

  //p_bits(org); printf("\n");
  //p_bits(mask); printf("\n");

  bit&=0x01;
  org = org | (bit<<bit_pos);

  //p_bits(org); printf("\n");
	  
  dest[byte_pos]=org;
  return;
}

//
// Extract L bits starting from Frame[ini-1] as unsigned long int (32 bits max)
//
static ULONG extract_bits( int ini, int L)
{
  ULONG res = 0;
  BYTE *dest = (BYTE*)&res;
  int k, pos_lsb;
  BYTE data;

  ini = ini-1;
  pos_lsb = ini + L - 1;
  for ( k=0; k<L; k++, pos_lsb--) 
  {
    data = peak_bit( pos_lsb, Frame[CurSat]);
    poke_bit( data, LSB[k], dest);
  }
  return res;
}

static int verify_frame( void)
{
 int k, sum = 0;

 for ( k=0; k<30; k++) 
   sum += ChkFrame[CurSat][k];

 k = (30-sum) / 3;

 return (k);
}

static BOOLEAN info_frame( ULONG nfr, char *info_msg)
{
  BYTE page, sv_id;
  int n_bad;
  char *ptr = info_msg;
  BOOLEAN fail = 0;
  BYTE sf_id, preamble;
  ULONG tow;
  ULONG av;
  char av_message[16];

  n_bad = verify_frame();
  if ( n_bad) 
  { 
    ptr += sprintf( ptr, " %d missing words\n", n_bad); 
    fail = 1; 
    return (fail);
  }
 
  preamble = Frame[CurSat][0];
  tow = extract_bits( 25, 17);
 
  if ( preamble != 0x8b) 
  {
    ptr += sprintf( ptr, "Bad preamble "); 
    fail = 1;
  }
  
  if ( tow != nfr) 
  {
    ptr += sprintf( ptr, "Tow missmatch "); 
    fail = 1;
  }
 
  if ( fail) 
  {
    ptr+=sprintf( ptr, "\n");  
   //ptr+=sprintf(ptr,": skipping\n"); //printf("%s",info_msg); 
    return fail;
  } 

  sf_id = (BYTE)extract_bits( 44, 3);  
//  *sf_id_ptr = sf_id;

  ptr += sprintf( ptr, " Subframe %d", sf_id);

//  printf( "\nSF_ID %d\n", sf_id);

  if ( sf_id <= 3) 
  { 
    ptr += sprintf( ptr, "                Ephemeris data\n"); 
    return fail; 
  }

  sv_id = (BYTE)extract_bits(51,6); 
  page  = pages[sv_id];

// printf("SV_ID %d  PAGE %d\n",sv_id,page);

  switch ( sv_id)
  {
  case 0:  
    ptr+=sprintf( ptr, ", page --       ");  
    break; 
    //ptr+=sprintf(ptr,"Dummy sat:");  break;
  case 57: 
    ptr+=sprintf( ptr, ", page %2d*      ",page);  
    break;
    //ptr+=sprintf(ptr,"(1,6,11,16,21) : ");  break;
  case 62: 
    ptr+=sprintf( ptr, ", page %2d*      ",page); 
    break;
    //ptr+=sprintf(ptr,"(12,24)        : "); break;