ogr_navdecode.c

The OpenGPSRec receiver software runs on the real time operating system RTAI-Linux. It compiles with

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogr_navdecode.c                                         *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   09.12.02                                                *
   *                                                                      *
   *    Author:   C. Kelley                                               *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * Copyright (C) 2001-2003  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 'ogr_user.c', 'ogr_navdecode.c', 'ogr_navsolve.c',         *
   * 'ogr_rtproc.c' are modified versions of the files 'gpsfuncs.cpp',    *
   * 'gpsrcvr.cpp' from Clifford Kelley's OpenSourceGPS distribution.     *
   * The unmodified files can be obtained from                            *
   *             http://www.home.earthlink.net/~cwkelley                  *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *                      Decode navigation message                       *
   *                                                                      *
   ************************************************************************ */

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

   dd.mm.yy -

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

/***********************************************************************
  GPS RECEIVER (GPSRCVR) Ver. 1.02
  12 Channel All-in-View GPS Receiver Program based on Mitel GP2021
  chipset
  Clifford Kelley [e-mail]
  Copyright (c) 1996-2001 Clifford Kelley.  All Rights Reserved.
  This LICENSE must be included with the GPSRCVR code.
***********************************************************************/

/*
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

      CONDITIONS

1. Redistributions of GPSRCVR source code must retain the above copyright
notice, this list of conditions, and the following disclaimer.

2. Redistributions in binary form must contain the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.

3. All modifications to the source code must be clearly marked as
such.  Binary redistributions based on modified source code must be
clearly marked as modified versions in the documentation and/or other
materials provided with the distribution.

4. Notice must be given of the location of the availability of the
unmodified current source code, e.g.,
  http://www.Kelley.com/
or
  ftp://ftp.Kelley.com
in the documentation and/or other materials provided with the
distribution.

5. All advertising and published materials mentioning features or use
of this software must display the following acknowledgment:  "This
product includes software developed by Clifford Kelley and other
contributors."

6. The name of Clifford Kelley may not be used to endorse or promote
products derived from this software without specific prior written
permission.

      DISCLAIMER

This software is provided by Clifford Kelley and contributors "as is" and
any expressed or implied warranties, including, but not limited to, the
implied warranties of merchantability and fitness for a particular
purpose are disclaimed.  In no event shall Clifford Kelley or
contributors be liable for any direct, indirect, incidental, special,
exemplary, or consequential damages (including, but not limited to,
procurement of substitute goods or services; loss of use, data, or
profits; or business interruption) however caused and on any theory of
liability, whether in contract, strict liability, or tort (including
negligence or otherwise) arising in any way out of the use of this
software, even if advised of the possibility of such damage.
*/

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

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

#include  "port.h"
#include  "ogr_defines.h"
#include  "ogr_structs.h"
#include  "ogr_prototypes.h"
#include  "ogr_globals.h"

/* ----------------------------- defines --------------------------------- */

#define CONST_2p12 4096.0
#define CONST_2p4  16.0
#define CONST_2p8  256.0
#define CONST_2p10 1024.0
#define CONST_2p18 262144.0
#define CONST_2p24 16777216.0

#define CONST_2m5  0.03125
#define CONST_2m11 4.8828125e-4
#define CONST_2m19 1.90734863281250e-06
#define CONST_2m20 9.53674316406250e-07
#define CONST_2m21 4.76837158203125e-07
#define CONST_2m23 1.19209289550781e-07
#define CONST_2m24 5.96046447753906e-08
#define CONST_2m27 7.45058059692383e-09
#define CONST_2m29 1.86264514923096e-09
#define CONST_2m30 9.31322574615479e-10
#define CONST_2m31 4.65661287307739e-10
#define CONST_2m33 1.16415321826935e-10
#define CONST_2m38 3.63797880709171e-12
#define CONST_2m43 1.13686837721616e-13
#define CONST_2m50 8.88178419700125e-16
#define CONST_2m55 2.77555756156289e-17

/* ---------------------------- globals ----------------------------------- */

/* ---------------------------- prototypen -------------------------------- */

static INT16 bit_test_l( unsigned long data, INT16 bit_n);

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

PARAMETERS
  bit    :
  parity :

