/* ************************************************************************    *                                                                      *   *                          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 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////  add 6 parity bits to eachword in subframes////  based on gpsrcvr.cpp/parity_check()//  Clifford Kelley cwkelley@earthlink.net//  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);