ogr_navsolve2.c

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

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogr_navsolve2.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                   *
   *                                                                      *
   ************************************************************************ */

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

#define FALSE 0
#define TRUE  1

// first  numerical eccentricity
#define E1SQR ((EARTH_MAJ_AXIS*EARTH_MAJ_AXIS-EARTH_MIN_AXIS*EARTH_MIN_AXIS)/(EARTH_MAJ_AXIS*EARTH_MAJ_AXIS)) 

// second numerical eccentricity
#define E2SQR ((EARTH_MAJ_AXIS*EARTH_MAJ_AXIS-EARTH_MIN_AXIS*EARTH_MIN_AXIS)/(EARTH_MIN_AXIS*EARTH_MIN_AXIS))

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

#if 0

/*******************************************************************************
FUNCTION dops_old( 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_old( int SV[], double Xs[32][3], double Xr[], double Cr, 
  double *hdop, double *vdop, double *tdop, double *pdop, double *gdop)
{
  INT16  i, 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;

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

  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 (i=0; i<32; i++)
  {
    if ( SV[i] == TRUE)
    {
/*
 *   Compute line of sight vectors
 */
//    ddx = (track_sat[i].x - x);
//    ddy = (track_sat[i].y - y);
//    ddz = (track_sat[i].z - z);

      ddx = (Xs[i][0] - x);
      ddy = (Xs[i][1] - y);
      ddz = (Xs[i][2] - z);

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

//      printf("dops(): r = %.8e\n", r);

      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)
  {
//  should 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;
}

#endif


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

#if 0
/*
 *  azimuth and elevation from sat and user ECEF XYZ