ogr_solveosg.c

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

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogr_solveosg.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.            *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * 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                  *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *              Solve for navigation solution (OpenSourceGPS)           *
   *                                                                      *
   ************************************************************************ */

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

   18.07.03 - renamed 'tr_time to 'transmit_time'

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

/***********************************************************************
  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 --------------------------------- */

// use ionospheric & tropospheric correction?
#define IONO_CORR  1
#define TROPO_CORR 1

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

static ECEF satpos_almanac( double time, int n);
//static ECEF llh_to_ecef( LLH pos);

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

static PVT    rpvt;

#ifdef USE_OPENSRCGPS_NAVFIX
static ECEFT  track_sat[NOFCHN+1];
static double meas_dop[13], 
              dt[13],
              cbias; 
#endif

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




/*
 *  find location of maximum in array buf[n]
 */
INT16 maxidx( 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 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

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

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

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

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

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

PARAMETERS 
  i : satellite's PRN

PURPOSE
  THIS FUNCTION DETERMINES THE SATELLITES TO SEARCH FOR
  WHEN ALMANAC DATA IS AVAILABLE

WRITTEN BY
  Clifford Kelley

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

XMITINFO satfind( int i)
{
  double    ralt, tdot, b, az,
            satang, alm_time, almanac_date,
            range1, range2, 
            xls, yls, zls,
            xaz, yaz, 
            xn, yn, zn, xe, ye,
            time_s = 0.0, 
            xr, yr, zr;
  ECEF      gpspos1, 
            gpspos2;
  XMITINFO  res;
  struct tm *gmt;

  gmt = gmtime( &The_Time);

  gmt2julian( &time_s, &Ctrl.gps_week, gmt);

//  we need gps_week info in RT module  *** CHECKME: really? ***
//  RT_Ctrl->gps_week = gps_week;

#if 0
//
//      INITIALIZE ALL THE CONSTANTS
//

//  Julian date of almanach (week start)
  almanac_date = GPS_Alm[i].week * 7.0 + 2444244.5;

  if ( gps_week - GPS_Alm[i].week > 512)
    almanac_date += 1024 * 7.0;

//  time-of-week (seconds)
  alm_time  = (time_s - almanac_date) * 86400.0;

//  time-of-week in seconds
  Ctrl->clock_tow = (UINT32)( (time_s - gps_week*7.0 - 2444244.5)*86400.0);

#endif

  almanac_date = GPS_Alm[i].week * 7.0 + 2444244.5;

  if ( Ctrl.gps_week - GPS_Alm[i].week > 512)
    almanac_date += 1024 * 7.0;

  alm_time  = (time_s - almanac_date) * 86400.0;

/*
      CALCULATE THE POSITION OF THE SATELLITES
*/
  if ( GPS_Alm[i].inc > 0.0 && i > 0)
  {
    double slo, clo, sla, cla;

    gpspos1 = satpos_almanac( alm_time, i);
    gpspos2 = satpos_almanac( alm_time+60.0, i);

/*
 *     CALCULATE THE POSITION OF THE RECEIVER
 */
    Rcvr_Info.pos = llh_to_ecef( Rcvr_Info.llh);

    slo = sin( Rcvr_Info.llh.lon);
    clo = cos( Rcvr_Info.llh.lon);
    sla = sin( Rcvr_Info.llh.lat);
    cla = cos( Rcvr_Info.llh.lat);

    xn  = -clo * sla;
    yn  = -slo * sla;
    zn  =  cla;

    xe  = -slo;
    ye  =  clo;

/*
 *     DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
 */
    xr = Rcvr_Info.pos.x;
    yr = Rcvr_Info.pos.y;
    zr = Rcvr_Info.pos.z;

    xls = gpspos1.x - xr;
    yls = gpspos1.y - yr;
    zls = gpspos1.z - zr;

    range1 = sqrt( xls*xls + yls*yls + zls*zls);
    ralt   = sqrt( xr * xr + yr * yr + zr * zr);

    tdot   = (xr * xls + yr * yls + zr * zls) / range1 / ralt;

    xls = xls / range1;
    yls = yls / range1;
    zls = zls / range1;

    range2 = sqrt( pow( gpspos2.x - xr, 2.0) +
                   pow( gpspos2.y - yr, 2.0) +
                   pow( gpspos2.z - zr, 2.0));

    if ( tdot >= 1.0 )
      b = 0.0;
    else if ( tdot <= -1.0 )
      b = PI;
    else
      b = acos( tdot);

    satang = PI / 2.0 - b;

    xaz = xe*xls + ye*yls;
    yaz = xn*xls + yn*yls + zn*zls;

    az = atan2( xaz, yaz);

    res.pos = gpspos1;
//    res.pos.x = gpspos1.x;
//    res.pos.y = gpspos1.y;
//    res.pos.z = gpspos1.z;

    res.elevation = satang;
    res.azimuth   = az;
    res.doppler   = (range1-range2) * 5.2514 / 60.0;
  }
  return (res);
}


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

PARAMETERS
  time : GPS time at time of transmission
  n    : PRN

PURPOSE
  This subroutine calculates the satellite position
  based on almanac data
    
     R    - RADIUS OF SATELLITE AT TIME T
     SLAT - SATELLITE LATITUDE
     SLONG- SATELLITE LONGITUDE
     T    - TIME FROM START OF WEEKLY EPOCH
     ETY  - ORBITAL ECCENTRICITY
     TOA  - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
     INC  - ORBITAL INCLINATION
     RRA  - RATE OF RIGHT ASCENSION
     SQA  - SQUARE ROOT OF SEMIMAJOR AXIS
     LAN  - LONGITUDE OF NODE AT WEEKLY EPOCH
     AOP  - ARGUMENT OF PERIGEE
     MA   - MEAN ANOMALY AT TOA

WRITTEN BY
  Clifford Kelley

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

static ECEF satpos_almanac( double time, int n)
{
  double ei, ea, diff, r, ta, la, aol, 
         xp, yp, d_toa;
  ECEF   res;

/*
 *  MA IS THE ANGLE FROM PERIGEE AT TOA
 */
  d_toa = time - GPS_Alm[n].toa;
  if ( d_toa > 302400.0)
    d_toa = d_toa - 604800.0;

  ei = GPS_Alm[n].ma + d_toa * GPS_Alm[n].n0;
  ea = ei;

  do
  {
    diff = (ei-(ea-GPS_Alm[n].ety*sin(ea)))/(1.-GPS_Alm[n].ety*cos(ea));
    ea   = ea + diff;
  } while (fabs(diff) > 1.0e-6);

/*
 *      EA IS THE ECCENTRIC ANOMALY
 */
  if ( GPS_Alm[n].ety != 0.0 )
    ta = atan2( sqrt( 1.0 - pow( GPS_Alm[n].ety, 2.0)) * sin( ea), 
                cos(ea) - GPS_Alm[n].ety);
  else
    ta = ea;

/*
 *      TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
 */
  r = GPS_Alm[n].sqra * GPS_Alm[n].sqra * 
      (1.0 - GPS_Alm[n].ety * GPS_Alm[n].ety * cos( ea));

/*
 *    R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
 */
  aol=ta+GPS_Alm[n].aop;

/*
 *    AOL IS THE ARGUMENT OF LATITUDE
 *    LA IS THE LONGITUDE OF THE ASCENDING NODE
 */
  la = GPS_Alm[n].lan + 
       (GPS_Alm[n].rra - OMEGAE) * d_toa - 
       GPS_Alm[n].toa * OMEGAE;
  xp = r * cos( aol);
  yp = r * sin( aol);

  res.x = xp * cos( la) - yp * cos( GPS_Alm[n].inc) * sin( la);
  res.y = xp * sin( la) + yp * cos( GPS_Alm[n].inc) * cos( la);
  res.z = yp * sin( GPS_Alm[n].inc);

  return (res);
}

#ifdef USE_OPENSRCGPS_NAVFIX

/*******************************************************************************
FUNCTION satpos_ephemeris()
RETURNS  satellite position in ECEF coordinates

PARAMETERS
  t : GPS system time at time of transmission
  n : PRN

PURPOSE
  This subroutine calculates the satellite position
  based on broadcast ephemeris data

     R     - RADIUS OF SATELLITE AT TIME T
     Crc   - RADIUS COSINE CORRECTION TERM
     Crs   - RADIUS SINE   CORRECTION TERM
     SLAT  - SATELLITE LATITUDE
     SLONG - SATELLITE LONGITUDE
     TOE   - TIME OF EPHEMERIS FROM START OF WEEKLY EPOCH
     ETY   - ORBITAL INITIAL ECCENTRICITY
     TOA   - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
     INC   - ORBITAL INCLINATION
     IDOT  - RATE OF INCLINATION ANGLE
     CUC   - ARGUMENT OF LATITUDE COSINE CORRECTION TERM
     CUS   - ARGUMENT OF LATITUDE SINE   CORRECTION TERM
     CIC   - INCLINATION COSINE CORRECTION TERM
     CIS   - INCLINATION SINE   CORRECTION TERM
     RRA   - RATE OF RIGHT ASCENSION
     SQA   - SQUARE ROOT OF SEMIMAJOR AXIS
     LAN   - LONGITUDE OF NODE AT WEEKLY EPOCH
     AOP   - ARGUMENT OF PERIGEE
     MA    - MEAN ANOMALY AT TOA
     DN    - MEAN MOTION DIFFERENCE

WRITTEN BY
  Clifford Kelley

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

static ECEFT satpos_ephemeris( double t, int n)
{
  double Mk, 
         Ek, Ek_old, 
         true_anomaly, 
         aol, delr, delal, delinc,
         r, inc, Wk, xp, yp, 
         bclk,
         d_toc, 
         d_toe, 
         xn, yn, zn, xe, ye, xls, yls,
         zls, range1, ralt, tdot,
         xaz, yaz, b;
  ECEFT  res;

//
//     MA IS THE ANGLE FROM PERIGEE AT TOA
//
  d_toc = t - GPS_Eph[n].toc;

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

//  at this point relativistc correction not yet included
//  (eccentric anomaly 'Ek' is not yet known)
  bclk  = GPS_Eph[n].af0 + 
          GPS_Eph[n].af1 * d_toc +
          GPS_Eph[n].af2 * d_toc * d_toc - 
          GPS_Eph[n].tgd;

  d_toe = t - bclk - GPS_Eph[n].toe;

//
//  time from ephemeris reference epoch (GPS-Specs: variable t_k)
//
  if ( d_toe > 302400.0)
    d_toe -= 604800.0;
  else if ( d_toe < -302400.0)
    d_toe += 604800.0;

/*
 *  mean anomaly
 */
  Mk = GPS_Eph[n].ma + (GPS_Eph[n].n0 + GPS_Eph[n].dn) * d_toe;

/*
 *  Ek : eccentric anomaly  
 *  Mk = Ek - sin( Ek)    solve by iteration
 */
  Ek = Mk;
  do
  {
    Ek_old = Ek;
    Ek = Mk + GPS_Eph[n].ety * sin( Ek);
  } while ( fabs( Ek_old - Ek) > 1.0e-9 );

/*
 *  true_anomaly (angle from perigee)
 */
  true_anomaly = atan2( sqrt( 1.0 - GPS_Eph[n].ety * 
                              GPS_Eph[n].ety) * sin( Ek),
                        cos( Ek) - GPS_Eph[n].ety);

/*
 *  aol IS THE ARGUMENT OF LATITUDE OF THE SATELLITE
 */
  aol = true_anomaly + GPS_Eph[n].w;

/*
 *  second harmonic perturbations of the orbit
 */

//  radius correction
  delr   = GPS_Eph[n].crc * cos( 2.0*aol) + 
           GPS_Eph[n].crs * sin( 2.0*aol);
//  arg of latitude correction