ogsnavdecode.c

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

/* ************************************************************************ 
   *                                                                      *
   *                          GPS Simulation                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogsnavdecode.cpp                                        *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   02.03.02                                                *
   *                                                                      *
   *    Author:   C. Kelley, G. Beyerle                                   *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * Copyright (C) 2001,2002  C. Kelley, G. 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 decoding                        *
   *                                                                      *
   ************************************************************************ */

/* ******************************* 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 ( message[j%1500] == !invert)
//          sf[sfr][word] += scale;

        if ( sf[sfr][word] & scale)
          msg[j%1500] = 1;
        else  
          msg[j%1500] = -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

/*******************************************************************************
FUNCTION parity_check()
RETURNS  None.

PARAMETERS
  subrame array

PURPOSE
  check parity and remove parity bits from frame buffer sf[][]

WRITTEN BY
  Clifford Kelley

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

static void parity_check( unsigned long sf[6][11])
{
  long pb1 = 0x3b1f3480L,    // just bits 6-29 (GPS bits 1-24)
       pb2 = 0x1d8f9a40L, 
       pb3 = 0x2ec7cd00L,
       pb4 = 0x1763e680L, 
       pb5 = 0x2bb1f340L, 
       pb6 = 0x0b7a89c0L;
  int  parity, m_parity;
  int  d29 = 0, d30 = 0, sfm, word, 
       b_1, b_2, b_3, b_4, b_5, b_6;
  int  err_bit;

  for ( sfm = 1; sfm <= 5; sfm++)
  {
    p_error[sfm] = 0;
    for ( word=1; word<=10; word++)
    {

//  transmitted parity
      m_parity = (int)( sf[sfm][word] & 0x3f);

//  calculate parity
      b_1 = parity_exor_7to30( d29,     sf[sfm][word] & pb1) << 5;
      b_2 = parity_exor_7to30( d30,     sf[sfm][word] & pb2) << 4;
      b_3 = parity_exor_7to30( d29,     sf[sfm][word] & pb3) << 3;
      b_4 = parity_exor_7to30( d30,     sf[sfm][word] & pb4) << 2;
      b_5 = parity_exor_7to30( 0,       sf[sfm][word] & pb5) << 1;
      b_6 = parity_exor_7to30( d29^d30, sf[sfm][word] & pb6);

      parity  = b_1+b_2+b_3+b_4+b_5+b_6;

//      printf("sf[%d][%d] = %x", sfm, word, sf[sfm][word]);
//      printf(", m_parity = %x", m_parity);
//      printf(", parity   = %x\n", parity);
//      getchar();

      if ( parity != m_parity)
        err_bit = 1;
      else  
      {
        err_bit = 0;
//        printf( "parity check passed.\n");
      }  

      p_error[sfm] = (p_error[sfm] << 1) + err_bit;

      if ( d30 == 1) 
        sf[sfm][word] = 0x3fffffc0L & ~sf[sfm][word];
//        sf_noparity[sfm][word] = 0x3fffffc0L & ~sf[sfm][word];

      sf[sfm][word] = sf[sfm][word] >> 6;   // remove 6 parity bits
//      sf_noparity[sfm][word] = sf[sfm][word] >> 6;   // remove 6 parity bits

      d29 = (m_parity & 0x2) >> 1;
      d30 = m_parity & 0x1;
    }
  }
  return;
}

/*******************************************************************************
FUNCTION decode_navmsg()
RETURNS  None.

PARAMETERS

PURPOSE
  This function assembles and decodes the 1500 bit nav message
  into almanac and ephemeris messages

WRITTEN BY
  Clifford Kelley
  Modifications by G. Beyerle

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

void decode_navmsg( unsigned long sf[6][11], NAVDATA *nav)
{
  int i, j, k;
  static int i4page, i5page;   // GB: int inserted
  int   i4data, i5data, isv;
  int   i4p, i5p;
  unsigned long tmpulng;
  long  tmplng;
  char  tmpchr;
  int  tmpint;
  unsigned int tmpuint;
  float d_toe;

//
//    assemble the bits into subframes and words
//

//  d_toe = clock_tow - gps_eph[prn].toe;

//  if (d_toe > 302400.0) 
//    d_toe = d_toe - 604800.0;
//  else if ( d_toe < -302400.0)
//    d_toe = d_toe + 604800.0;

//  if ( gps_eph[prn].valid == 0 || 
//       (almanac_valid == 0 && almanac_flag == 0) || 
//       fabs( d_toe) > 7200.0)

//  pattern = (int)( (sf[1][1] >> 22) & 0xff);
//  printf( "pattern = %x", pattern); 

//    for (i=0; i<300;i++)
//      printf( "%d", Hist[i]); 

//  getchar();

//
//   EPHEMERIS DECODE subframes 1, 2, 3
//
//  subframe 1
//

//  k = 1;
//  while (!((p_error[1]==0 || p_error[1]==0x200) && 
//       p_error[2]==0 && p_error[3]==0))
//  {
//    
//    copy_msg_to_sf( message, k);
//  
//    parity_check(); // check the parity of the 1500 bit message
//
//    printf("(%d) perr[1] = 0x%x perr[2] = 0x%x perr[3] = 0x%x\n", 
//      k, p_error[1], p_error[2], p_error[3]);
//
//    k += 1;
//  }

// check the parity of the 1500 bit message and strip 6 parity bits
  parity_check( sf); 

//  printf("perr[1] = 0x%x, perr[2] = 0x%x, perr[3] = 0x%x, perr[4] = 0x%x, perr[5] = 0x%x\n", 
//    p_error[1], p_error[2], p_error[3], p_error[4], p_error[5]);
//  printf("*** copy sf_noparity[][] to sf[][] ***\n");
//  getchar();

  if (( p_error[1] == 0 || p_error[1] == 0x200) && 
        p_error[2] == 0 && p_error[3] == 0)
  {
#if 0
    printf( "parity check passed!\n");
    printf( "p_error[1] = %x\n", p_error[1]);
    printf( "p_error[2] = %x\n", p_error[2]);
    printf( "p_error[3] = %x\n", p_error[3]);
    getchar();
#endif

// was:
//    nav->sf1how       = (long)( (sf[1][2] & 0xffff80) >> 7);

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

    nav->sf4svid      = (int)( (sf[4][3] & 0x3f0000) >> 16);
    nav->sf5svid      = (int)( (sf[5][3] & 0x3f0000) >> 16);

    if ( nav->sf4svid >= 0 && nav->sf4svid < 64)
      nav->sf4pageno  = satid2page[nav->sf4svid];
    else  
      nav->sf4pageno  = -1;

    if ( nav->sf5svid >= 0 && nav->sf5svid < 64)
      nav->sf5pageno  = satid2page[nav->sf5svid];
    else  
      nav->sf5pageno  = -1;

#if 0
    printf("\n");
    printf("nav->sf4pageno = %d\n", nav->sf4pageno);
    printf("nav->sf4svid   = %d\n", nav->sf4svid);
    printf("nav->sf5pageno = %d\n", nav->sf5pageno);
    printf("nav->sf5svid   = %d\n", nav->sf5svid);
//    printf("sf[4][2] = %08x\n", sf[4][2]);
//    printf("sf[5][2] = %08x\n", sf[5][2]);
    getchar();
#endif

    nav->eph.valid = 0;

//    iodct=iodc & 0xff;
//    iode=sf[2][3]  >> 16;
//    idoe=sf[3][10] >> 16;
//    if ( iodct != iode || iodct!= idoe || iode != idoe || gps_eph[prn].sqra==0.0)
//    
    nav->eph.how       = (long)( sf[1][2] >> 2);
    nav->eph.sfid      = (int)( (sf[1][2] & 0x1c) >> 2);
    tmplng             = (long)( (sf[1][2] & 0xffff80) >> 7);
    nav->eph.tow       = tmplng * 6 - 6;   // z-count/4 -> week seconds
    nav->eph.asflag    = (int)( (sf[1][2] & 0x20) >> 5);
    nav->eph.alertflag = (int)( (sf[1][2] & 0x40) >> 6);

    nav->eph.week   = (int)( sf[1][3] >> 14);
//      icapl2              = ( sf[1][3] & 0x3000 ) >> 12;
    nav->eph.ura    = (int)(( sf[1][3] & 0xF00 ) >> 8);
    nav->eph.health = (int)(( sf[1][3] & 0xFC ) >> 2);
    tmpint          = (int)(((sf[1][3] & 0x3) << 8 ) | ((sf[1][8] & 0xFF0000L) >> 16));
    nav->eph.iodc   = tmpint;
    tmpint          = (int)( sf[1][7] & 0xFF);
    nav->eph.tgd    = tmpint * pow( 2.0, -31.0);
    tmpuint         = (int)( sf[1][8] & 0xFFFF);
    nav->eph.toc    = tmpuint * 16.0;
    tmpint          = (int)( sf[1][9] >> 16);
    nav->eph.af2    = tmpint * pow( 2.0, -55.0);
    tmpint          = (int)( sf[1][9] & 0xFFFF);
    nav->eph.af1    = tmpint * pow( 2.0, -43.0);
    tmplng          = sf[1][10] >> 2;
    if ( bit_test_long( tmplng, 22)) 
      tmplng |= 0xFFC00000L;