ogslibrary.c

C写的用软件无线电实现的GPS模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          GPS Simulation                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogslibrary.cpp                                          *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   02.03.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' 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                         *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *           Library routines for OpenSourceGPS GPS simulator           *
   *                                                                      *
   ************************************************************************ */

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

   dd.mm.yy -

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

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

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

#ifdef unix
#include <unistd.h>
#endif

#define OSGLIBRY_H

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

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

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

/* -------------------------- prototypes (global) ------------------------- */

static int exor_long( unsigned long x);

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

//
//  read key:value pair from parameter file
//
int read_key_value_pair_double( char *tok, char pattern[], double *val, char sep[])
{
  char *token;
  int ret = 0;

  if ( strstr( tok, pattern))
  {
    token = strtok( NULL, sep);
    if ( token) 
    {
      sscanf( token, "%e", val);
      printf( "%s = %f\n", pattern, *val);
      ret = 1;
    }  
  }
  return (ret);
}  

//
//  read key:value pair from parameter file
//
int read_key_value_pair_float( char *tok, char pattern[], float *val, char sep[])
{
  char *token;
  int ret = 0;

  if ( strstr( tok, pattern))
  {
    token = strtok( NULL, sep);
    if ( token) 
    {
      sscanf( token, "%f", val);
      printf( "%s = %f\n", pattern, *val);
      ret = 1;
    }  
  }
  return (ret);
}  

int read_key_value_pair_int( char *tok, char pattern[], int *val, char sep[])
{
  char *token;
  int ret = 0;

  if ( strstr( tok, pattern))
  {
    token = strtok( NULL, sep);
    if ( token) 
    {
      sscanf( token, "%d", val);
      printf( "%s = %d\n", pattern, *val);
      ret = 1;
    }  
  }
  return (ret);
}  

int read_key_value_pair_uint( char *tok, char pattern[], unsigned int *val, char sep[])
{
  char *token;
  int ret = 0;

  if ( strstr( tok, pattern))
  {
    token = strtok( NULL, sep);
    if ( token) 
    {
      sscanf( token, "%d", val);
      printf( "%s = %d\n", pattern, *val);
      ret = 1;
    }  
  }
  return (ret);
}  

int read_key_value_pair_long( char *tok, char pattern[], long *val, char sep[])
{
  char *token;
  int ret = 0;

  if ( strstr( tok, pattern))
  {
    token = strtok( NULL, sep);
    if ( token) 
    {
      sscanf( token, "%d", val);
      printf( "%s = %d\n", pattern, *val);
      ret = 1;
    }  
  }
  return (ret);
}  


//
//  create C/A code and write to global variable CACODE[][]
//
//  adapted from MATLAB routine written by
//  Fredrik Johansson, Rahman Mollaei, Jonas Thor, Joergen Uusitalo
//  Lulea University of Technology, Sweden
//  http://www.sm.luth.se/csee/courses/sms/019/1998/navstar/navstar.html
//
void calc_cacode( void)
{
  int prn;    // 1,...,32 
  int s1, s2;
  int i, j, tmp;

  int tap[32][2] = 
    {{2,  6}, {3, 7}, {4, 8}, {5, 9}, {1, 9}, 
     {2, 10}, {1, 8}, {2, 9}, {3,10}, {2, 3},
     {3,  4}, {5, 6}, {6, 7}, {7, 8}, {8, 9}, 
     {9, 10}, {1, 4}, {2, 5}, {3, 6}, {4, 7}, 
     {5,  8}, {6, 9}, {1, 3}, {4, 6}, {5, 7}, 
     {6,  8}, {7, 9}, {8,10}, {1, 6}, {2, 7}, 
     {3,  8}, {4, 9}};

// loop over all PRNs
  for ( prn=1; prn<=NOFSAT; prn++)
  {

// initial state
    int g1[11] = {0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},  // we don't use first element
        g2[11] = {0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};

// select taps for G2 delay
    s1 = tap[prn-1][0];
    s2 = tap[prn-1][1];
  
    for ( i=0; i<NOFCHIPS; i++)
    {
//  Gold code is obtained from direct G1 output and delayed G2 output
      CACode[prn][i] = -g1[10] * g2[s1] * g2[s2];

//  Generator 1 - shift reg 1  (1+X^3+X^10)
      tmp = g1[3]*g1[10];
      for ( j=10; j>1; j--)
        g1[j] = g1[j-1];
      g1[1] = tmp;
    
//  Generator 2 - shift reg 2  (1+X^2+X^3+X^6+X^8+X^9+X^10)
      tmp = g2[2]*g2[3]*g2[6]*g2[8]*g2[9]*g2[10];
      for ( j=10; j>1; j--)
        g2[j] = g2[j-1];
      g2[1] = tmp;
    }
  }
  return;
}

