/* ************************************************************************    *                                                                      *   *                          GPS Simulation                              *   *                                                                      *   * -------------------------------------------------------------------- *   *                                                                      *   *    Module:   gpsfuncs.c                                              *   *                                                                      *   *   Version:   0.1                                                     *   *                                                                      *   *      Date:   17.02.02                                                *   *                                                                      *   *    Author:   G. Beyerle                                              *   *                                                                      *   * -------------------------------------------------------------------- *   *                                                                      *   * Copyright (c) 1996-2001 Clifford Kelley.  All Rights Reserved.       *   * Copyright (C) 2002-2006 Georg 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 'gpsfuncs.cpp', 'gpsrcvr.cpp' and 'gp2021.cpp' are modi-   *   * fied versions of the files with the same name from Clifford Kelley's *    * OpenSourceGPS distribution. The unmodified files can be obtained     *   * from http://www.home.earthlink.net/~cwkelley                         *   *                                                                      *   * -------------------------------------------------------------------- *   *                                                                      *   *                                GPS receiver                          *   *                                                                      *   ************************************************************************ *//* ******************************* changes ********************************   dd.mm.yy -   ************************************************************************ *//***********************************************************************  GPS RECEIVER (GPSRCVR) Ver. 1.02  12 Channel All-in-View GPS Receiver Program based on Mitel GP2021  chipset  Clifford Kelley cwkelley@earthlink.net  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 withoutmodification, are permitted provided that the following conditions aremet:      CONDITIONS1. Redistributions of GPSRCVR source code must retain the above copyrightnotice, this list of conditions, and the following disclaimer.2. Redistributions in binary form must contain the above copyrightnotice, this list of conditions and the following disclaimer in thedocumentation and/or other materials provided with the distribution.3. All modifications to the source code must be clearly marked assuch.  Binary redistributions based on modified source code must beclearly marked as modified versions in the documentation and/or othermaterials provided with the distribution.4. Notice must be given of the location of the availability of theunmodified current source code, e.g.,  http://www.Kelley.com/or  ftp://ftp.Kelley.comin the documentation and/or other materials provided with thedistribution.5. All advertising and published materials mentioning features or useof this software must display the following acknowledgment:  "Thisproduct includes software developed by Clifford Kelley and othercontributors."6. The name of Clifford Kelley may not be used to endorse or promoteproducts derived from this software without specific prior writtenpermission.      DISCLAIMERThis software is provided by Clifford Kelley and contributors "as is" andany expressed or implied warranties, including, but not limited to, theimplied warranties of merchantability and fitness for a particularpurpose are disclaimed.  In no event shall Clifford Kelley orcontributors 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, orprofits; or business interruption) however caused and on any theory ofliability, whether in contract, strict liability, or tort (includingnegligence or otherwise) arising in any way out of the use of thissoftware, even if advised of the possibility of such damage.*//* ------------------------------- includes -------------------------------- */#include <stdio.h>#include <stdlib.h>#include <string.h>#include <time.h>#include <math.h>#ifndef linux# include <io.h>#endif#include "ogsdefine.h"#include "ogsstructs.h"#include "ogsextern.h"#include "ogsprototypes.h"#include "ogslibrary.h"/* ------------------------------ prototypes -------------------------------- */int matherr (struct exception *a);/* ------------------------------- globals ---------------------------------- */extern time_t thetime;/* ------------------------------ functions --------------------------------- *//*******************************************************************************FUNCTION bit_test_l()RETURNS  intPARAMETERS    Data    bit_nPURPOSE  This function returns a 1 if bit number bit_n of long Data is set  else it returns a 0WRITTEN BY  Clifford Kelley*******************************************************************************/inline int  bit_test_l( unsigned long data, char bit_n){  int result;  result = 0;  if (data & test_l[bit_n])    result = 1;  return result;}/*******************************************************************************FUNCTION satfind()RETURNS  None.PARAMETERS None.PURPOSE  THIS FUNCTION DETERMINES THE SATELLITES TO SEARCH FOR  WHEN ALMANAC DATA IS AVAILABLEWRITTEN BY  Clifford Kelley*******************************************************************************/SATVIS satfind( char i){  float   ralt, tdot, b, az;  float   satang, alm_time, almanac_date;  double  range1, range2, xls, yls, zls, xaz, yaz, xn, yn, zn, xe, ye;  long    jd_yr;  ECEF    gpspos1, gpspos2;  SATVIS  result;  int     jd_m;  struct tm *gmt;  double  time_s;  static int justonce = 1;/*      INITIALIZE ALL THE CONSTANTS*/  putenv( tzstr);  tzset();//  thetime = time( NULL);          // *** CHECKME ***  thetime = 1015302770L;  if ( justonce)  {    printf("*** set time to 1015302770 ***\n");    justonce = 0;  }        gmt = gmtime( &thetime);// set up the correct time  if (gmt->tm_mon <= 1)  {    jd_yr = (long)( 365.25 * (gmt->tm_year - 1. + 1900.));  // GB: (long)() inserted    jd_m  = (int)( 30.6001 * (gmt->tm_mon+14.));            // GB: (int)() inserted  }  else  {    jd_yr = (long)( 365.25 * (gmt->tm_year + 1900.));    jd_m  = (int)( 30.6001 * (gmt->tm_mon+2.));  }  time_s = gmt->tm_hour/24. + gmt->tm_min/1440. + gmt->tm_sec/86400. +            1720981.5 + jd_yr + jd_m + gmt->tm_mday;  gps_week     = (int)((time_s - 2444244.5)/7.);  almanac_date = gps_alm[i].week * 7.0 + 2444244.5;  if ( gps_week - gps_alm[i].week > 512)     almanac_date += 1024*7.0;  alm_time  = (time_s - almanac_date) * 86400.;  clock_tow = (long) ((time_s - gps_week*7. - 2444244.5) * 86400.); // GB: inserted cast/*      CALCULATE THE POSITION OF THE SATELLITES*/  if (gps_alm[i].inc > 0.0 && i>0)  {    gpspos1 = satpos_almanac( alm_time,i);    gpspos2 = satpos_almanac( alm_time+60.0,i);/*      CALCULATE THE POSITION OF THE RECEIVER*/    rec_pos_xyz = llh_to_ecef(current_loc);    xn  = -cos( current_loc.lon)*sin( current_loc.lat);    yn  = -sin( current_loc.lon)*sin( current_loc.lat);    zn  =  cos( current_loc.lat);    xe  = -sin( current_loc.lon);    ye  =  cos( current_loc.lon);/*     DETERMINE IF A CLEAR LINE OF SIGHT EXISTS*/    xls  = gpspos1.x - rec_pos_xyz.x;    yls  = gpspos1.y - rec_pos_xyz.y;    zls  = gpspos1.z - rec_pos_xyz.z;    range1 = sqrt(xls*xls+yls*yls+zls*zls);    ralt = sqrt( rec_pos_xyz.x * rec_pos_xyz.x +                  rec_pos_xyz.y * rec_pos_xyz.y +                  rec_pos_xyz.z * rec_pos_xyz.z);    tdot = ( rec_pos_xyz.x * xls +              rec_pos_xyz.y * yls +              rec_pos_xyz.z * zls) / range1 / ralt;    xls  = xls / range1;    yls  = yls / range1;    zls  = zls / range1;//    range2 = sqrt( pow( gpspos2.x - rec_pos_xyz.x, 2) + //                   pow( gpspos2.y - rec_pos_xyz.y, 2) +//                   pow( gpspos2.z - rec_pos_xyz.z, 2));    if ( tdot >= 1.00 )      b = 0.0;    else if ( tdot <= -1.00 )      b = pi;    else      b = acos( tdot);    satang = pi/2.0 - b;    xaz    = xe*xls + ye*yls;    yaz    = xn*xls + yn*yls + zn*zls;    if ( xaz != 0.0 || yaz != 0.0)       az = atan2( xaz, yaz);    else       az = 0.0;    result.x = gpspos1.x;    result.y = gpspos1.y;    result.z = gpspos1.z;    result.elevation = satang;    result.azimuth   = az;//    result.doppler   = (range1-range2) * 5.2514 / 60.;    result.doppler   = 0.;                                  // *** CHECKME ***    static int JustOnce = 1;    if ( JustOnce)    {      printf("sat_find: *** set doppler to zero! ***\n");      JustOnce = 0;    }  //    getchar();  }  return result;}/*******************************************************************************FUNCTION satpos_almanac()RETURNS  None.PARAMETERS None.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 TOAWRITTEN BY  Clifford Kelley******************************************************************************/ECEF satpos_almanac( float time, char n){  double ei,ea,diff,r,ta,la,aol,xp,yp,d_toa;  ECEF result;/*      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].w;  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. - pow(gps_alm[n].ety,2))*sin(ea),cos(ea)-gps_alm[n].ety);  else    ta = ea;/*      TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)*/  r = pow( gps_alm[n].sqra,2)*(1.-pow(gps_alm[n].ety,2)*cos(ea));/*      R IS THE RADIUS OF SATELLITE ORBIT AT TIME T*/  aol = ta + gps_alm[n].w;/*    AOL IS THE ARGUMENT OF LATITUDE    LA IS THE LONGITUDE OF THE ASCENDING NODE*/  la = gps_alm[n].omega0 + (gps_alm[n].omegadot-omegae) * d_toa - gps_alm[n].toa * omegae;  xp = r*cos(aol);  yp = r*sin(aol);  result.x = xp*cos(la) - yp*cos(gps_alm[n].inc)*sin(la);  result.y = xp*sin(la) + yp*cos(gps_alm[n].inc)*cos(la);  result.z = yp*sin(gps_alm[n].inc);  return result;}/*******************************************************************************FUNCTION satpos_ephemeris()RETURNS  None.PARAMETERS None.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 DIFFERENCEWRITTEN BY  Clifford Kelley