PURPOSE
  Add GPS bits 7 to 30, exor with 'bit'

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
static INT16 gps_exor( char bit, long parity)
{
  char i;
  INT16 res = 0;

  for ( i=7; i<=30; i++)
  {
    if ( bit_test_l( parity, i))
      res++;
  }

  res = res % 2;
  res = (bit ^ res) & 0x1;
  return (res);
}

/*******************************************************************************
FUNCTION bit_test_l()
RETURNS  INT16

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 (1<=bit<=32).

WRITTEN BY
  Clifford Kelley

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

static INT16 bit_test_l( unsigned long data, INT16 bit_n)
{
#if 0
  INT16 res = 0;

  if ( data & test_l[bit_n])
    res = 1;

  return (res);
#endif
  return ( (data >> (bit_n-1)) & 0x1);
}

#if 0
/*
 *  test parity bits and strip off subframe words
 *  input  : sfp[][]
 *  output : sf[][], p_error[]
 */
static void parity_test_and_remove( unsigned long sf[][11], 
  UINT16 p_error[], unsigned long sfp[][11])
{
  long pb1 = 0x3b1f3480L,
       pb2 = 0x1d8f9a40L,
       pb3 = 0x2ec7cd00L,
       pb4 = 0x1763e680L,
       pb5 = 0x2bb1f340L,
       pb6 = 0x0b7a89c0L;
  INT16  i, j, parity, msg_parity, err_bit;
  char d29 = 0, d30 = 0, 
       b_1, b_2, b_3, b_4, b_5, b_6;

  for ( j=1; j<=5; j++)
  {
    p_error[j] = 0;
    for ( i=1; i<=10; i++)
    {
//  gpsbit30 at bit30, gpsbit29 at bit31 ! 
      d30 = (sfp[j][i] >> 30) & 0x1;
      d29 = (sfp[j][i] >> 31) & 0x1;

      msg_parity = (INT16)(sfp[j][i] & 0x3f);

      b_1 = gps_exor( d29, sfp[j][i] & pb1) << 5;
      b_2 = gps_exor( d30, sfp[j][i] & pb2) << 4;
      b_3 = gps_exor( d29, sfp[j][i] & pb3) << 3;
      b_4 = gps_exor( d30, sfp[j][i] & pb4) << 2;
      b_5 = gps_exor(   0, sfp[j][i] & pb5) << 1;
      b_6 = gps_exor( d29^d30, sfp[j][i] & pb6);

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

      err_bit = (parity == msg_parity) ? 0 : 1;

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

//  copy word ...
      sf[j][i] = sfp[j][i];

//  ... invert ...
      if ( d30 == 1)
        sf[j][i] = ~sf[j][i];

//  ... and strip 6 parity bits
      sf[j][i] = (sf[j][i] >> 6) & 0xffffffL;
    }
  }
  return;
}
#endif

/*
 *  test parity bits and strip off subframe words
 *  input  : sfp[][]
 *  output : sf[][], p_error[]
 */
int check_parity_subframe( INT16 ch)
{
  char   d29 = 0, 
         d30 = 0;
  INT16  i,
         parity = 0,  
         msg_parity, 
         err_bit = 0;
//  long pb1 = 0x3b1f3480L,
//       pb2 = 0x1d8f9a40L,
//       pb3 = 0x2ec7cd00L,
//       pb4 = 0x1763e680L,
//       pb5 = 0x2bb1f340L,
//       pb6 = 0x0b7a89c0L;
  long pb1 = 0xbb1f3480L,
       pb2 = 0x5d8f9a40L,
       pb3 = 0xaec7cd00L,
       pb4 = 0x5763e680L,
       pb5 = 0x2bb1f340L,
       pb6 = 0xcb7a89c0L;
  unsigned long *sfp, *sf, word;

  sfp = Msg[ch].subframe_parity;
  sf  = Msg[ch].subframe;

  for ( i=1; i<=10; i++)
  {
    word = sfp[i];

//  gpsbit30 at bit30, gpsbit29 at bit31 ! 
    d30 = (word >> 30) & 0x1;
    d29 = (word >> 31) & 0x1;

    if ( d30)
      word ^= 0x3fffffc0;

    msg_parity = (INT16)(word & 0x3f);

    parity =  gps_exor( d29, word & pb1) << 5;
    parity += gps_exor( d30, word & pb2) << 4;
    parity += gps_exor( d29, word & pb3) << 3;
    parity += gps_exor( d30, word & pb4) << 2;
//    parity += gps_exor(   0, word & pb5) << 1;
//    parity += gps_exor( d29^d30, word & pb6);
    parity += gps_exor( d30, word & pb5) << 1;
    parity += gps_exor( d29, word & pb6);

    err_bit |= (parity == msg_parity) ? 0 : (0x1 << (9-i));

#if 0
    if ( err_bit)
    {
      int j;

      gotoxy( 1, 24);
      printf( "(%d)", ch);
      printf( " par = %x", err_bit);
      printf( " word(%d) = %08lx / %08lx", i, word, sfp[i]);
      printf( "\n");
      for (j=1;j<=10;j++)
        printf( " %08lx", sfp[j]);
      printf( "\n");
    }
#endif

//  strip 6 parity bits and write to buffer sf[]
    sf[i] = (word >> 6) & 0xffffffL;
  }  // --- for ( i=1; i<=10; i++) ---

  return (err_bit);
}

