ogr_tools.c

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

/* ************************************************************************ 
   *                                                                      *
   *                            OpenGPS Receiver                          *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   ogr_tools.c                                             *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   09.12.02                                                *
   *                                                                      *
   *    Author:   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                   *
   *                                                                      *
   ************************************************************************ */

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

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

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

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

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

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

/*
 * get elevation/azimuth of GPS from receiver location
 */
void calc_elevation_azimuth( double *elevation, double *azimuth, 
  ECEF xpos, ECEF rpos)
{
  double b, xr, yr, zr, 
         xls, yls, zls,
         range, ralt, tdot,
         slo, clo, sla, cla,
         xn, yn, zn, xe, ye,
         xaz, yaz;
  LLH    llh;

  llh = ecef_to_llh( rpos);

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

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

  xe  = -slo;
  ye  =  clo;
/*
*     DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
*/
  xr = rpos.x;
  yr = rpos.y;
  zr = rpos.z;

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

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

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

  xls = xls / range;
  yls = yls / range;
  zls = zls / range;

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

  *elevation = PI / 2.0 - b;

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

  *azimuth = atan2( xaz, yaz);

  return;
}

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