ogsnavencode.c

OpenGPSSim是一个gps模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          GPS Simulation                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogsnavencode.cpp                                        *
   *                                                                      *
   *   Version:   0.1                                                     *
   *      Date:   02.03.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                         *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *                      Navigation data encoding                        *
   *                                                                      *
   ************************************************************************ */

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

   dd.mm.yy -

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

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

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

#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 ------------------------------ */


//
//  printf content of sf[][]
//
static void  print_sf( unsigned long sf[6][11])
{
  int i, sfr, word;
  unsigned long scale;

  for ( sfr=1; sfr<=5; sfr++)
  {
    printf( "--- subframe %d ---\n", sfr);
    for ( word=1; word<=10; word++)
    {
      printf( " %08x", sf[sfr][word]);
      if (( word % 5) == 0)
        printf( "\n");
    }  
  }
  getchar();

  return;
}

//
//  copy frame buffer sf[][] to 1500 byte nav msg array
//
static void copy_sf_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;
        else  
          msg[j] = -1;

        scale = scale >> 1;
        j++;
      }
    }
  }

  return;
}

//
//  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;
}

#if 0
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

//
//  add 6 parity bits to eachword in subframes
//
//  based on gpsrcvr.cpp/parity_check()
//  Clifford Kelley [e-mail]
//  Copyright (c) 1996-2001 Clifford Kelley
//
static void add_parity( unsigned long sf[6][11], unsigned long sfp[6][11])
{
  unsigned long x;
  int  parity;
  long d29 = 0, d30 = 0; // assume that last 2 bits from previous frame are zero
  int  sfm, word, 
       b1, b2, b3, b4, b5, b6;
  long mask1 = 0x3b1f3480L,    // just bits 6-29 (GPS bits 1-24)
       mask2 = 0x1d8f9a40L, 
       mask3 = 0x2ec7cd00L,
       mask4 = 0x1763e680L, 
       mask5 = 0x2bb1f340L, 
       mask6 = 0x0b7a89c0L;

  for ( sfm = 1; sfm <= 5; sfm++)
  {
    for ( word=1; word<=10; word++)
    {

      sfp[sfm][word] = sf[sfm][word] << 6;

//  calculate parity
      b1 = parity_exor_7to30( d29, sfp[sfm][word] & mask1) << 5;
      b2 = parity_exor_7to30( d30, sfp[sfm][word] & mask2) << 4;
      b3 = parity_exor_7to30( d29, sfp[sfm][word] & mask3) << 3;
      b4 = parity_exor_7to30( d30, sfp[sfm][word] & mask4) << 2;
      b5 = parity_exor_7to30( d30, sfp[sfm][word] & mask5) << 1;
      b6 = parity_exor_7to30( d29, sfp[sfm][word] & mask6);

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

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

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

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

//
//  encode navigation data to array sfp[][]
//
void encode_navmsg( NAVDATA *nav, unsigned long sfp[6][11])
{
  char                schar;
  short unsigned int  sint;
  short int           ssint;
  int                 k, j, sfr, word;
  int                 i4satid, i5satid;
  char                uchar;
  long                slong;
  unsigned long       ulong,
                      preamble = 0x8b;
  unsigned long sf[6][11];    // 5 subframes (1,...,5), 
                              // comprising 10 words (1,...,10),  
                   
  i4satid = nav->sf4svid;     // subframe 4 SV id (1,...,63)
  i5satid = nav->sf5svid;     // subframe 5 SV id (1,...,63)

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

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

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

  sf[1][1]  |= preamble << 16; 
  sf[2][1]  |= preamble << 16; 
  sf[3][1]  |= preamble << 16; 
  sf[4][1]  |= preamble << 16; 
  sf[5][1]  |= preamble << 16; 

//  sf[1][2]  |= (nav->sf1how & 0x1ffff) << 7; 
//  sf[2][2]  |= (nav->sf2how & 0x1ffff) << 7; 
//  sf[3][2]  |= (nav->sf3how & 0x1ffff) << 7; 
//  sf[4][2]  |= (nav->sf4how & 0x1ffff) << 7; 
//  sf[5][2]  |= (nav->sf5how & 0x1ffff) << 7; 

  sf[1][2]  |= (nav->sf1how & 0x3fffff) << 2; 
  sf[2][2]  |= (nav->sf2how & 0x3fffff) << 2; 
  sf[3][2]  |= (nav->sf3how & 0x3fffff) << 2; 
  sf[4][2]  |= (nav->sf4how & 0x3fffff) << 2; 
  sf[5][2]  |= (nav->sf5how & 0x3fffff) << 2; 

  ulong     = nav->eph.week & 0x3FF;  // 10 bits
  sf[1][3]  |= (ulong << 14); 

  ulong     = nav->eph.ura & 0xF;  // 4 bits
  sf[1][3]  |= (ulong << 8); 

  ulong     = nav->eph.health & 0x3F;  // 6 bits
  sf[1][3]  |= (ulong << 2); 

  ulong     = nav->eph.iodc & 0x300;  // 10 bits, 2 MSB
  sf[1][3]  |= (ulong >> 8);
  ulong     = nav->eph.iodc & 0xFF;  // 10 bits, 8 LSB
  sf[1][8]  |= (ulong << 16);

//  nav->eph.iodc   = iodc;

  sf[1][7]  |= (long)( nav->eph.tgd / pow( 2.0, -31.0)) & 0xFF; // 8 bits, scale 2^-31

  sf[1][8]  |= (long)( nav->eph.toc / 16.0) & 0xFFFF;  // 16 bits

  ulong     = (long)( nav->eph.af2 / pow( 2.0, -55.0)) & 0xFF; // 8 bits, scale 2^-55
  sf[1][9]  |= (ulong << 16);

  sf[1][9]  |= (long)( nav->eph.af1 / pow( 2.0, -43.0)) & 0xFFFF; // 16 bits, scale 2^-43

  ulong     = (long)( nav->eph.af0 / pow( 2.0, -31.0)) & 0x3FFFFF;  // 22 bits, scale 2^-31
  sf[1][10] |= (ulong << 2);

//
//   subframe 2
//
  sf[2][3]  |= (long)( nav->eph.crs / pow( 2.0, -5.0)) & 0xFFFF;  // 16 bits, scale 2^-5

  ulong     = (long)( nav->eph.dn / (pow( 2.0, -43.0)*pi)) & 0xFFFF; // (Delta n) 16 bits, scale 2^-43, rad
  sf[2][4]  |= (ulong << 8);

  ulong     = (long)( nav->eph.ma / (pow( 2.0, -31.0)*pi));  // 32 bits, scale 2^-31, rad
  sf[2][4]  |= ((ulong & 0xFF000000) >> 24);
  sf[2][5]  |= (ulong & 0xFFFFFF);

  ulong     = (long)( nav->eph.cuc / pow( 2.0, -29.0)) & 0xFFFF; // 16 bits, scale 2^-29
  sf[2][6]  |= (ulong << 8);

  ulong     = (long)( nav->eph.ety / pow( 2.0, -33.0));  // 32 bits, scale 2^-33
  sf[2][6]  |= ((ulong & 0xFF000000) >> 24);
  sf[2][7]  |= (ulong & 0xFFFFFF);

  ulong     = (long)( nav->eph.cus / pow( 2.0, -29.0)) & 0xFFFF; // 16 bits, scale 2^-29
  sf[2][8]  |= (ulong << 8);