/*
 *  decode almanac data found in subframe 4 & 5
 */
void almanac_decode( INT16 i4page, unsigned long sf[])
{
  INT16 isv, iae, iatoa, tmpshrt;
  unsigned long tmpulng;
  long tmplng;

  isv = i4page;

  if ( isv < 1 || isv > 32)
    return;

  GPS_Alm[isv].week   = Ctrl.gps_week;
  iae = (INT16)(sf[3] & 0xffffL);
  GPS_Alm[isv].ety    = iae * CONST_2m21;
  iatoa = (INT16)(sf[4] >> 16);
  GPS_Alm[isv].toa    = iatoa * 4096.0;
  tmplng = sf[4] & 0xffffL;
  if ( bit_test_l( tmplng, 16)) 
    tmplng |= 0xffff0000L;
  GPS_Alm[isv].inc    = (tmplng * CONST_2m19 + 0.3) * PI;
  tmpshrt = (INT16)(sf[5] >> 8);
  GPS_Alm[isv].rra    = tmpshrt * CONST_2m38 * PI;
  GPS_Alm[isv].health = (INT16)(sf[5] & 0xFF);
  tmpulng = sf[6];
  GPS_Alm[isv].sqra    = tmpulng * CONST_2m11;
  if ( GPS_Alm[isv].sqra > 0.0) 
    GPS_Alm[isv].n0 = SQRTGM / pow( GPS_Alm[isv].sqra, 3.0);
  tmplng = sf[7];
    if ( bit_test_l( tmplng, 24)) 
  tmplng |= 0xff000000L;
  GPS_Alm[isv].lan    = tmplng * CONST_2m23 * PI;
  tmplng = sf[8];
  if (bit_test_l( tmplng, 24)) 
    tmplng |= 0xff000000L;
  GPS_Alm[isv].aop    = tmplng * CONST_2m23 * PI;
  tmplng = sf[9];
  if ( bit_test_l( tmplng, 24)) 
    tmplng |= 0xff000000L;
  GPS_Alm[isv].ma     = tmplng * CONST_2m23 * PI;
    //      tmplng=(sf[10] >> 13) | (sf[10] & 0x1C);
  tmplng = (sf[10] >> 13) | ((sf[10] & 0x1C) >> 2);  // fixed (GB-020217)  
    if (bit_test_l( tmplng, 11)) 
  tmplng |= 0xfffff800L;
  GPS_Alm[isv].af0    = tmplng * CONST_2m20;
  tmplng = (sf[10] | 0xffe0) >> 5;
  if ( bit_test_l( tmplng, 11)) 
    tmplng |= 0xfffff800L;
  GPS_Alm[isv].af1    = tmplng * CONST_2m38;

  Ctrl.almanac_read |= (0x1L << (isv-1));

//  all almanac data collected?
//  if ( almanac_read == 0xffffffff)
//    almanac_valid = 1;

  Ctrl.almanac_valid = 1;

  for ( isv=1; isv<=32; isv++)
  {
    if ( GPS_Alm[isv].inc > 0.0 &&
         GPS_Alm[isv].week != Ctrl.gps_week % 1024)
      Ctrl.almanac_valid = 0;
  }

  return;
}

/*
 * decode subframe 1
 */