ogrt_maths.c

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

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogrt_maths.c                                            *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   09.12.02                                                *
   *                                                                      *
   *    Author:   C. Kelley, G. Beyerle                                   *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * 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.            *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * OpenGPS is based on Clifford Kelley's OpenSourceGPS distribution.    *
   * The OpenSourceGPS distribution can be obtained from                  *
   *             http://www.home.earthlink.net/~cwkelley                  *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *                         Real-time process                            *
   *                                                                      *
   ************************************************************************ */

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

   26.01.03 - replace 'trackch' by 'RT_Ctrl->leadch'
              use fields RT_Ctrl->accum_missed & ->accum_new
   18.07.03 - Chan[].ms_count wraps at 6000 (instead of 30000)
   18.07.03 - move Ms_Count to struct Chan[]
   31.07.03 - fixed bug in find/read_preamble()
   03.08.03 - copy carrier-aided code tracking from OpenSourceGPS

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

/***********************************************************************
  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-2002 Clifford Kelley.  All Rights Reserved.
  This LICENSE must be included with the OGR 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 <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>

#include <rtai.h>
#include <rtai_sched.h>
#include <rtai_fifos.h>
#include <rtai_shm.h>

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

#include  "ogrt_defines.h"
#define OGRMAIN
# include  "ogrt_globals.h"
#undef OGRMAIN


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

/* ------------------------------ local variables ------------------------- */

/* ------------------------------ prototypes ------------------------------ */

/* ------------------------------ procedures ------------------------------ */

/* ------------------------------------------------------------------------- *
FUNCTION sign()
RETURNS  integer

PARAMETERS long

PURPOSE
       This function returns +1 when parameter is positive
            0 when zero
           -1 when negative

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

inline INT16 sign( long data)
{
  INT16 res = 0;

  if ( data == 0)
    res = 0;
  else 
    res = ((data > 0) ? 1 : -1);

  return (res);
}


/* ------------------------------------------------------------------------- *
FUNCTION xors()
RETURNS  Integer

PARAMETERS
  pattern  a 32 bit data
        
PURPOSE
  XOR bits in variable 'pattern'

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

INT16 xors( long pattern)
{
  INT16 count=0, i;
                    
  pattern = pattern >> 6;
                     
  for ( i=0; i<26; i++)
  {
    count += (INT16)( pattern & 0x1);
    pattern = pattern >> 1;
  }
  count = count % 2;
                                      
  return (count);
}
                                        
#if 0
/* 
 *  find location of largest element in array buf
 *  determine minimum as well
 */
INT16 find_max_element( INT16 *buf, INT16 n, INT16 *minelem)
{
  INT16 i, res=0, tmp;

  *minelem = tmp = buf[0];

  for (i=1;i<n;i++)
  {
    if ( buf[i] > tmp)
    {
      tmp = buf[i];
      res = i;
    }
    if ( buf[i] < *minelem)
      *minelem = buf[i]; 
  }
  return res;
}
#endif

/* ------------------------------------------------------------------------- *
FUNCTION find_max_element()
RETURNS  Integer

PARAMETERS
  buf  : array
  n    : length of array
        
PURPOSE
  find location of maximum in array

WRITTEN BY
  Georg Beyerle
* ------------------------------------------------------------------------- */

INT16 find_max_element( INT16 *buf, INT16 n)
{
  INT16 i, res = 0, tmp;

  tmp = buf[0];

  for (i=1; i<n; i++)
  {
    if ( buf[i] > tmp)
    {
      tmp = buf[i];
      res = i;
    }
  }
  return res;
}

/* ------------------------------------------------------------------------- *
FUNCTION find_max_element_long()
RETURNS  Integer

PARAMETERS
  buf  : array
  n    : length of array
        
PURPOSE
  find location of maximum in array of longs

WRITTEN BY
  Georg Beyerle
* ------------------------------------------------------------------------- */

INT16 find_max_element_long( long *buf, INT16 n)
{
  INT16 i, res = 0, tmp;

  tmp = buf[0];

  for (i=1; i<n; i++)
  {
    if ( buf[i] > tmp)
    {
      tmp = buf[i];
      res = i;
    }
  }
  return res;
}

/*
 *
 */
INT16 find_elem_center_grav( long *buf, INT16 n)
{
  INT16 i;
  long tmp1 = 0l, tmp2 = 0l;

  for (i=0; i<n; i++)
  {
    tmp1 += i * buf[i];
    tmp2 += buf[i];
  }

  return ((INT16) (tmp1 / tmp2));
}

