ogr_solvessvl.c

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

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogr_solvesvl.c                                          *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   09.12.02                                                *
   *                                                                      *
   *    Author:   Sam Storm van Leeuwen, G. Beyerle                       *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * Copyright (C) 2001-2003  Sam Storm van Leeuwen, 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                         * 
   *         (port of stdalone.pas from Sam Storm van Leeuwen)            *
   *                                                                      *
   ************************************************************************ */

/*
 *
 * @author Sam Storm van Leeuwen <[e-mail]>
 * ported to C by David Johnston 
 * (http://privatewww.essex.ac.uk/~djjohn/projects/gps/djjweb/index.html)
 *
 * The original Pascal code and documentation is available from
 *   http://home-2.worldonline.nl/~samsvl/software.htm
 *
 */

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

   18.07.03 - 

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

/* ------------------------------- 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"
#include  "ogr_error.h"

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

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

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

//static UINT16 SelPRN = 8;

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

/*******************************************************************************
FUNCTION dops( int SV[], double Xs[32][3], double Xr[], double Cr, 
  double *hdop, double *vdop, double *tdop, double *pdop, double *gdop)

RETURNS  
  dilutions of precision

PARAMETERS None.

PURPOSE
  This routine computes the dops

INPUTS:
  track_sat[nsl][3]  array of satellite locations in ECEF 
                     when the signal was sent
  nsl                number of satellites used
                     receiver position

OUTPUTS:
  hdop : horizontal dilution of precision
  vdop : vertical dilution of precision
  tdop : time dilution of precision
  pdop : position dilution of precision
  gdop : geometric dilution of precision (rss of pdop & tdop)

WRITTEN BY
  Clifford Kelley

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

static void dops( int n_max, ECEF rcvr_pos, ECEF xmit_pos[], double Cr, 
  double *hdop, double *vdop, double *tdop, double *pdop, double *gdop)
{
  INT16  n, k, nsl;
  double ddx, ddy, ddz, 
         ddt, r, 
         rxy, 
         ms[5][13], 
         x, y, z,
         a1 = 0.0, b1 = 0.0, c1 = 0.0, d1 = 0.0, 
         e1 = 0.0, f1 = 0.0, g1 = 0.0, h1 = 0.0,
         i1 = 0.0, j1 = 0.0, k1 = 0.0, l1 = 0.0, 
         m1 = 0.0, n1 = 0.0, o1 = 0.0, p1 = 0.0, 
         denom,
         msx, msy, msz;
  ECEF   north, east, up;
  double Xs[NOFCHN][3];

//  copy transmitter positions to 2d array
  for ( n = 0; n < n_max; n++) 
  {
    Xs[n][0] = xmit_pos[n].x;
    Xs[n][1] = xmit_pos[n].y;
    Xs[n][2] = xmit_pos[n].z;
  }

  x       = rcvr_pos.x;
  y       = rcvr_pos.y;
  z       = rcvr_pos.z;

  r       = sqrt( x*x + y*y + z*z);
  rxy     = sqrt( x*x + y*y);
  north.x = -x/rxy * z/r;
  north.y = -y/rxy * z/r;
  north.z = rxy/r;
  east.x  = -y/rxy;
  east.y  = x/rxy;
//east.z=0.0; just for completeness
  up.x    = x/r;
  up.y    = y/r;
  up.z    = z/r;

  k = 0;
  for ( n=0; n<n_max; n++)
  {
/*
 *   Compute line of sight vectors
 */
    ddx = (Xs[n][0] - x);
    ddy = (Xs[n][1] - y);
    ddz = (Xs[n][2] - z);

    r = sqrt( ddx * ddx + ddy * ddy + ddz * ddz);

    ms[0][k] = ddx / r;
    ms[1][k] = ddy / r;
    ms[2][k] = ddz / r;
    ms[3][k] = 1.0;
    k += 1;
  }

  nsl = k;

  if ( nsl > NOFCHN)
  {
//  shouldn't happen ...
    return;
  }

  for ( k = 0; k < nsl; k++)
  {
    msx = ms[0][k]*north.x + ms[1][k]*north.y + ms[2][k]*north.z;
    msy = ms[0][k]*east.x  + ms[1][k]*east.y;
    msz = ms[0][k]*up.x    + ms[1][k]*up.y    + ms[2][k]*up.z;

    ms[0][k] = msx;
    ms[1][k] = msy;
    ms[2][k] = msz;
    ms[3][k] = 1.0;
  }

  for ( k = 0; k < nsl; k++)
  {
    a1 += ms[0][k] * ms[0][k];
    b1 += ms[0][k] * ms[1][k];
    c1 += ms[0][k] * ms[2][k];
    d1 += ms[0][k] * ms[3][k];

    f1 += ms[1][k] * ms[1][k];
    g1 += ms[1][k] * ms[2][k];
    h1 += ms[1][k] * ms[3][k];

    k1 += ms[2][k] * ms[2][k];
    l1 += ms[2][k] * ms[3][k];

    p1 += ms[3][k];
  }

  o1 = l1;
  m1 = d1;
  n1 = h1;
  e1 = b1;
  i1 = c1;
  j1 = g1;

