ogsencdec.c

OpenGPSSim是一个gps模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          GPS Simulation                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogsencdec.cpp                                           *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   17.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' 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                         *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *                 Encode / decode navigation message                   *
   *                                                                      *
   ************************************************************************ */

/* ******************************* changes ********************************

   dd.mm.yy -

   ************************************************************************ */

// to do:

/* ------------------------------- includes ------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h>
#include <assert.h>

#define MAIN

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

#include "ogsnavencode.h"
#include "ogsnavdecode.h"

#undef MAIN

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

//#define XMTRBUFLEN 150000
#define RECLEN 256
#define NBUFLEN 3000
#define TMPNAMELEN 32

/* ------------------------------- structs -------------------------------- */

typedef struct
{
  char          *record;
  unsigned long cnt50hz,
                data_encoded,
                data_decoded;
  int           sv,
                parityok;
} REC0x36;

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

static char NavBit[NAVMSGLEN];

//static unsigned long sf_noparity[6][11];
static unsigned long SubFrame[6][11];
static short int Hist[SUBFRAMELEN]; 
static short int HistInv[SUBFRAMELEN]; 

static NAVDATA Nav;
static int NBufPos = 0;
static char NBuf[NBUFLEN];

char ProgramName[] = "ogsencdec";
static char Version[] = "0.1.1";

//extern char *InputFileName;
extern int ReadAsyncOutput, ReadOGSRcvrOutput;
extern int SatID;
extern int ListOnlyPRN;
extern int optind;

/* ------------------------------ prototypen ------------------------------ */

static int analyze_databit_histogram( int *maxhist, short int hist[]);
static void find_preamble( char message[], int *idx, int *invert);
static void copy_msg_to_subframe( char message[], int ofs, int invert,
  unsigned long sf[6][11]);

void getargs( int argc, char *argv[]);
int copy_to_navbuf( FILE *fd, int sel_sv, char *buf, long *c50_ptr, int *svprnlist);

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

void usage( void)
{
  printf( "usage: %s [options] filename\n", ProgramName);
  printf( "version %s (compiled %s %s) \n", Version, __DATE__, __TIME__);
  printf( " options:\n");
  printf( "   -h        : print this message                  \n");
  printf( "   -v        : verbose                             \n");
  printf( "   -s PRN    : satellite id (PRN)                  \n");
  printf( "   -a        : data from 'async' output            \n");
  printf( "   -l        : list PRN present in async file      \n");
  printf( "   -r        : data from 'ogsrcvr' output          \n");
  exit( 0);
}

void check_options( void)
{
  if ( ReadAsyncOutput && ReadOGSRcvrOutput)
  {
    printf( "Option -a and -r cannot be set both at the same time.\n");
    usage();
    exit( -1);
  }

  if (( SatID < 1 || SatID > 32) && ReadAsyncOutput && !ListOnlyPRN)
  {
    printf( "Satellite id outside allowed range 1-32 (option -s).\n");
    usage();
    exit( -1);
  }
  
  if ( ListOnlyPRN && !ReadAsyncOutput)
  {
    printf( "Option -l allowed only in combination with option -a.\n");
    usage();
    exit( -1);
  }

  return;  
}  

static void conv_async_to_binaryASCII( char *infile)
{
  int i, k, j, len;
  int found = 0, buflen;
  int svprnlist[33];
  long c50;
  unsigned char id, reclen, sv;
  unsigned char rec[RECLEN];
  char *tmpfile;
  char tmpname[TMPNAMELEN];
  static FILE *fp = NULL, *fpout;
  
  for ( i=1; i<=32; i++)
    svprnlist[i] = 0;

  len = strlen( OGSDataDir);

  tmpfile = conmalloc( len+TMPNAMELEN+1);  // ".../data/nav-wwww-ssssss.txt"

  if ( !fp)
  {
    fp = fopen( infile, "rb");
    if ( !fp)
    {
      printf( "Error opening file '%s'.\n", infile);
      exit( -1);
    }
    else
      printf( "Opened file '%s'.\n", infile);
  }

  do
  {
//    buflen = copy_to_navbuf( fp, SatID-1, NavBit, &c50, svprnlist);
    buflen = copy_to_navbuf( fp, SatID-1, NavBit, &c50, svprnlist);

    if ( buflen == NAVMSGLEN)
    {
      found = 1;

      copy_msg_to_subframe( NavBit, 0, 0, SubFrame);
      decode_navmsg( SubFrame, &Nav);

//  write 1500 bits as byte stream (1500 bytes) to file ...
      strcpy( tmpfile, OGSDataDir);
      sprintf( tmpname, "nav-%02d-%04d-%06d-%02d-%02d", SatID, 
        Nav.eph.week, Nav.eph.tow, Nav.sf4pageno, Nav.sf5pageno);
      strcat( tmpfile, tmpname);

      fpout = fopen( tmpfile, "wb");
      if ( !fpout)
      {
        printf( "Error opening file %s.\n", tmpfile);
        exit(-1);
      }
      fwrite( NavBit, NAVMSGLEN, 1, fpout);
      fclose( fpout);

// ... and write as ASCII to file
      strcpy( tmpfile, OGSDataDir);
      sprintf( tmpname, "nav-%02d-%04d-%06d-%02d-%02d.txt", SatID,
        Nav.eph.week, Nav.eph.tow, Nav.sf4pageno, Nav.sf5pageno);
      strcat( tmpfile, tmpname);

      fpout = fopen( tmpfile, "w");
      if ( !fpout)
      {
        printf( "Error opening file %s.\n", tmpfile);
        exit(-1);
      }

//      printf( "\n%s\n", tmpfile);
//      getchar();

      write_sf1to3( &Nav, fpout);
      write_sf4( &Nav, fpout);
      write_sf5( &Nav, fpout);

      fclose( fpout);
//      printf( "file %s written\n", tmpfile);
    }
  } while ( buflen > 0);
  
  if ( feof( fp))
  {
    printf( "\n");
//    printf("NBufPos = %d\n", NBufPos);
    printf( "File %s closed!\n", infile);
    fclose( fp);
    fp = NULL;
  }  
  
  if ( !found || ListOnlyPRN)
  {
    int first = 1;
    printf( "\n");
    if ( !ListOnlyPRN)
      printf( "No data record found for PRN %d\n", SatID);
    printf( "Following PRNs found: ");

    for ( i=1; i<=32; i++)
      if ( svprnlist[i])
      {
        if ( !first)
          printf( ", ", i);
        printf( "%d", i);
        first = 0;
      }  
    printf( "\n");
  }
  
  return;
}