/* ------------------------------------------------------------------------- *
FUNCTION rt_labs( long x)
RETURNS
  modulus of 'x'

PARAMETERS
  x long integer

PURPOSE
  modulus of variable

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

inline long rt_labs( long x)
{
  return ((x < 0) ? -x : x);
} 

/* ------------------------------------------------------------------------- *
FUNCTION fix_sqrt(long x)
RETURNS  long integer

PARAMETERS
      x long integer

PURPOSE
  This function finds the fixed point square root of a long integer

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

long fix_sqrt( long x)
{
  long xt, scr;
  INT16 i=0;

  xt=x;
  do
  {
     xt = xt>>1;
     i++;
  } while ( xt > 0);

  i = (i>>1)+1;
  xt = x>>i;
  do
  {
     scr = (x - xt*xt) >> 1;
     scr = scr / xt;
     xt  += scr;
  } while ( scr != 0);
  xt = xt << 7;
  return (xt);
}

/* ------------------------------------------------------------------------- *
FUNCTION fix_atan2( long y, long x)
RETURNS  long integer

PARAMETERS 
  x  in phase fixed point value
  y  quadrature fixed point value

PURPOSE
  This function computes the fixed point arctangent2( y, x) 
  represented by x and y in the parameter list
  1 radian = 16384 = 2^14 = 2^FIXPT_BASE
  based on the power series  atan(x) = x - 2/9*x^3

WRITTEN BY
  Clifford Kelley

CHANGES
  Fixed for (y == x), added special code for (x == 0)
* ------------------------------------------------------------------------- */

#define SCALED_PI_ON_2  25736L
#define SCALED_PI       51472L

//#define SCALED_PI_ON_2 ((long)(1.570796326794897 * (0x1L<<FIXPT_BASE)))
//#define SCALED_PI      ((long)(3.141592653589793 * (0x1L<<FIXPT_BASE)))

inline long fix_atan2( long y, long x)
{
  long res = 0, n, n3;

  if ((x == 0) && (y == 0))
    return (res);

  if ( x > 0 &&  x >= rt_labs( y))
  {
     n      = (y << FIXPT_BASE) / x;
     n3     = ((( n*n >> FIXPT_BASE) * n) >> (FIXPT_BASE-1)) / 9;
     res    = n - n3;
  }
  else if (x <= 0 && -x >= rt_labs( y))
  {
     n  = (y << FIXPT_BASE) / x;
     n3 = ((( n*n >> FIXPT_BASE) * n) >> (FIXPT_BASE-1)) / 9;
     if ( y > 0)
       res = n - n3 + SCALED_PI;
     else if (y <= 0)
       res = n - n3 - SCALED_PI;
  }
  else if ( y > 0 &&  y > rt_labs( x))
  {
     n      = (x << FIXPT_BASE) / y;
     n3     = ((( n*n >> FIXPT_BASE) * n) >> (FIXPT_BASE-1)) / 9;
     res    = -n + n3 + SCALED_PI_ON_2;
  }
  else if ( y < 0 && -y > rt_labs( x))
  {
     n      = (x << FIXPT_BASE) / y;
     n3     = ((( n*n >> FIXPT_BASE) * n) >> (FIXPT_BASE-1)) / 9;
     res    = -n + n3 - SCALED_PI_ON_2;
  }

  return (res);
}

/* ------------------------------------------------------------------------- *
FUNCTION fix_atan( long y, long x)
RETURNS  long integer

PARAMETERS 
  x  in phase fixed point value
  y  quadrature fixed point value

PURPOSE
  This function computes the fixed point arctangent(y/x) 
  represented by x and y in the parameter list
  1 radian = 16384 = 2^14
  based on the power series  atan(x) = x - 2/9*x^3

WRITTEN BY
  Clifford Kelley, G. Beyerle
* ------------------------------------------------------------------------- */

inline long fix_atan( long y, long x)
{
  long res = 0, n, n3;

  if ( y == 0)
    return (res);

  if ( x == 0)
    return (SCALED_PI_ON_2 * sign( y));

  if ( rt_labs( y) <= rt_labs( x))
  {
    n   = (y << FIXPT_BASE) / x;
    n3  = ((( n*n >> FIXPT_BASE) * n) >> (FIXPT_BASE-1)) / 9;
    res = n - n3;
  }
  else
  {
    n   = (x << FIXPT_BASE) / y;
    n3  = ((( n*n >> FIXPT_BASE) * n) >> (FIXPT_BASE-1)) / 9;
    if ( sign( y) * sign( x) > 0)
      res = SCALED_PI_ON_2 - (n - n3);
    else 
      res = -SCALED_PI_ON_2 - (n - n3);
  }

  return (res);
}

/* ------------------------------------------------------------------------- *
FUNCTION rss( long a, long b)
RETURNS
  long integer

PARAMETERS
  a : long integer
  b : long integer

PURPOSE
  This function finds the fixed point magnitude of a 2 dimensional vector

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

long rss( long a, long b)
{
  long res, c, d;

  c = rt_labs( a);
  d = rt_labs( b);

  if (c==0 && d==0)
    res = 0;
  else if ( c > d)
    res = (d>>1) + c;
  else
    res = (c>>1) + d;

  return (res);
}

/* ------------------------------- end of file ---------------------------- */