/* ************************************************************************    *                                                                      *   *                          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)//      {