static void conv_ogsrcvr_to_ASCII( char *infile)
{
  int i, k, j, nof, sfno, d30;
  int startidx, invert;
  int buflen;
  int svprnlist[33];
  long c50;
  unsigned char id, reclen, sv;
  unsigned char rec[RECLEN];
  char *ptr;
  FILE *fp;
  
  for ( i=1; i<=32; i++)
    svprnlist[i] = 0;

//  len = strlen( OGSDataDir);

  fp = fopen( infile, "rb");
  if ( !fp)
  {
    printf( "Error opening file '%s'.\n", infile);
    exit( -1);
  }
  else
    printf( "Opened file '%s'.\n", infile);

  nof = fread( NavBit, sizeof( char), NAVMSGLEN, fp);
  if ( nof != NAVMSGLEN)
  {
    printf( "File %s contains just %d bytes (should be 1500).\n", infile, nof);
    exit(-1);
  }

//  for ( i=0; i<=40; i++)
//    printf( "%d ", NavBit[i]);
//  printf( "Read %d bytes.\n", nof);

  find_preamble( NavBit, &startidx, &invert);

  printf( "Preamble found at byte offset %d.\n", startidx);
  if ( invert)
    printf( "Bits are sign inverted.\n");

  copy_msg_to_subframe( NavBit, startidx, invert, SubFrame);
  decode_navmsg( SubFrame, &Nav);

//  place first subframe at beginning

//  re-read data from preamble location
  fseek( fp, startidx, SEEK_SET);
  nof = fread( NavBit, sizeof( char), NAVMSGLEN, fp);
  if ( nof != NAVMSGLEN)
  {
    printf( "Read only %d bytes (should be 1500).\n", nof);
    exit(-1);
  }

  fclose( fp);

  copy_msg_to_subframe( NavBit, 0, invert, SubFrame);
  decode_navmsg( SubFrame, &Nav);

//  place first subframe at beginning
  sfno = Nav.sf1how & 0x7;

//  re-arrange
  ptr = conmalloc( NAVMSGLEN);

  for ( i=0; i<NAVMSGLEN; i++)
    ptr[i] = NavBit[(i+(6-sfno)*300)%1500];

  for ( i=0; i<NAVMSGLEN; i++)
    NavBit[i] = ptr[i];

  free( ptr);

  copy_msg_to_subframe( NavBit, 0, 0, SubFrame);
  decode_navmsg( SubFrame, &Nav);

//  sfno = Nav.sf1how & 0x7;
//  printf( "sfno1 %d.\n", sfno);
//  sfno = Nav.sf1how & 0x7;

//  write to file
  ptr = conmalloc( strlen( infile) + 5);

  strcpy( ptr, infile);
  strcat( ptr, ".txt");

  fp = fopen( ptr, "w");
  if ( !fp)
  {
    printf( "Error opening file %s.\n", ptr);
    exit(-1);
  }

  write_sf1to3( &Nav, fp);
  write_sf4( &Nav, fp);
  write_sf5( &Nav, fp);

  printf( "Finished writing file '%s'.\n", ptr);

  free( ptr);

  return;
}

//
//  copy frame buffer sf[][] to 1500 byte nav msg array
//
static void copy_subframe_to_msg( unsigned long sf[6][11], char msg[])
{
  int i, j, sfr, word;
  unsigned long scale;

  j=0;
  for ( sfr=1; sfr<=5; sfr++)
  {
    for ( word=1; word<=10; word++)
    {
      scale = 0x1L << 29; 

      for ( i=0; i<30; i++)
      {

        if ( sf[sfr][word] & scale)
          msg[j] = 1;