/*
 *     SOLVE FOR THE DIAGONALS OF THE MATRIX INVERSE
 */
  denom = (k1*p1-l1*o1) * (a1*f1-b1*e1) + 
          (l1*n1-j1*p1) * (a1*g1-c1*e1) + 
          (j1*o1-k1*n1) * (a1*h1-d1*e1) + 
          (l1*m1-i1*p1) * (c1*f1-b1*g1) + 
          (i1*o1-k1*m1) * (d1*f1-b1*h1) +
          (i1*n1-j1*m1) * (c1*h1-d1*g1);

  ddx = f1*(k1*p1-l1*o1) + g1*(l1*n1-j1*p1) + h1*(j1*o1-k1*n1);
  ddy = a1*(k1*p1-l1*o1) + c1*(l1*m1-i1*p1) + d1*(i1*o1-k1*m1);
  ddz = a1*(f1*p1-h1*n1) + b1*(h1*m1-e1*p1) + d1*(e1*n1-f1*m1);
  ddt = a1*(f1*k1-g1*j1) + b1*(g1*i1-e1*k1) + c1*(e1*j1-f1*i1);

  if ( denom <= 0.0)
  {
    *hdop = 0.0;      // something went wrong
    *vdop = 0.0; 
    *tdop = 0.0;
    *pdop = 0.0;
    *gdop = 0.0;
  }
  else
  {
    *hdop = sqrt( (ddx+ddy) / denom);
    *vdop = sqrt( ddz / denom);
    *tdop = sqrt( ddt / denom);
    *pdop = sqrt( (*vdop) * (*vdop) + (*hdop) * (*hdop));
    *gdop = sqrt( (*pdop) * (*pdop) + (*tdop) * (*tdop));
  }
  return;
}


/*
 *  azimuth and elevation of GPS sat as seen from receiver
 */
void sat_azimuth_elevation( ECEF xmit_pos, ECEF rcvr_pos, 
  double *Az, double *El)
{
  long   i, k;
  double R, p, 
         Xr[3], Xs[3], s,
         e[3][3], d[3];

  Xr[0] = rcvr_pos.x;
  Xr[1] = rcvr_pos.y;
  Xr[2] = rcvr_pos.z;

  Xs[0] = xmit_pos.x;
  Xs[1] = xmit_pos.y;
  Xs[2] = xmit_pos.z;

  p = sqrt( Xr[0] * Xr[0] + Xr[1] * Xr[1]);

  if ( p == 0.0)
  {
/* *** CHECKME *** */
    *El = atan2( (Xr[2]>0 ? 1 : -1)*( Xs[2]-Xr[2]), 
                 sqrt( Xs[0]*Xs[0] + Xs[1]*Xs[1]));
    *Az = atan2( Xs[1]-Xr[1], Xs[0]-Xr[0]);
    return;
  }

  R = sqrt( Xr[0]*Xr[0] + Xr[1]*Xr[1] + Xr[2]*Xr[2]);

  e[0][0] = -(Xr[1] / p);
  e[0][1] =   Xr[0] / p;
  e[0][2] = 0.0;
  e[1][0] = -(Xr[0] * Xr[2] / (p * R));
  e[1][1] = -(Xr[1] * Xr[2] / (p * R));
  e[1][2] = p / R;
  e[2][0] = Xr[0] / R;
  e[2][1] = Xr[1] / R;
  e[2][2] = Xr[2] / R;

/* matrix multiply vector from user */
  for (k = 0; k < 3; k++) 
  {
    d[k] = 0.0;
    for (i = 0; i < 3; i++)
      d[k] += (Xs[i] - Xr[i]) * e[k][i];
  }

  s = d[2] / sqrt( d[0]*d[0] + d[1]*d[1] + d[2]*d[2]);

  *El = atan2( s, sqrt( 1.0 - s*s));
  *Az = atan2( d[0], d[1]);

  return;
}