#if 0

//
//  encode navigation data to array sf2[][] and copy to message[1500] buffer
//
void encode_navmess( char prn, char ch, int i4satid, int i5satid, 
  IONODATA *iono, UTCDATA *utc)
{
  char                schar;
  short unsigned int  sint;
  short int           ssint;
  int                 k, j, sfr, word, isv;
  char                uchar;
  unsigned long       ulong;

//  clear subframe array
  for ( sfr=1; sfr<=5; sfr++)
  {
    for ( word=1; word<=10; word++)
    {
      sf2[sfr][word] = 0;
    }
  }

//
//  copy ephemeris & almanac data to subframe array sf2[][]
//

//
//  EPHEMERIS ENCODE  subframes 1 to 3
//
//  subframe 1
//
//  iodc = int(((sf2[1][3] & 0x3) <<8 ) | ((sf2[1][8] & 0xFF0000L) >> 16));

//  iweek               = int(sf2[1][3] >> 14);
//  gps_eph[prn].week   = iweek;
  ulong                 = gps_eph[prn].week & 0x3FF;  // 10 bits
  sf2[1][3]             = sf2[1][3] | (ulong << 14); 

//  iura                = int(( sf2[1][3] & 0xF00 ) >> 8);
//  gps_eph[prn].ura    = iura;
  ulong                 = gps_eph[prn].ura & 0xF;  // 4 bits
  sf2[1][3]             = sf2[1][3] | (ulong << 8); 

//  ihealth             = int(( sf2[1][3] & 0xFC ) >> 2);
//  gps_eph[prn].health = ihealth;
  ulong                 = gps_eph[prn].health & 0x3F;  // 6 bits
  sf2[1][3]             = sf2[1][3] | (ulong << 2); 

//  iodc = int(((sf2[1][3] & 0x3) << 8 ) | ((sf2[1][8] & 0xFF0000L) >> 16));
  ulong                 = gps_eph[prn].iodc & 0x300;  // 10 bits, 2 MSB
  sf2[1][3]             = sf2[1][3] | (ulong >> 8);
  ulong                 = gps_eph[prn].iodc & 0xFF;  // 10 bits, 8 LSB
  sf2[1][8]             = sf2[1][8] | (ulong << 16);

//  gps_eph[prn].iodc   = iodc;

//  itgd                = int( sf2[1][7] & 0xFF);
//  gps_eph[prn].tgd    = itgd*4.656612873e-10;
  schar                 = char( gps_eph[prn].tgd/4.656612873e-10);  // 8 bits, scale 2^-31
  ulong                 = long( schar) & 0xFF;
  sf2[1][7]             = sf2[1][7] | (ulong);

//  itoc                = int( sf2[1][8] & 0xFFFF);
//  gps_eph[prn].toc    = itoc*16.0;
  ulong                 = long( gps_eph[prn].toc/16.0) & 0xFFFF;  // 16 bits
  sf2[1][8]             = sf2[1][8] | (ulong);

//  iaf2                = int( sf2[1][9] >> 16);
//  gps_eph[prn].af2    = iaf2*2.775557562e-17;
  schar                 = char( gps_eph[prn].af2/2.775557562e-17);  // 8 bits, scale 2^-55
  ulong                 = long( schar) & 0xFF;
  sf2[1][9]             = sf2[1][9] | (ulong << 16);

//  iaf1                = int( sf2[1][9] & 0xFFFF);
//  gps_eph[prn].af1    = iaf1*1.136868377e-13;
  ssint                 = int( gps_eph[prn].af1/1.136868377e-13);  // 16 bits, scale 2^-43
  ulong                 = long( ssint) & 0xFFFF;
  sf2[1][9]             = sf2[1][9] | (ulong);

//  iaf0                = sf2[1][10] >> 2;
//  if ( bit_test_l( iaf0, 22)) 
//    iaf0 = iaf0 | 0xFFC00000L;
//  gps_eph[prn].af0    = iaf0*4.656612873e-10;
  ulong                 = long(gps_eph[prn].af0/4.656612873e-10) & 0x3FFFFF;  // 22 bits, scale 2^-31
  if ( ulong & (0x1L << (22-1)))  // test bit 22
    ulong = ulong | 0xFFC00000L;
  sf2[1][10]            = sf2[1][10] | (ulong << 2);

//
//   subframe 2
//

//  icrs = int(sf2[2][3] & 0xFFFF);
//  gps_eph[prn].crs    = icrs*.03125;
  ssint                 = int( gps_eph[prn].crs/.03125);  // 16 bits, scale 2^-5
  ulong                 = long( ssint) & 0xFFFF;
  sf2[2][3]             = sf2[2][3] | (ulong);

//  idn                 = int(sf2[2][4] >> 8);
//  gps_eph[prn].dn     = idn*1.136868377e-13*pi;
  ssint                 = int( gps_eph[prn].dn/(1.136868377e-13*pi));  // (Delta n) 16 bits, scale 2^-43, rad
  ulong                 = long( ssint) & 0xFFFF;
  sf2[2][4]             = sf2[2][4] | (ulong << 8);

//  im0                 = ((sf2[2][4] & 0xFF) << 24) | sf2[2][5];
//  gps_eph[prn].ma     = im0*4.656612873e-10*pi;
  ulong                 = long( gps_eph[prn].ma/(4.656612873e-10*pi));  // 32 bits, scale 2^-31, rad
  sf2[2][4]             = sf2[2][4] | ((ulong & 0xFF000000) >> 24);
  sf2[2][5]             = sf2[2][5] | (ulong & 0xFFFFFF);

//  icuc                = int(sf2[2][6] >> 8);
//  gps_eph[prn].cuc    = icuc*1.862645149e-9;
  ssint                 = int( gps_eph[prn].cuc/(1.862645149e-9));  // 16 bits, scale 2^-29
  ulong                 = long( ssint) & 0xFFFF;
  sf2[2][6]             = sf2[2][6] | (ulong << 8);

//  ie                  = ((sf2[2][6] & 0xFF) << 24) | sf2[2][7];
//  gps_eph[prn].ety    = ie*1.164153218e-10;
  ulong                 = long( gps_eph[prn].ety/(1.164153218e-10));  // 32 bits, scale 2^-33
  sf2[2][6]             = sf2[2][6] | ((ulong & 0xFF000000) >> 24);
  sf2[2][7]             = sf2[2][7] | (ulong & 0xFFFFFF);

//  icus                = int(sf2[2][8] >> 8);
//  gps_eph[prn].cus    = icus*1.862645149e-9;
  ssint                 = int( gps_eph[prn].cus/1.862645149e-9);  // 16 bits, scale 2^-29
  ulong                 = long( ssint) & 0xFFFF;
  sf2[2][8]             = sf2[2][8] | (ulong << 8);

//  isqra               = (((sf2[2][8] & 0xFF) << 24) | sf2[2][9]);
//  gps_eph[prn].sqra   = isqra*1.907348633e-6;
//  if (gps_eph[prn].sqra>0.0) 
//    gps_eph[prn].wm   = 19964981.84/pow(gps_eph[prn].sqra,3);
  ulong                 = long( gps_eph[prn].sqra/(1.907348633e-6));  // 32 bits, scale 2^-19
  sf2[2][8]             = sf2[2][8] | ((ulong & 0xFF000000) >> 24);
  sf2[2][9]             = sf2[2][9] | (ulong & 0xFFFFFF);

//  itoe                = int(sf2[2][10] >> 8);
//  gps_eph[prn].toe    = itoe*16.;
  ssint                 = int( gps_eph[prn].toe/16.);  // 16 bits, scale 2^4
  ulong                 = long( ssint) & 0xFFFF;
  sf2[2][10]            = sf2[2][10] | (ulong << 8);


//
// subframe 3
//
//  icic                = int(sf2[3][3] >> 8);
//  gps_eph[prn].cic    = icic*1.862645149e-9;
  ssint                 = int( gps_eph[prn].cic/1.862645149e-9);  // 16 bits, scale 2^-29
  ulong                 = long( ssint) & 0xFFFF;
  sf2[3][3]             = sf2[3][3] | (ulong << 8);

//  iomega0             = ((sf2[3][3] & 0xFF) << 24) | sf2[3][4];
//  gps_eph[prn].w0 = iomega0*4.656612873e-10*pi;
  ulong                 = long( gps_eph[prn].w0/(4.656612873e-10*pi));  // 32 bits, scale 2^-31, rad
  sf2[3][3]             = sf2[3][3] | ((ulong & 0xFF000000) >> 24);
  sf2[3][4]             = sf2[3][4] | (ulong & 0xFFFFFF);

//  icis                = int(sf2[3][5] >> 8);
//  gps_eph[prn].cis    = icis*1.862645149e-9;
  ssint                 = int( gps_eph[prn].cis/1.862645149e-9);  // 16 bits, scale 2^-29
  ulong                 = long( ssint) & 0xFFFF;
  sf2[3][5]             = sf2[3][5] | (ulong << 8);

//  inc0                = ((sf2[3][5] & 0xFF) << 24) | sf2[3][6];
//  gps_eph[prn].inc0   = inc0*4.656612873e-10*pi;
  ulong                 = long( gps_eph[prn].inc0/(4.656612873e-10*pi));  // 32 bits, scale 2^-31, rad
  sf2[3][5]             = sf2[3][5] | ((ulong & 0xFF000000) >> 24);
  sf2[3][6]             = sf2[3][6] | (ulong & 0xFFFFFF);

//  icrc                = int(sf2[3][7] >> 8);
//  gps_eph[prn].crc    = icrc*.03125;
  ssint                 = int( gps_eph[prn].crc/.03125);  // 16 bits, scale 2^-5
  ulong                 = long( ssint) & 0xFFFF;
  sf2[3][7]             = sf2[3][7] | (ulong << 8);

//  iw = ((sf2[3][7] & 0xFF) << 24) | sf2[3][8];
//  gps_eph[prn].w = iw*4.656612873e-10*pi;
  ulong                 = long( gps_eph[prn].w/(4.656612873e-10*pi));  // 32 bits, scale 2^-31, rad
  sf2[3][7]             = sf2[3][7] | ((ulong & 0xFF000000) >> 24);
  sf2[3][8]             = sf2[3][8] | (ulong & 0xFFFFFF);

//  iomegadot = sf2[3][9];
//  if (bit_test_l(iomegadot,24)) 
//    iomegadot           = iomegadot | 0xFF000000L;
//  gps_eph[prn].omegadot = iomegadot*1.136868377e-13*pi;
  ulong                 = long(gps_eph[prn].omegadot/(1.136868377e-13*pi)) & 0xFFFFFF;  // 24 bits, scale 2^-43, rad
  if ( ulong & (0x1L << (24-1)))  // test bit 24
    ulong = ulong | 0xFF000000L;
  sf2[3][9]             = sf2[3][9] | (ulong << 2);

//  idot=int((sf2[3][10] & 0xFFFC) >> 2);
//  if (bit_test_l(idot,14)) 
//    idot=idot | 0xC000;
//  gps_eph[prn].idot   = idot*1.136868377e-13*pi;
  ssint                 = int( gps_eph[prn].idot/(1.136868377e-13*pi));  // 14 bits, scale 2^-43, rad
  ulong                 = long( ssint) & 0x3FFF;
  sf2[3][10]            = sf2[3][10] | (ulong << 2);

//
//    ALMANAC ENCODE  subframes 4 and 5
//
//    SUBFRAME 4
//

//      i4p = int((sf2[4][3] & 0x3F0000L) >> 16);
//      if ( i4p != i4satid)
//      {