ogslibrary.c

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

//          gps_alm[isv].lan=iaomega0*1.192092896E-7*pi;
//          iaw=sf2[5][8];
//          if (bit_test_l(iaw,24)) iaw=iaw | 0xFF000000L;
//          gps_alm[isv].aop=iaw*1.192092896E-7*pi;
//          iam0=sf2[5][9];
//          if (bit_test_l(iam0,24)) iam0=iam0 | 0xFF000000L;
//          gps_alm[isv].ma=iam0*1.192092896E-7*pi;
//          iaaf0=int((sf2[5][10] >> 13) | (sf2[5][10] & 0x1C));
//          if (bit_test_l(iaaf0,11)) iaaf0=iaaf0 | 0xF800;
//          gps_alm[isv].af0=iaaf0*9.536743164E-7;
//          iaaf1=int((sf2[5][10] & 0xFFE0) >> 5);
//          if (bit_test_l(iaaf1,11)) 
//            iaaf1=iaaf1 | 0xF800;
//          gps_alm[isv].af1=iaaf1*3.637978807E-12;

//
//  copied from subframe4, pages 1-24
//
    ssint     = int( gps_alm[isv].ety/(4.768371582E-7));  // 16 bits, scale 2^-21
    ulong     = long( ssint) & 0xFFFF;
    sf2[5][3] = sf2[5][3] | (ulong);

    schar     = char( gps_alm[isv].toa/4096.0);  // 8 bits, scale 2^12
    ulong     = long( schar) & 0xFF;
    sf2[5][4] = sf2[5][4] | (ulong << 16);

    ssint     = int( (gps_alm[isv].inc/pi-0.3)/1.907348633E-6);  // 16 bits, scale 2^-19
    ulong     = long( ssint) & 0xFFFF;
    sf2[5][4] = sf2[5][4] | (ulong);

    ssint     = int( gps_alm[isv].rra/(3.637978807E-12*pi));  // 16 bits, scale 2^-38
    ulong     = long( ssint) & 0xFFFF;
    sf2[5][5] = sf2[5][5] | (ulong << 8);

    ulong     = long( gps_alm[isv].health) & 0xFF;
    sf2[5][5] = sf2[5][5] | (ulong);

    ssint     = int( gps_alm[isv].sqa/(4.8828125E-4));  // 16 bits, scale 2^-11
    ulong     = long( ssint) & 0xFFFF;
    sf2[5][6] = sf2[5][6] | (ulong);

    ulong     = long( gps_alm[isv].lan/(1.192092896E-7*pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad
    if ( ulong & (0x1L << (24-1)))  // test bit 24
      ulong = ulong | 0xFF000000L;
    sf2[5][7] = sf2[5][7] | (ulong);

    ulong     = long( gps_alm[isv].aop/(1.192092896E-7*pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad
    if ( ulong & (0x1L << (24-1)))  // test bit 24
      ulong = ulong | 0xFF000000L;
    sf2[5][8] = sf2[5][8] | (ulong);

    ulong     = long( gps_alm[isv].ma/(1.192092896E-7*pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad
    if ( ulong & (0x1L << (24-1)))  // test bit 24
      ulong = ulong | 0xFF000000L;
    sf2[5][9] = sf2[5][9] | (ulong);

    ulong     = long( gps_alm[isv].af0/(9.536743164E-7)) & 0x7FF;  // 11 bits, scale 2^-20
    if ( ulong & (0x1L << (11-1)))  // test bit 11
      ulong = ulong | 0xFFFFF800L;
    sf2[5][10] = sf2[5][10] | ((ulong & 0x7) << 2);
    sf2[5][10] = sf2[5][10] | ((ulong & 0x7F8) << 13);

    ulong     = long( gps_alm[isv].af1/(3.637978807E-12)) & 0x7FF;  // 11 bits, scale 2^-38
    if ( ulong & (0x1L << (11-1)))  // test bit 11
      ulong = ulong | 0xFFFFF800L;
    sf2[5][10] = sf2[5][10] | (ulong << 5);
  }

//
//  add to each 24 bit word 6 parity bits
//
  add_parity_bits();

//
//  5 subframe a 300 bits; 10 words a 30 bits
// 

//  copy from sf2[][] to chan[ch].message[1500] buffer
  unsigned long scale;

  j=0;
  for ( sfr=1; sfr<=5; sfr++)
  {
    for ( word=1; word<=10; word++)
    {
//      scale = 536870912L;                // 536870912L = 2^29
      scale = 0x1L << 29;
//      sf2[sfr][word] = 0;
      for ( k=0; k<=29; k++)
      {
//        if ( chan[ch].message[(j+chan[ch].offset)%1500] == 1)
//          sf2[sfr][word] += scale;
        if ( sf2[sfr][word] & scale)
          chan[ch].message[(j+chan[ch].offset)%1500] = 1;
        else 
          chan[ch].message[(j+chan[ch].offset)%1500] = -1;
        scale = scale >> 1;
        j++;
      }
    }
  } // --- for ( sfr=1; sfr<=5; sfr++) ---

  return;
}


//
//  add 6 parity bits to sf2[][]
//
//  based on gpsrcvr.cpp/parity_check()
//  Clifford Kelley [e-mail]
//  Copyright (c) 1996-2001 Clifford Kelley
//
void add_parity_bits( void)
{
  unsigned long x;
  int  parity;
  char d29=1, d30=1, // assume that last 2 bits from last frame are one
       sfm, word, 
       b1, b2, b3, b4, b5, b6;
  long mask1 = 0xEC7CD2L, 
       mask2 = 0x763E69L, 
       mask3 = 0xBB1F34L,
       mask4 = 0x5D8F9AL, 
       mask5 = 0xAEC7CDL, 
       mask6 = 0x2DEA27L;

  for ( sfm = 1; sfm <= 5; sfm++)
  {
    for ( word=1; word<=10; word++)
    {
      x   = (sf2[sfm][word] & mask1) | (d29 << 31);  // bit 31 of sf2[][] is unused
      b1 = exor_long( x) << 5;
      x   = (sf2[sfm][word] & mask2) | (d30 << 31);  // bit 31 of sf2[][] is unused
      b2 = exor_long( x) << 4;
      x   = (sf2[sfm][word] & mask3) | (d29 << 31);  // bit 31 of sf2[][] is unused
      b3 = exor_long( x) << 3;
      x   = (sf2[sfm][word] & mask4) | (d30 << 31);  // bit 31 of sf2[][] is unused
      b4 = exor_long( x) << 2;
      x   = (sf2[sfm][word] & mask5) | (d30 << 31);  // bit 31 of sf2[][] is unused
      b5 = exor_long( x) << 1;
      x   = (sf2[sfm][word] & mask6) | (d29 << 31);  // bit 31 of sf2[][] is unused
      b6 = exor_long( x);

      parity = b1 + b2 + b3 + b4 + b5 + b6;

      if ( d30 == 1) 
        sf2[sfm][word] = ~sf2[sfm][word] & 0x03fffffc0L;

//  add 6 parity bits
      sf2[sfm][word] = (sf2[sfm][word] << 6) | (parity & 0x3F);

// update d29/d30 bits
      d29 = (parity & 0x2) >> 1;
      d30 = parity & 0x1;
    }
  }
}

//
//  xor all 32 bits in a long variable (b31^b30^b29^...^b2^b1^b0)
//
static int exor_long( unsigned long x)
{
  char i;
  int res = 0;

  for ( i=0; i<32; i++)
  {
    res = res ^ (x & 0x1);
    x = x >> 1;
  }
  return res;
}

inline double frem( double x, double y)
{
  double res;

  if ( x >= 0.0)
    res = fmod( x, y);
  else
    res = -fmod( -x, y);    
  return res;
}

#endif



/*******************************************************************************
FUNCTION bit_test_long()
RETURNS  int

PARAMETERS
    Data
    bit_n

PURPOSE
  This function returns a 1 if bit number bit_n of long Data is set
  else it returns a 0
  NOTE: bit_n = 1,...,32

WRITTEN BY
  Clifford Kelley

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

int bit_test_long( unsigned long data, int bit_n)
{
  return ( data & (0x1L << (bit_n-1)));
}

/*******************************************************************************
FUNCTION parity_exor_7to30( int bit, long parity)
RETURNS  
  GPS parity

PARAMETERS 
  parity : long word to be processed

PURPOSE
  xor bits 7 to 30 in nav msg word
  and xor result with parameter 'bit'
  
WRITTEN BY
  Clifford Kelley

*******************************************************************************/
int parity_exor_7to30( int bit, long parity)
{
  int i;
  int res = 0;

  for ( i=7; i<=30; i++)
  {
    if ( bit_test_long( parity, i)) 
      res++;
  }
//  res = res % 2;
  res = (bit^res) & 0x1;

  return (res);
}

//
//  controlled malloc()
//
char *conmalloc( size_t len)
{
  char *ptr;
  
  ptr = (char*) malloc( len);
  if ( !ptr)
  {
    printf( "malloc() failed!\n");
    exit(-1);
  }

  return (ptr);
}

#define DIRBUFLEN 512

//
//  extract position of 'bin' and 'data' dir and write to global variable
//
void set_directories( char *ptr)
{
  char *ptr2, *ptr3;

#ifdef unix
//  if relative pathname we need to add current working dir
  if ( ptr[0] != '/')
  {
    char workdir[DIRBUFLEN];
    ptr2 = getcwd( workdir, DIRBUFLEN);
    if ( !ptr2 || strlen( workdir)+strlen( ptr) > DIRBUFLEN-1)
    {
      printf( "Increase dir buffer size!\n");
      exit(-1);
    }
    strcat( workdir, "/");
    strcat( workdir, ptr);
    ptr = workdir;
  }
#endif

//  printf(">%s<\n", ptr);
//  getchar();

  OGSBinDir  = (char*) conmalloc( strlen( ptr) + 1);
  OGSDataDir = (char*) conmalloc( strlen( ptr) + 1);

  strcpy( OGSBinDir, ptr);

//  printf("%s\n", OGSBinDir);

#ifdef __TURBOC__
  ptr2 = OGSBinDir;
  while ( *ptr2)
  {
    if (*ptr2 == '\\')
      *ptr2 = '/';
    ptr2++;  
  }
#endif

//  printf( "%s\n", OGSBinDir);

  ptr2 = strrchr( OGSBinDir, '/');
  if ( !ptr2)
  {
    printf( "invalid OGS directory structure! (%s)\n", OGSBinDir);
    exit(-1);
  }
  *ptr2 = '\0';

  strcpy( OGSDataDir, OGSBinDir);

  ptr2 = strrchr( OGSDataDir, '/');
  if ( !ptr2)
  {
    printf( "invalid OGS directory structure! (%s)\n", OGSBinDir);
    exit(-1);
  }
  *ptr2 = '\0';

  strcat( OGSBinDir, "/");
  strcat( OGSDataDir, "/data/");

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

  return;
}



#ifndef __TURBOC__

void clrscr( void)
{
  printf( "\x1B[2J\x1B[0d\x1B[0G");
  return;
}

void gotoxy( int x, int y)
{
  printf( "\x1B[%dd \x1B[%dG", y, x);
  return;
}

//  don't know how to do these...
#ifdef linux

//
// from kbhit.c, WROX book "Beginning LINUX Programming" (www.wrox.com)
//
#include <termios.h>
#include <term.h>
#include <curses.h>
#include <unistd.h>

static struct termios   initial_settings, 
                        new_settings;
static int              peek_character = -1;

void init_keyboard( void)
{
  tcgetattr( 0, &initial_settings);
  new_settings = initial_settings;
  new_settings.c_lflag &= ~ICANON;
  new_settings.c_lflag &= ~ECHO;
  new_settings.c_lflag &= ~ISIG;
  new_settings.c_cc[VMIN] = 1;
  new_settings.c_cc[VTIME] = 0;
  tcsetattr( 0, TCSANOW, &new_settings);
}

void close_keyboard( void)
{
  tcsetattr(0, TCSANOW, &initial_settings);
}

int kbhit( void)
{
  char ch;
  int nread;

  if ( peek_character != -1)
    return 1;
  new_settings.c_cc[VMIN]=0;
  tcsetattr( 0, TCSANOW, &new_settings);
  nread = read(0,&ch,1);
  new_settings.c_cc[VMIN]=1;
  tcsetattr( 0, TCSANOW, &new_settings);

  if ( nread == 1) 
  {
    peek_character = ch;
    return 1;
  }
  return 0;
}

int readch( void)
{
  char ch;

  if ( peek_character != -1) 
  {
    ch = peek_character;
    peek_character = -1;
    return ch;
  }
  read( 0, &ch, 1);
  return ch;
}

#else
// not linux

int kbhit( void)
{
  return 0;
}

char getch( void)
{
  return '0';
}

#endif

#endif

/* ------------------------------ end of file ----------------------------- */