/*
 * ion     iono correction coefficients from nav message
 *
 * Latu    user's latitude [rad]
 * Lonu    user's longitude [rad]
 * Az      SV azimuth [rad]
 * El    SV elevation [rad]
 * Ttr    SV signal transmission time [sec]
 * dTiono  Ionospheric delay [sec]
 */
static double ionocorr( IONO iono, double Latu, double Lonu, double Az, 
                        double El, double Ttr)
{
  double phi, 
         Lati, Loni, Latm, 
         T, F_, x, per, amp, 
         a0, a1, a2, a3, 
         b0, b1, b2, b3,
         res;

/* for clarity copy array */
  a0 = Iono.al0;
  a1 = Iono.al1;
  a2 = Iono.al2;
  a3 = Iono.al3;
  b0 = Iono.b0;
  b1 = Iono.b1;
  b2 = Iono.b2;
  b3 = Iono.b3;

/* convert from radians to semicircles */
  Latu /= M_PI;
  Lonu /= M_PI;
  Az   /= M_PI;
  El   /= M_PI;

/* calculation */
  phi  = 0.0137 / (El + 0.11) - 0.022;
  Lati = Latu + phi * cos( Az * M_PI);

  if ( Lati > 0.416)
    Lati = 0.416;
  else if (Lati < -0.416)
    Lati = -0.416;

  Loni = Lonu + phi * sin( Az * M_PI) / cos( Lati * M_PI);
  Latm = Lati + 0.064 * cos( (Loni - 1.617) * M_PI);
  T = 4.32e+4 * Loni + Ttr;

  while ( T >= 86400.0)
    T -= 86400.0;
  while ( T < 0.0)
    T += 86400.0;

  F_ = 1.0 + 16.0 * (0.53 - El) * (0.53 - El) * (0.53 - El);
  per = b0 + b1 * Latm + b2 * Latm * Latm + b3 * Latm * Latm * Latm;

  if ( per < 72000.0)
    per = 72000.0;

  x = 2 * M_PI * (T - 50400.0) / per;
  amp = a0 + 
        a1 * Latm + 
        a2 * Latm * Latm + 
        a3 * Latm * Latm * Latm;

  if ( amp < 0.0)
    amp = 0.0;

  if ( fabs( x) >= 1.57)
    res = F_ * 5.0e-9;
  else
    res = F_ * (5.0e-9 + amp * 
      (1.0 - x * x / 2.0 + x * x * x * x / 24.0));

  return (res);
}

/* 
 * calculate satellite position in ECEF co-ordinate system
 *
 * IN:   ephemeris  eph   
 *       satellite GPS time  Ttr
 * OUT:  relativistic correction term  Trel
 *       satellite position  X  
 */
