/* ************************************************************************    *                                                                      *   *                          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 0inline 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 arrayPURPOSE  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.PARAMETERSPURPOSE  This function assembles and decodes the 1500 bit nav message  into almanac and ephemeris messagesWRITTEN 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;