ECEF satellite_position( INT16 prn, double Ttr, double *Trel)
{
  double     ec, A, T, n0, n, M, E, Eold, 
             snu, cnu, nu, phi, du, dr, di, u, r, i,
             Xdash, Ydash, Wc;
  ECEF       res;
  EPHEMERIS *eph;

//  select ephemeris data
  eph = &GPS_Eph[prn];

  A    = eph->sqra * eph->sqra;

#if 0
  Crs  = eph->crs;
  dn   = eph->dn;
  M0   = eph->ma;
  Cuc  = eph->cuc;
  ec   = eph->ety;
  Cus  = eph->cus;
  Toe  = eph->toe;
  Cic  = eph->cic;
  W0   = eph->W0;
  Cis  = eph->cis;
  i0   = eph->inc0;
  Crc  = eph->crc;
  w    = eph->w;
  Wdot = eph->Wdot;
  Idot = eph->idot;
#endif

#if 0
  if ( first == 0)
  {
    first = 1;  
    printf(" dn=%e M0=%e A=%e Toe=%e W0=%e i0%e w=%e Wdot=%e Idot=%e\n", 
      dn, M0, A, Toe, W0, i0, w, Wdot, Idot);
  }
#endif

  T = Ttr - eph->toe;
  if ( T > 302400.0)
    T -= 604800.0;
  else if (T < -302400.0)
    T += 604800.0;

//  printf(" T=%.12e Toe=%.12e\n", T, Toe);

  n0 = sqrt( GRAVCONST / (A * A * A));
  n = n0 + eph->dn;

  M = eph->ma + n * T;  
  E = M;   /* start value for E */

//  printf(" dn=%.9e Mo=%.9e ec=%.9e A=%.9e Toe=%.9e W0=%.9e i0=%.9e w=%.9e Wdot=%.9e idot=%.9e\n", 
//    dn, M0, ec, A, Toe, W0, i0, w, Wdot, Idot);

  ec = eph->ety;

#if 0
  if ( prn == SelPRN)
  {
    printf(" dn=%.8e M0=%.8e ec=%.8e A=%.8e Toe=%.8e W0=%.8e i0%.8e w=%.8e Wdot=%.8e Idot=%.8e\n", 
      eph->dn, eph->ma, ec, A, eph->toe, eph->W0, eph->inc0, eph->w, eph->Wdot, eph->idot);
  }
#endif

  do 
  {
    Eold = E;
    E = M + ec * sin( E);
  } while ( fabs( E - Eold) >= 1.0e-8);

//  printf(" M0=%.12e n=%.12e T=%.12e Ttr=%.12e Toe=%.12e E=%.12e\n", M0, n, T, Ttr, Toe, E);
//  printf(" E=%.9e M=%.9e ec=%.9e\n", E, M, ec);

  snu = sqrt(1 - ec * ec) * sin( E) / (1 - ec * cos( E));
  cnu = (cos( E) - ec) / (1 - ec * cos( E));

  nu = atan2( snu, cnu);

  phi = nu + eph->w;

  du = eph->cuc * cos( 2 * phi) + eph->cus * sin( 2 * phi);
  dr = eph->crc * cos( 2 * phi) + eph->crs * sin( 2 * phi);
  di = eph->cic * cos( 2 * phi) + eph->cis * sin( 2 * phi);

  u = phi + du;
  r = A * (1 - ec * cos( E)) + dr;
  i = eph->inc0 + eph->idot * T + di;

  Xdash = r * cos( u);
  Ydash = r * sin( u);

  Wc = eph->W0 + (eph->Wdot - OMEGAE) * T - OMEGAE * eph->toe;

  res.x = Xdash * cos( Wc) - Ydash * cos( i) * sin( Wc);
  res.y = Xdash * sin( Wc) + Ydash * cos( i) * cos( Wc);
  res.z = Ydash * sin( i);

#if 0
  if ( prn == SelPRN)
  {
    printf(" du=%.8e dr=%.8e di=%.8e Xd=%.8e Yd=%.8e Wc=%.8e X=[%.8e %.8e %.8e]\n", 
      du, dr, di, Xdash, Ydash, Wc, res.x, res.y, res.z);
  }
#endif

  *Trel = FCONST * ec * eph->sqra * sin( E);   /* relativistic correction term */

  return (res);
}

/*
 *  At many places in the following procedure solve the subdeterminant 
 *  value of a 4 x 4 array is required. For convenience the function sub 
 * is defined below
 *
 * IN:  A  input 4 x 4 array
 * IN:  r  row number to be deleted
 * IN:  c_  column number to be deleted
 * OUT:    value of 3 x 3 subdeterminant
 */
static double sub( double A[4][4], long r, long c_)
{
  double B[3][3];
  long i, i1, j, j1;

  i1 = 0;
  for (i = 0; i <= 3; i++)