www.pudn.com > opengpssim.zip > gpsfuncs.c
/* ************************************************************************
* *
* 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 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
#include
#include
#include
#include
#ifndef linux
# include
#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 int
PARAMETERS
Data
bit_n
PURPOSE
This function returns a 1 if bit number bit_n of long Data is set
else it returns a 0
WRITTEN 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 AVAILABLE
WRITTEN 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 TOA
WRITTEN 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 DIFFERENCE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
ECEFT satpos_ephemeris(double t, char n)
{
double ei,ea,diff,ta,aol,delr,delal,delinc,r,inc;
double la,xp,yp,bclk,tc,d_toc,d_toe;
double xn,yn,zn,xe,ye,xls,yls,zls,range1,ralt,tdot,satang,xaz,yaz;
double b,az;
ECEFT result;
//
// MA IS THE ANGLE FROM PERIGEE AT TOA
//
d_toc = t-gps_eph[n].toc;
if ( d_toc>302400.0)
d_toc = d_toc-604800.0;
else if (d_toc<-302400.0)
d_toc=d_toc+604800.0;
bclk = gps_eph[n].af0 + gps_eph[n].af1*d_toc +
gps_eph[n].af2*d_toc*d_toc - gps_eph[n].tgd;
tc = t - bclk;
d_toe = tc - gps_eph[n].toe;
if ( d_toe > 302400.0)
d_toe = d_toe-604800.0;
else if (d_toe < -302400.0)
d_toe = d_toe + 604800.0;
ei = gps_eph[n].ma + d_toe*gps_eph[n].wm + gps_eph[n].dn;
ea = ei;
do
{
diff = (ei-(ea-gps_eph[n].ety*sin(ea)))/(1.0E0-gps_eph[n].ety*cos(ea));
ea = ea+diff;
} while ( fabs(diff) > 1.0e-9 );
bclk = bclk-4.442807633E-10*gps_eph[n].ety*gps_eph[n].sqra*sin(ea);
result.tb = bclk;
//
// ea is the eccentric anomaly
//
ta = atan2(sqrt(1.00-pow(gps_eph[n].ety,2))*sin(ea),cos(ea)-gps_eph[n].ety);
//
// TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
//
aol = ta + gps_eph[n].w;
//
// AOL IS THE ARGUMENT OF LATITUDE OF THE SATELLITE
//
// calculate the second harmonic perturbations of the orbit
//
delr = gps_eph[n].crc*cos( 2.0*aol) + gps_eph[n].crs*sin( 2.0*aol);
delal = gps_eph[n].cuc*cos( 2.0*aol) + gps_eph[n].cus*sin( 2.0*aol);
delinc = gps_eph[n].cic*cos( 2.0*aol) + gps_eph[n].cis*sin( 2.0*aol);
//
// R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
//
r = pow(gps_eph[n].sqra,2)*(1.00-gps_eph[n].ety*cos(ea))+delr;
aol = aol+delal;
inc = gps_eph[n].inc0+delinc+gps_eph[n].idot*d_toe;
// WRITE(6,*)T-TOE(N)
//
// LA IS THE CORRECTED LONGITUDE OF THE ASCENDING NODE
//
la = gps_eph[n].omega0 + (gps_eph[n].omegadot-omegae)*d_toe -
omegae*gps_eph[n].toe;
xp = r*cos(aol);
yp = r*sin(aol);
result.x = xp*cos(la)-yp*cos(inc)*sin(la);
result.y = xp*sin(la)+yp*cos(inc)*cos(la);
result.z = yp*sin(inc);
result.az = 0.0;
result.el = 0.0;
if ( rec_pos_xyz.x != 0.0 || rec_pos_xyz.y != 0.0 || rec_pos_xyz.z != 0.0)
{
// fprintf(stream,"x=%20.10lf,y=%20.10lf,z=%20.10lf\n",rec_pos_xyz.x,
// rec_pos_xyz.y,rec_pos_xyz.z);
/*
CALCULATE THE POSITION OF THE RECEIVER
*/
xn = -cos(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
yn = -sin(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
zn = cos(rec_pos_llh.lat);
xe = -sin(rec_pos_llh.lon);
ye = cos(rec_pos_llh.lon);
/*
DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
*/
xls = result.x-rec_pos_xyz.x;
yls = result.y-rec_pos_xyz.y;
zls = result.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;
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.el = satang;
result.az = az;
}
return result;
}
/*******************************************************************************
FUNCTION read_initial_data()
RETURNS None.
PARAMETERS None.
PURPOSE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void read_initial_data(void)
{
int id;
SATVIS dummy;
for (id=1; id<=32; id++)
gps_alm[id].inc = 0.0;
//
// READ THE INPUT DATA FILE(s)
//
// for initialization, if we have data we will switch to warm or hot start
status = cold_start;
read_ion_utc();
read_almanac();
dummy = satfind( 0);
read_ephemeris();
thetime = time( NULL);
// printf( "thetime = %d\n", thetime);
// getchar();
return;
}
/*******************************************************************************
FUNCTION receiver_loc()
RETURNS None.
PARAMETERS None.
PURPOSE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
LLH receiver_loc( void)
{
float latitude, longitude, height;
char text[10];
LLH result;
FILE *in;
result.lat = 0.0;
result.lon = 0.0;
result.hae = 0.0;
/*
* READ THE CURRENT LOCATION DATA FILE
*/
char infile[] = "curloc.dat";
char *tmpstr;
tmpstr = conmalloc( strlen( OGSDataDir) + strlen( infile) + 1);
strcpy( tmpstr, OGSDataDir);
strcat( tmpstr, infile);
if ((in = fopen( tmpstr, "rt")) == NULL)
{
printf( "error opening %s.\n", tmpstr);
status = cold_start;
}
else
{
fscanf( in, "%10s", text);
fscanf( in, "%f", &latitude);
fscanf( in, "%10s", text);
fscanf( in, "%f", &longitude);
fscanf( in, "%10s", text);
fscanf( in, "%f", &height);
result.lat = latitude / 57.296;
result.lon = longitude / 57.296;
result.hae = height;
}
fclose( in);
if ( tmpstr)
free( tmpstr);
return (result);
}
/*******************************************************************************
FUNCTION navmess()
RETURNS None.
PARAMETERS None.
PURPOSE
This function assembles and decodes the 1500 bit nav message
into almanac and ephmeris messages
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void decode_navmess( char prn, char ch)
{
int i,j,k;
unsigned long isqra, ie, iomega0;
long iaf0, iomegadot;
char itgd, iaf2;
int iweek, icapl2, iura, ihealth, iodc, iodct, iaf1;
unsigned int itoe, itoc;
int iode, icrs, idn, icuc, icus, fif, icic, iomegad;
int icis, icrc, idoe, idot;
unsigned int iae, iatoa;
// static i4page, i5page;
static int i4page, i5page; // GB: int inserted
int i4data, i5data, isv, iaomegad;
long iaaf0, iaaf1, iadeli, iaomega0, im0, inc0, iw;
unsigned long iasqr;
long iaw, iam0, scale, ia0, ia1;
char ial0, ial1, ial2, ial3, ibt0, ibt1, ibt2, ibt3;
int itot, iWNt, idtls, iWNlsf, iDN, idtlsf, WNa;
int sfr, word, i4p, i5p;
float d_toe;
//
// assemble the bits into subframes and words
//
d_toe = clock_tow - gps_eph[prn].toe;
if (d_toe > 302400.0)
d_toe = d_toe - 604800.0;
else if ( d_toe < -302400.0)
d_toe = d_toe + 604800.0;
if ( gps_eph[prn].valid==0 ||
(almanac_valid==0 && almanac_flag==0) ||
fabs( d_toe) > 7200.0)
{
j=0;
for ( sfr=1; sfr<=5; sfr++)
{
for ( word=1;word<=10;word++)
{
scale = 536870912L; // 2^29
sf[sfr][word] = 0;
for ( i=0;i<=29;i++)
{
if ( chan[ch].message[(j+chan[ch].offset)%1500] == 1)
sf[sfr][word] += scale;
scale = scale>>1;
j++;
}
}
}
parity_check(); // check the parity of the 1500 bit message
//
// EPHEMERIS DECODE subframes 1, 2, 3
//
// subframe 1
//
if ((p_error[1]==0 || p_error[1]==0x200) && p_error[2]==0 && p_error[3]==0)
{
gps_eph[prn].valid=0;
iodc = (int)(((sf[1][3] & 0x3) <<8 ) | ((sf[1][8] & 0xFF0000L) >> 16));
// iodct=iodc & 0xff;
// iode=sf[2][3] >> 16;
// idoe=sf[3][10] >> 16;
// if ( iodct != iode || iodct!= idoe || iode != idoe || gps_eph[prn].sqra==0.0)
//
iweek = (int)(sf[1][3] >> 14);
gps_eph[prn].week = iweek;
// icapl2 = ( sf[1][3] & 0x3000 ) >> 12;
iura = (int)(( sf[1][3] & 0xF00 ) >> 8);
gps_eph[prn].ura = iura;
ihealth = (int)(( sf[1][3] & 0xFC ) >> 2);
gps_eph[prn].health = ihealth;
gps_eph[prn].iodc = iodc;
itgd = (int)( sf[1][7] & 0xFF);
gps_eph[prn].tgd = itgd*4.656612873e-10;
itoc = (int)( sf[1][8] & 0xFFFF);
gps_eph[prn].toc = itoc*16.0;
iaf2 = (int)( sf[1][9] >> 16);
gps_eph[prn].af2 = iaf2*2.775557562e-17;
iaf1 = (int)( sf[1][9] & 0xFFFF);
gps_eph[prn].af1 = iaf1*1.136868377e-13;
iaf0 = sf[1][10] >> 2;
if ( bit_test_l( iaf0, 22))
iaf0 = iaf0 | 0xFFC00000L;
gps_eph[prn].af0 = iaf0*4.656612873e-10;
//
// subframe 2
//
icrs = (int)(sf[2][3] & 0xFFFF);
gps_eph[prn].crs = icrs*.03125;
idn = (int)(sf[2][4] >> 8);
gps_eph[prn].dn = idn*1.136868377e-13*pi;
im0 = ((sf[2][4] & 0xFF) << 24) | sf[2][5];
gps_eph[prn].ma = im0*4.656612873e-10*pi;
icuc = (int)(sf[2][6] >>8);
gps_eph[prn].cuc = icuc*1.862645149e-9;
ie = ((sf[2][6] & 0xFF) << 24) | sf[2][7];
gps_eph[prn].ety = ie*1.164153218e-10;
icus = (int)(sf[2][8] >> 8);
gps_eph[prn].cus = icus*1.862645149e-9;
isqra = (((sf[2][8] & 0xFF) << 24) | sf[2][9]);
gps_eph[prn].sqra = isqra*1.907348633e-6;
if (gps_eph[prn].sqra>0.0)
gps_eph[prn].wm = 19964981.84/pow(gps_eph[prn].sqra,3);
itoe = (int)(sf[2][10] >> 8);
gps_eph[prn].toe = itoe*16.;
// fif=(sf[2][10] & 0x80) >> 7;
//
// subframe 3
//
icic = (int)(sf[3][3] >> 8);
gps_eph[prn].cic = icic*1.862645149e-9;
icis = (int)(sf[3][5] >> 8);
gps_eph[prn].cis = icis*1.862645149e-9;
inc0 = ((sf[3][5] & 0xFF) << 24) | sf[3][6];
gps_eph[prn].inc0 = inc0*4.656612873e-10*pi;
iomega0 = ((sf[3][3] & 0xFF) << 24) | sf[3][4];
gps_eph[prn].omega0=iomega0*4.656612873e-10*pi;
icrc=(int)(sf[3][7] >> 8);
gps_eph[prn].crc=icrc*.03125;
iw=((sf[3][7] & 0xFF) << 24) | sf[3][8];
gps_eph[prn].w=iw*4.656612873e-10*pi;
iomegadot=sf[3][9];
if (bit_test_l(iomegadot,24))
iomegadot=iomegadot | 0xFF000000L;
gps_eph[prn].omegadot=iomegadot*1.136868377e-13*pi;
idot=(int)((sf[3][10] & 0xFFFC) >> 2);
if (bit_test_l(idot,14))
idot=idot | 0xC000;
gps_eph[prn].idot=idot*1.136868377e-13*pi;
if ( gps_eph[prn].inc0 != 0.0 &&
gps_eph[prn].sqra > 5000.0 &&
gps_eph[prn].ety < .05)
gps_eph[prn].valid = 1;
}
//
// ALMANAC DECODE subframes 4 and 5
//
// SUBFRAME 4
//
if (p_error[4]==0 && p_error[5]==0 && almanac_valid==0 && almanac_flag==0)
{
almanac_flag=1;
// i4data= sf[4][3] >> 22;
i4p = (int)((sf[4][3] & 0x3F0000L) >> 16);
if ( i4p != i4page)
{
i4page=i4p;
if (i4page > 24 && i4page < 33)
{
isv=i4page ;
gps_alm[isv].week=gps_week;
iae=(int)(sf[4][3] & 0x00FFFFL);
gps_alm[isv].ety=iae*4.768371582E-7;
iatoa=(int)(sf[4][4] >> 16);
gps_alm[isv].toa=iatoa*4096.0;
iadeli=sf[4][4] & 0x00FFFFL;
if (bit_test_l(iadeli,16))
iadeli=iadeli | 0xFFFF0000L;
gps_alm[isv].inc=(iadeli*1.907348633E-6+0.3)*pi;
iomegad=(int)(sf[4][5] >> 8);
gps_alm[isv].omegadot=iomegad*3.637978807E-12*pi;
gps_alm[isv].health=(int)(sf[4][5] & 0x0000FF);
iasqr=sf[4][6];
gps_alm[isv].sqra=iasqr*4.8828125E-4;
if (gps_alm[isv].sqra>0.0)
gps_alm[isv].w=19964981.84/pow(gps_alm[isv].sqra,3);
iaomega0=sf[4][7];
if (bit_test_l(iaomega0,24))
iaomega0=iaomega0 | 0xFF000000L;
gps_alm[isv].omega0=iaomega0*1.192092896E-7*pi;
iaw=sf[4][8];
if (bit_test_l(iaw,24))
iaw=iaw | 0xFF000000L;
gps_alm[isv].w=iaw*1.192092896E-7*pi;
iam0=sf[4][9];
if (bit_test_l(iam0,24))
iam0=iam0 | 0xFF000000L;
gps_alm[isv].ma=iam0*1.192092896E-7*pi;
// iaaf0=(sf[4][10] >> 13) | (sf[4][10] & 0x1C);
iaaf0=(sf[4][10] >> 13) | ((sf[4][10] & 0x1C) >> 2); // fixed (GB-020217)
if (bit_test_l(iaaf0,11))
iaaf0=iaaf0 | 0xFFFFF800L;
gps_alm[isv].af0=iaaf0*9.536743164E-7;
iaaf1=(sf[4][10] | 0xFFE0) >> 5;
if (bit_test_l(iaaf1,11))
iaaf1=iaaf1 | 0xFFFFF800L;
gps_alm[isv].af1=iaaf1*3.637978807E-12;
}
else if ( i4page == 55 )
{
gps_alm[prn].text_message[0]=(char)((sf[4][3] & 0x00FF00) >> 8);
gps_alm[prn].text_message[1]=(char)( sf[4][3] & 0x0000FF);
for ( k=1;k<=7;k++)
{
gps_alm[prn].text_message[3*k-1] = (char)(sf[4][k+3] >> 16);
gps_alm[prn].text_message[3*k ] = (char)((sf[4][k+3] & 0x00FF00) >> 8);
gps_alm[prn].text_message[3*k+1] = (char)(sf[4][k+3] & 0x0000FF);
}
}
else if ( i4page == 56 )
{
// broadcast ionospheric data
ial0 = (int)((sf[4][3] & 0x00FF00) >> 8);
Iono.al0 = ial0*9.313225746e-10;
ial1 = (int)(sf[4][3] & 0x0000FF);
Iono.al1 = ial1*7.4505806e-9;
ial2 = (int)(sf[4][4] >> 16);
Iono.al2 = ial2*5.960464478e-8;
ial3 = (int)((sf[4][4] & 0x00FF00) >> 8);
Iono.al3 = ial3*5.960464478e-8;
ibt0 = (int)(sf[4][4] & 0x0000FF);
Iono.b0 = ibt0*2048.;
ibt1 = (int)(sf[4][5] >> 16);
Iono.b1 = ibt1*16384.;
ibt2 = (int)((sf[4][5] & 0x00FF00) >> 8);
Iono.b2 = ibt2*65536.;
ibt3 = (int)(sf[4][5] & 0x00FF);
Iono.b3 = ibt3*65536.;
ia1 = sf[4][6];
if (bit_test_l( ia1, 24))
ia1 = ia1 | 0xFF000000L;
Utc.a1 = ia1*8.881784197e-16;
ia0 = (sf[4][7] << 8) | (sf[4][8] >> 16);
Utc.a0 = ia0*9.31322574615e-10;
itot = (int)((sf[4][8] & 0x00FF00) >> 8);
Utc.tot = itot*4096;
iWNt = (int)(sf[4][8] & 0xFF);
Utc.WNt = iWNt;
idtls = (int)(sf[4][9] >> 16);
if (idtls > 128)
idtls = idtls | 0xFF00;
Utc.dtls = idtls;
iWNlsf = (int)((sf[4][9] & 0x00FF00) >> 8);
Utc.WNlsf = iWNlsf;
iDN = (int)(sf[4][9] & 0x0000FF);
Utc.DN = iDN;
// idtlsf = (int)( sf[4][9] >> 16);
idtlsf = (int)( sf[4][10] >> 16); // fixed (GB-020217)
if (idtlsf > 128)
idtlsf = idtlsf | 0xFF00;
Utc.dtlsf = idtlsf;
}
else if ( i4page == 63 )
{
ASV[1]= (int)((sf[4][3] & 0x00F000) >>12);
ASV[2]= (int)((sf[4][3] & 0x000F00) >>8);
ASV[3]= (int)((sf[4][3] & 0x0000F0) >>4);
ASV[4]= (int)( sf[4][3] & 0x00000F);
ASV[5]= (int)( sf[4][4] >>20);
ASV[6]= (int)((sf[4][4] & 0x0F0000L) >>16);
ASV[7]= (int)((sf[4][4] & 0x00F000) >>12);
ASV[8]= (int)((sf[4][4] & 0x000F00) >> 8);
ASV[9]= (int)((sf[4][4] & 0x0000F0) >> 4);
ASV[10]=(int)(sf[4][4] & 0x00000F);
ASV[11]=(int)(sf[4][5] >>20);
ASV[12]=(int)((sf[4][5] & 0x0F0000L) >>16);
ASV[13]=(int)((sf[4][5] & 0x00F000) >>12);
ASV[14]=(int)((sf[4][5] & 0x000F00) >> 8);
ASV[15]=(int)((sf[4][5] & 0x0000F0) >> 4);
ASV[16]=(int)(sf[4][5] & 0x00000F);
ASV[17]=(int)(sf[4][6] >>20);
ASV[18]=(int)((sf[4][6] & 0x0F0000L) >>16);
ASV[19]=(int)((sf[4][6] & 0x00F000) >>12);
ASV[20]=(int)((sf[4][6] & 0x000F00) >> 8);
ASV[21]=(int)((sf[4][6] & 0x0000F0) >> 4);
ASV[22]=(int)(sf[4][6] & 0x00000F);
ASV[23]=(int)(sf[4][7] >>20);
ASV[24]=(int)((sf[4][7] & 0x0F0000L) >>16);
ASV[25]=(int)((sf[4][7] & 0x00F000) >>12);
ASV[26]=(int)((sf[4][7] & 0x000F00) >> 8);
ASV[27]=(int)((sf[4][7] & 0x0000F0) >> 4);
ASV[28]=(int)( sf[4][7] & 0x00000F);
ASV[29]=(int)( sf[4][8] >>20);
ASV[30]=(int)((sf[4][8] & 0x0F0000L) >>16);
ASV[31]=(int)((sf[4][8] & 0x00F000) >>12);
ASV[32]=(int)((sf[4][8] & 0x000F00) >> 8);
SVh[25]=(int)(sf[4][8] & 0x00003F);
if( SVh[25]==0x3f)
gps_alm[25].inc=0.0;
SVh[26]=(int)(sf[4][9] >>18);
if( SVh[26]==0x3f)
gps_alm[26].inc=0.0;
SVh[27]=(int)((sf[4][9] & 0x03F000L) >>12);
if( SVh[27]==0x3f)
gps_alm[27].inc=0.0;
SVh[28]=(int)((sf[4][9] & 0x000FC0) >>6);
if( SVh[28]==0x3f)
gps_alm[28].inc=0.0;
SVh[29]= (int)(sf[4][9] & 0x00003F);
if( SVh[29]==0x3f)
gps_alm[29].inc=0.0;
SVh[30]= (int)(sf[4][10] >>18);
if( SVh[30]==0x3f)
gps_alm[30].inc=0.0;
SVh[31]=(int)((sf[4][10]& 0x03F000L) >>12);
if( SVh[31]==0x3f)
gps_alm[31].inc=0.0;
SVh[32]=(int)((sf[4][10]& 0x000FC0) >>6);
if( SVh[32]==0x3f)
gps_alm[32].inc=0.0;
}
}
//
// SUBFRAME 5
//
// i5data= sf[5][3] >> 22;
i5p=(int)((sf[5][3] & 0x3F0000L) >> 16);
chan[ch].page5=i5p;
if ( i5page != i5p)
{
i5page=i5p;
if ( i5page == 51 )
{
// iatoa = (int)((sf[5][3] & 0xFF00) >>8);
// atoa = iatoa*4096;
SVh[1] = (int)(sf[5][4] >>18);
if( SVh[1]==0x3f) gps_alm[1].inc=0.0;
SVh[2] = (int)((sf[5][4] & 0x03F000L)>>12);
if( SVh[2]==0x3f) gps_alm[2].inc=0.0;
SVh[3] = (int)((sf[5][4] & 0x000FC0)>>6);
if( SVh[3]==0x3f) gps_alm[3].inc=0.0;
SVh[4] = (int)(sf[5][4] & 0x00003F);
if( SVh[4]==0x3f) gps_alm[4].inc=0.0;
SVh[5] = (int)(sf[5][5] >>18);
if( SVh[5]==0x3f) gps_alm[5].inc=0.0;
SVh[6] = (int)((sf[5][5] & 0x03F000L)>>12);
if( SVh[6]==0x3f) gps_alm[6].inc=0.0;
SVh[7] = (int)((sf[5][5] & 0x000FC0)>>6);
if( SVh[7]==0x3f) gps_alm[7].inc=0.0;
SVh[8] = (int)(sf[5][5] & 0x00003F);
if( SVh[8]==0x3f) gps_alm[8].inc=0.0;
SVh[9] = (int)(sf[5][6] >> 18);
if( SVh[9]==0x3f) gps_alm[9].inc=0.0;
SVh[10] = (int)((sf[5][6] & 0x03F000L)>>12);
if( SVh[10]==0x3f) gps_alm[10].inc=0.0;
SVh[11] = (int)((sf[5][6] & 0x000FC0)>>6);
if( SVh[11]==0x3f) gps_alm[11].inc=0.0;
SVh[12] = (int)(sf[5][6] & 0x00003F);
if( SVh[12]==0x3f) gps_alm[12].inc=0.0;
SVh[13] = (int)(sf[5][7] >>18);
if( SVh[13]==0x3f) gps_alm[13].inc=0.0;
SVh[14] = (int)((sf[5][7] & 0x03F000L)>>12);
if( SVh[14]==0x3f) gps_alm[14].inc=0.0;
SVh[15] = (int)((sf[5][7] & 0x000FC0)>>6);
if( SVh[15]==0x3f) gps_alm[15].inc=0.0;
SVh[16] = (int)(sf[5][7] & 0x00003F);
if( SVh[16]==0x3f) gps_alm[16].inc=0.0;
SVh[17] = (int)(sf[5][8] >>18);
if( SVh[17]==0x3f) gps_alm[17].inc=0.0;
SVh[18] = (int)((sf[5][8] & 0x03F000L)>>12);
if( SVh[18]==0x3f) gps_alm[18].inc=0.0;
SVh[19] = (int)((sf[5][8] & 0x000FC0)>>6);
if( SVh[19]==0x3f) gps_alm[19].inc=0.0;
SVh[20] = (int)(sf[5][8] & 0x00003F);
if( SVh[20]==0x3f) gps_alm[20].inc=0.0;
SVh[21] = (int)(sf[5][9] >>18);
if( SVh[21]==0x3f) gps_alm[21].inc=0.0;
SVh[22] = (int)((sf[5][9] & 0x03F000L)>>12);
if( SVh[22]==0x3f) gps_alm[22].inc=0.0;
SVh[23] = (int)((sf[5][9] & 0x000FC0)>>6);
if( SVh[23]==0x3f) gps_alm[23].inc=0.0;
SVh[24] = (int)(sf[5][9] & 0x00003F);
if( SVh[24]==0x3f) gps_alm[24].inc=0.0;
}
else
{
isv=i5page;
gps_alm[isv].week=gps_week;
iae=(int)(sf[5][3] & 0xFFFF);
gps_alm[isv].ety=iae*4.768371582E-7;
iatoa=(int)(sf[5][4] >> 16);
gps_alm[isv].toa=iatoa*4096.0;
iadeli=(int)(sf[5][4] & 0xFFFF);
gps_alm[isv].inc=(iadeli*1.907348633E-6+0.3)*pi;
iaomegad=(int)(sf[5][5] >> 8);
gps_alm[isv].omegadot=iaomegad*3.637978807E-12*pi;
gps_alm[isv].health=(int)(sf[5][5] & 0xFF);
iasqr=sf[5][6];
gps_alm[isv].sqra=iasqr*4.8828125E-4;
if (gps_alm[isv].sqra>0.0) gps_alm[isv].w=19964981.84/pow(gps_alm[isv].sqra,3);
iaomega0=sf[5][7];
if (bit_test_l(iaomega0,24)) iaomega0=iaomega0 | 0xFF000000L;
gps_alm[isv].omega0=iaomega0*1.192092896E-7*pi;
iaw=sf[5][8];
if (bit_test_l(iaw,24)) iaw=iaw | 0xFF000000L;
gps_alm[isv].w =iaw*1.192092896E-7*pi;
iam0 = sf[5][9];
if ( bit_test_l( iam0, 24))
iam0 = iam0 | 0xFF000000L;
gps_alm[isv].ma = iam0*1.192092896E-7*pi;
iaaf0 = (int)((sf[5][10] >> 13) | (sf[5][10] & 0x1C));
if ( bit_test_l(iaaf0,11))
iaaf0 = iaaf0 | 0xF800;
gps_alm[isv].af0 = iaaf0*9.536743164E-7;
iaaf1 = (int)((sf[5][10] & 0xFFE0) >> 5);
if ( bit_test_l( iaaf1, 11))
iaaf1 = iaaf1 | 0xF800;
gps_alm[isv].af1 = iaaf1*3.637978807E-12;
}
}
}
}
return;
}
/*******************************************************************************
FUNCTION parity_check()
RETURNS None.
PARAMETERS None.
PURPOSE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void parity_check(void)
{
long pb1=0x3b1f3480L, pb2=0x1d8f9a40L, pb3=0x2ec7cd00L,
pb4=0x1763e680L, pb5=0x2bb1f340L, pb6=0x0b7a89c0L;
int parity, m_parity;
char d29=0, d30=0,
sfm, word, b_1, b_2, b_3, b_4, b_5, b_6;
int err_bit;
for ( sfm = 1; sfm <= 5; sfm++)
{
p_error[sfm]=0;
for ( word=1; word<=10; word++)
{
m_parity = (int)( sf[sfm][word] & 0x3f);
b_1 = exor( d29, sf[sfm][word] & pb1) << 5;
b_2 = exor( d30, sf[sfm][word] & pb2) << 4;
b_3 = exor( d29, sf[sfm][word] & pb3) << 3;
b_4 = exor( d30, sf[sfm][word] & pb4) << 2;
b_5 = exor( 0, sf[sfm][word] & pb5) << 1;
b_6 = exor( d29^d30, sf[sfm][word] & pb6);
parity = b_1+b_2+b_3+b_4+b_5+b_6;
err_bit = 0;
if (parity != m_parity)
{
err_bit=1;
}
p_error[sfm]=(p_error[sfm] << 1) + err_bit;
if ( d30 == 1)
sf[sfm][word] = 0x03fffffc0L & ~sf[sfm][word];
sf[sfm][word] = sf[sfm][word]>>6; // remove 6 parity bits
d29 = (m_parity & 0x2) >> 1;
d30 = m_parity & 0x1;
}
}
}
/*******************************************************************************
FUNCTION exor()
RETURNS None.
PARAMETERS None.
PURPOSE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
int exor(char bit, long parity)
{
char i;
int result;
result=0;
for (i=7;i<=30;i++)
{
if (bit_test_l(parity,i)) result++;
}
result = result%2;
result = (bit^result) & 0x1;
return result;
}
/*******************************************************************************
FUNCTION ecef_to_llh()
RETURNS None.
PARAMETERS None.
PURPOSE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
LLH ecef_to_llh( ECEF pos)
{
double p,n,thet,esq,epsq;
LLH result;
p = sqrt(pos.x*pos.x+pos.y*pos.y);
thet = atan(pos.z*a/(p*b));
esq = 1.0-b*b/(a*a);
epsq = a*a/(b*b)-1.0;
result.lat = atan((pos.z+epsq*b*pow(sin(thet),3))/(p-esq*a*pow(cos(thet),3)));
result.lon = atan2(pos.y,pos.x);
n = a*a/sqrt(a*a*cos(result.lat)*cos(result.lat) +
b*b*sin(result.lat)*sin(result.lat));
result.hae = p/cos(result.lat)-n;
return result;
}
/*******************************************************************************
FUNCTION llh_to_ecef()
RETURNS None.
PARAMETERS None.
PURPOSE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
ECEF llh_to_ecef(LLH pos)
{
double n;
ECEF result;
n = a*a/sqrt(a*a*cos(pos.lat)*cos(pos.lat) +
b*b*sin(pos.lat)*sin(pos.lat));
result.x = (n+pos.hae)*cos(pos.lat)*cos(pos.lon);
result.y = (n+pos.hae)*cos(pos.lat)*sin(pos.lon);
result.z = (b*b/(a*a)*n+pos.hae)*sin(pos.lat);
return result;
}
/*******************************************************************************
FUNCTION pos_vel_time(int)
RETURNS None.
PARAMETERS None.
PURPOSE
This routine processes the all-in-view pseudorange to arrive
at a receiver position
INPUTS:
pseudo_range[nsl] Vector of measured range from satellites to the receiver
sat_location[nsl][3] Array of satellite locations in ECEF when the signal was sent
nsl number of satellites used
OUTPUTS:
RP[3] VECTOR OF RECEIVER POSITION IN ECEF (X,Y,Z)
CBIAS RECEIVER CLOCK BIAS FROM GPS TIME
VARIABLES USED:
C SPEED OF LIGHT IN VACUUM IN M/S
S[6][5] MATRIX USED FOR SOLVING FOR RECEIVER POSITION CORRECTION
B[5] RESULTING RECEIVER CLOCK BIAS & POSITION CORRECTIONS
X,Y,Z TEMPORARY RECEIVER POSITION
T TEMPORARY RECEIVER CLOCK BIAS
R[5] RANGE FROM RECEIVER TO SATELLITES
IF THE POSITION CANNOT BE DETERMINED THE RESULT OF RP
WILL BE (0,0,0) THE CENTER OF THE EARTH
WRITTEN BY
Clifford Kelley
*******************************************************************************/
PVT pos_vel_time( int nsl)
{
double dd[5][5],r,ms[5][13],pm[5][13],bm[13],br[5],correct_mag,x,y,z,t;
double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom,alpha;
int i,j,k,nits;
PVT result;
//
// USE ITERATIVE APPROACH TO SOLVING FOR THE POSITION OF
// THE RECEIVER
//
nits = 0;
t = 0.0;
x = rec_pos_xyz.x;
y = rec_pos_xyz.y;
z = rec_pos_xyz.z;
do
{
for (i=1;i<=nsl;i++)
{
//
// Compute range in ECI at the time of arrival at the receiver
//
alpha=(dt[i]-t)*omegae;
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x,2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y,2)+
pow(track_sat[i].z-z,2));
bm[i]=r-(dt[i]-t)*c;
ms[1][i]=(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x)/r;
ms[2][i]=(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y)/r;
ms[3][i]=(track_sat[i].z-z)/r;
ms[4][i]=1.0;
}
a1=0.;b1=0.;c1=0.;d1=0.;
// e1=0.;
f1=0.;g1=0.;h1=0.;
// i1=0.;j1=0.;
k1=0.;l1=0.;
// m1=0.;n1=0.;o1=0.;p1=0.;
// m1=0.;n1=0.;
p1=0.;
for (k=1;k<=nsl;k++)
{
a1+=ms[1][k]*ms[1][k];
b1+=ms[1][k]*ms[2][k];
c1+=ms[1][k]*ms[3][k];
d1+=ms[1][k]*ms[4][k];
// e1+=ms[2][k]*ms[1][k];
f1+=ms[2][k]*ms[2][k];
g1+=ms[2][k]*ms[3][k];
h1+=ms[2][k]*ms[4][k];
// i1+=ms[3][k]*ms[1][k];
// j1+=ms[3][k]*ms[2][k];
k1+=ms[3][k]*ms[3][k];
l1+=ms[3][k]*ms[4][k];
// m1+=ms[1][k];
// n1+=ms[2][k];
// o1+=ms[3][k];
p1+=ms[4][k];
}
o1=l1; m1=d1; n1=h1; e1=b1; i1=c1; j1=g1;
/*
SOLVE FOR 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);
dd[1][1] = f1*(k1*p1-l1*o1)+g1*(l1*n1-j1*p1)+h1*(j1*o1-k1*n1);
dd[1][2] = e1*(l1*o1-k1*p1)+g1*(i1*p1-l1*m1)+h1*(k1*m1-i1*o1);
dd[1][3] = e1*(j1*p1-n1*l1)-i1*(f1*p1-n1*h1)+m1*(f1*l1-j1*h1);
dd[1][4] = e1*(n1*k1-j1*o1)+i1*(f1*o1-n1*g1)+m1*(j1*g1-f1*k1);
dd[2][1] = b1*(l1*o1-k1*p1)+j1*(c1*p1-d1*o1)+n1*(d1*k1-c1*l1);
dd[2][2] = a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1);
dd[2][3] = a1*(l1*n1-j1*p1)+i1*(b1*p1-n1*d1)+m1*(j1*d1-b1*l1);
dd[2][4] = a1*(j1*o1-n1*k1)-i1*(b1*o1-n1*c1)+m1*(b1*k1-c1*j1);
dd[3][1] = b1*(g1*p1-h1*o1)-f1*(c1*p1-o1*d1)+n1*(c1*h1-d1*g1);
dd[3][2] = a1*(o1*h1-g1*p1)+e1*(c1*p1-o1*d1)+m1*(d1*g1-c1*h1);
dd[3][3] = a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1);
dd[3][4] = a1*(n1*g1-f1*o1)+e1*(b1*o1-c1*n1)+m1*(c1*f1-b1*g1);
dd[4][1] = b1*(h1*k1-g1*l1)+f1*(c1*l1-d1*k1)+j1*(d1*g1-c1*h1);
dd[4][2] = a1*(g1*l1-h1*k1)-e1*(c1*l1-d1*k1)+i1*(c1*h1-d1*g1);
dd[4][3] = a1*(j1*h1-f1*l1)+e1*(b1*l1-d1*j1)+i1*(d1*f1-b1*h1);
dd[4][4] = a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1);
if ( denom <= 0.0 )
{
result.x=0.0; // something went wrong
result.y=0.0; // set solution to center of earth
result.z=0.0;
result.dt=0.0;
}
else
{
for (i=1; i<=4; i++)
{
for ( j=1; j<=4; j++)
dd[i][j] = dd[i][j]/denom;
}
for (i=1; i<=nsl; i++)
{
for (j=1; j<=4; j++)
{
pm[j][i] = 0.0;
for ( k=1; k<=4; k++)
pm[j][i] += dd[j][k]*ms[k][i];
}
}
for (i=1;i<=4;i++)
{
br[i]=0.0;
for ( j=1; j<=nsl; j++)
br[i] += bm[j]*pm[i][j];
}
nits++;
x = x+br[1];
y = y+br[2];
z = z+br[3];
t = t-br[4]/c;
// printf("%lf,%lf,%lf,%20.10lf\n",x,y,z,t);
// printf("%lf,%lf,%lf,%lf\n",br[1],br[2],br[3],br[4]);
correct_mag=sqrt(br[1]*br[1]+br[2]*br[2]+br[3]*br[3]);
}
} while ( correct_mag > 0.01 && correct_mag < 1.e8 && nits < 10);
result.x = x;
result.y = y;
result.z = z;
result.dt = t;
//
// Now for Velocity
//
for (i=1;i<=nsl;i++)
{
alpha = (dt[i]-t)*omegae;
r = sqrt( pow( track_sat[i].x * cos(alpha) - track_sat[i].y * sin(alpha)-x, 2) +
pow( track_sat[i].y * cos(alpha) + track_sat[i].x * sin(alpha)-y, 2) +
pow( track_sat[i].z-z, 2));
bm[i] = ((track_sat[i].x * cos(alpha) - track_sat[i].y * sin(alpha)-x) * d_sat[i].x +
(track_sat[i].y * cos(alpha) + track_sat[i].x * sin(alpha)-y) * d_sat[i].y +
(track_sat[i].z-z) * d_sat[i].z)/r - meas_dop[i] * lambda;
// fprintf(out," %d meas_dop= %f bm= %f\n",i,meas_dop[i],bm[i]);
}
for (i=1; i<=4; i++)
{
br[i]=0.0;
for (j=1;j<=nsl;j++)
br[i] += bm[j]*pm[i][j];
}
// fprintf(out," %f %f %f %f\n",pm[1][1],pm[1][2],pm[1][3],pm[1][4]);
// fprintf(out," %f %f %f %f\n",pm[2][1],pm[2][2],pm[2][3],pm[2][4]);
// fprintf(out," %f %f %f %f\n",pm[3][1],pm[3][2],pm[3][3],pm[3][4]);
// fprintf(out," %f %f %f %f\n",pm[4][1],pm[4][2],pm[4][3],pm[4][4]);
result.xv = br[1] + y*omegae; // get rid of earth
result.yv = br[2] - x*omegae; // rotation rate
result.zv = br[3];
// fprintf(out,"vel x=%f,y=%f,z=%f\n",result.xv,result.yv,result.zv);
result.df = br[4] / c * 1000000.0;
return (result);
}
/*******************************************************************************
FUNCTION dops( int nsl)
RETURNS None.
PARAMETERS None.
PURPOSE
This routine computes the dops
INPUTS:
sat_location[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
*******************************************************************************/
void dops( int nsl)
{
double ddx,ddy,ddz,ddt,r,rxy,ms[5][13],x,y,z;
double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom;
double msx,msy,msz;
int i,k;
ECEF north,east,up;
x = rec_pos_xyz.x;
y = rec_pos_xyz.y;
z = rec_pos_xyz.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;
for (i=1;i<=nsl;i++)
{
//
// Compute line of sight vectors
//
r=sqrt(pow(track_sat[i].x-x,2)+
pow(track_sat[i].y-y,2)+
pow(track_sat[i].z-z,2));
ms[1][i]=(track_sat[i].x-x)/r;
ms[2][i]=(track_sat[i].y-y)/r;
ms[3][i]=(track_sat[i].z-z)/r;
ms[4][i]=1.0;
}
for (i=1;i<=nsl;i++)
{
msx=ms[1][i]*north.x+ms[2][i]*north.y+ms[3][i]*north.z;
msy=ms[1][i]*east.x+ms[2][i]*east.y;
msz=ms[1][i]*up.x+ms[2][i]*up.y+ms[3][i]*up.z;
ms[1][i]=msx;ms[2][i]=msy;ms[3][i]=msz;
ms[4][i]=1.0;
}
a1=0.;b1=0.;c1=0.;d1=0.;
// e1=0.;
f1=0.;g1=0.;h1=0.;
// i1=0.;j1=0.;
k1=0.;l1=0.;
// m1=0.;
// n1=0.;
// o1=0.;
p1=0.;
for (k=1; k<=nsl; k++)
{
a1+=ms[1][k]*ms[1][k];
b1+=ms[1][k]*ms[2][k];
c1+=ms[1][k]*ms[3][k];
d1+=ms[1][k]*ms[4][k];
// e1+=ms[2][k]*ms[1][k];
f1+=ms[2][k]*ms[2][k];
g1+=ms[2][k]*ms[3][k];
h1+=ms[2][k]*ms[4][k];
// i1+=ms[3][k]*ms[1][k];
// j1+=ms[3][k]*ms[2][k];
k1+=ms[3][k]*ms[3][k];
l1+=ms[3][k]*ms[4][k];
// m1+=ms[1][k];
// n1+=ms[2][k];
// o1+=ms[3][k];
p1+=ms[4][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=1.e6; // something went wrong
vdop=1.e6; // set dops to a large number
tdop=1.e6;
pdop=1.e6;
gdop=1.e6;
}
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(void);
}
/*******************************************************************************
FUNCTION tropo_iono()
RETURNS None.
PARAMETERS None.
PURPOSE
This function corrects the pseudoranges with a tropospheric model
and the broadcast ionospheric message corrections.
WRITTEN BY
Clifford Kelley
*******************************************************************************/
double tropo_iono( float az, float el, double gps_time)
{
double d_Trop,d_Ion,psi,phi,lambdai,phim,per,x,F,amp,t;
// Try a troposphere model
// if (current_loc.hae>7010.) d_Trop=(4.75/sin(el+.01745))/c;
d_Trop = 2.47/(sin(el)+.0121)*exp(-current_loc.hae*1.33e-4)/c;
// If available from the nav message use its Ionosphere model
if ( Iono.b0 != 0.0 && Iono.al0 != 0.0)
{
psi = 0.0137/(el/pi+0.11)-.022;
phi = current_loc.lat/pi+psi*cos(az);
if ( phi > 0.416)
phi = 0.416;
else if (phi < -0.416 )
phi = -0.416;
lambdai = current_loc.lon/pi+psi*sin(az)/cos(phi*pi);
t = (int)(43200.*lambdai+gps_time)%86400L;
if ( t < 0.0)
t = t + 86400.0;
phim = phi+0.064*cos((lambdai-1.617)*pi);
per = Iono.b0 +
Iono.b1*phim +
Iono.b2*phim*phim +
Iono.b3*phim*phim*phim;
if ( per <72000.0 )
per = 72000.0;
x = 2.*pi*(t-50400.)/per;
F = 1.0+16.0*pow(0.53-el/pi,3);
amp = Iono.al0 +
Iono.al1 * phim +
Iono.al2 * phim*phim +
Iono.al3 * phim*phim*phim;
if ( amp < 0.0 )
amp = 0.0;
if (fabs(x) < 1.5707)
d_Ion=F*(5.0e-9+amp*(1.0-x*x/2.+x*x*x*x/24.0));
else
d_Ion=F*5.0e-9;
}
// else try this
else
{
d_Ion=16.33/sin(sqrt(el*el+0.1255))/c;
}
// fprintf(stream,"d_trop=%le,d_ion=%le,az=%f,el=%f\n",d_Trop,d_Ion,az,el);
return (d_Trop + d_Ion);
}
/*******************************************************************************
FUNCTION read_ion_utc()
RETURNS None.
PARAMETERS None.
PURPOSE
This function
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void read_ion_utc( void)
{
char infile[] = "ion_utc.dat";
char *tmpstr;
FILE *in;
tmpstr = conmalloc( strlen( OGSDataDir) + strlen( infile) + 1);
strcpy( tmpstr, OGSDataDir);
strcat( tmpstr, infile);
if (( in = fopen( tmpstr, "rt")) == NULL)
{
printf( "error opening %s.\n", tmpstr);
exit(-1);
}
else
{
while ( !feof( in)) // GB: replaced eof() by feof()
{
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.al0);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.al1);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.al2);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.al3);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.b0);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.b1);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.b2);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Iono.b3);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.a0);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.a1);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.dtls);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.tot);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.WNt);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.WNlsf);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.DN);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &Utc.dtlsf);
}
}
fclose( in);
if ( tmpstr)
free( tmpstr);
return;
}
/*******************************************************************************
FUNCTION read_almanac()
RETURNS None.
PARAMETERS None.
PURPOSE
This function
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void read_almanac( void)
{
int id, health, week;
float eccen, rinc, rras, sqra;
float ratoa, aopg, rma, af0, af1, toa;
char infile[] = "current.alm";
char *buf;
FILE *in;
buf = conmalloc( strlen( OGSDataDir) + strlen( infile) + 1);
strcpy( buf, OGSDataDir);
strcat( buf, infile);
if ((in = fopen( buf, "rt")) == NULL)
{
printf( "error opening '%s'.\n", buf);
for ( id=1; id<=32; id++)
{
gps_alm[id].week = gps_week-1;
gps_alm[id].inc = 1.0;
}
}
else
{
status = warm_start;
while ( !feof( in)) // GB: replaced eof() by feof()
{
fscanf( in, "%45c", &header);
fscanf( in, "%27c", &text);
fscanf( in, "%d", &id);
fscanf( in, "%27c", &text);
fscanf( in, "%i", &health);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &eccen);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &toa);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &rinc);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &rras);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &sqra);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &ratoa);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &aopg);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &rma);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &af0);
fscanf( in, "%27c", &text);
fscanf( in, "%f", &af1);
fscanf( in, "%27c", &text);
fscanf( in, "%i", &week);
fscanf( in, "%c", &trailer);
gps_alm[id].health = health;
gps_alm[id].week = week;
gps_alm[id].toa = toa;
gps_alm[id].ety = eccen;
gps_alm[id].toa = toa;
gps_alm[id].inc = rinc;
gps_alm[id].omegadot = rras;
gps_alm[id].sqra = sqra;
gps_alm[id].omega0 = ratoa;
gps_alm[id].w = aopg;
gps_alm[id].ma = rma;
gps_alm[id].af0 = af0;
gps_alm[id].af1 = af1;
gps_alm[id].sat_file = 0;
if (gps_alm[id].sqra > 0.0)
gps_alm[id].w = 19964981.84/pow(gps_alm[id].sqra,3);
}
fclose( in);
alm_gps_week = week;
alm_toa = toa;
}
if ( buf)
free( buf);
return;
}
/*******************************************************************************
FUNCTION read_ephemeris()
RETURNS None.
PARAMETERS None.
PURPOSE
This function
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void read_ephemeris( void)
{
int id,health,week;
double toc,toe;
double crc, crs, cic, cis, cuc, cus, tgd, ety, inc0, omegadot, w0, w, ma;
double daf0, daf1, daf2, esqra;
float d_toe;
char infile[] = "current.eph";
char *buf;
FILE *in;
buf = conmalloc( strlen( OGSDataDir) + strlen( infile) + 1);
strcpy( buf, OGSDataDir);
strcat( buf, infile);
if (( in = fopen( buf, "rt")) == NULL)
{
printf( "error opening '%s'.\n", buf);
}
else
{
while ( !feof( in)) // GB: replaced eof() by feof()
{
fscanf( in, "%37c", &header);
fscanf( in, "%27c", &text);
fscanf( in, "%i", &id);
fscanf( in, "%27c", &text);
fscanf( in, "%i", &health);
fscanf( in, "%27c", &text);
fscanf( in, "%i", &week);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &toe);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &toc);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &tgd);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &daf0);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &daf1);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &daf2);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &ety);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &inc0);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &omegadot);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &esqra);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &w0);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &w);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &ma);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &cuc);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &cus);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &crc);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &crs);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &cic);
fscanf( in, "%27c", &text);
fscanf( in, "%le", &cis);
fscanf( in, "%c", &trailer);
d_toe = clock_tow-toe;
if ( d_toe > 302400.0)
d_toe = d_toe-604800.0;
else if (d_toe < -302400.0)
d_toe = d_toe + 604800.0;
//
// If week is current and time is less than 2 hours old use for hot start
// note: gps_week is computed from the PCs real time clock
// and does not roll over at 1024
//
if ( week == (gps_week % 1024) && fabs( d_toe) < 7200.0)
gps_eph[id].valid=1;
gps_eph[id].health = health;
gps_eph[id].week = week;
gps_eph[id].toe = toe;
gps_eph[id].toc = toc;
gps_eph[id].tgd = tgd;
gps_eph[id].af0 = daf0;
gps_eph[id].af1 = daf1;
gps_eph[id].af2 = daf2;
gps_eph[id].ety = ety;
gps_eph[id].inc0 = inc0;
gps_eph[id].omegadot = omegadot;
gps_eph[id].sqra = esqra;
gps_eph[id].omega0 = w0;
gps_eph[id].w = w;
gps_eph[id].ma = ma;
gps_eph[id].cuc = cuc;
gps_eph[id].cus = cus;
gps_eph[id].crc = crc;
gps_eph[id].crs = crs;
gps_eph[id].cic = cic;
gps_eph[id].cis = cis;
if (gps_eph[id].sqra > 0.0)
gps_eph[id].wm = 19964981.84 / pow( gps_eph[id].sqra, 3);
}
fclose( in);
}
if ( buf)
free( buf);
return;
}
/*******************************************************************************
FUNCTION write_almanac()
RETURNS None.
PARAMETERS None.
PURPOSE
This function writes the broadcast almanac data to a file for later
use. In particular to support a warm or hot start.
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void write_almanac( void)
{
int i;
out = fopen( "current.alm", "w+");
if ( !out)
{
printf( "write_almanac: error opening file 'current.alm'.\n");
exit(-1);
}
for (i=1;i<=32;i++)
{
if ( gps_alm[i].inc > 0.0)
{
//******** Week 131 almanac for PRN-01 ********
//ID: 01
//Health: 000
//Eccentricity: 0.5362033844E-002
//Time of Applicability(s): 589824.0000
//Orbital Inclination(rad): 0.9676266920
//Rate of Right Ascen(r/s): -0.8126052768E-008
//SQRT(A) (m 1/2): 5153.620605
//Right Ascen at Week(rad): -0.1178026838E+001
//Argument of Perigee(rad): -1.701721362
//Mean Anom(rad): 0.2097295909E+001
//Af0(s): 0.2183914185E-003
//Af1(s/s): 0.3637978807E-011
//week: 131
fprintf( out, "******** Week %4d almanac for PRN-%02d ********\n",
gps_alm[i].week % 1024, i);
fprintf( out, "ID: %02d\n", i);
fprintf( out, "Health: %03d\n", gps_alm[i].health);
fprintf( out, "Eccentricity: %10.9e\n", gps_alm[i].ety);
fprintf( out, "Time of Applicability(s): %10.9e\n", gps_alm[i].toa);
fprintf( out, "Orbital Inclination(rad): %10.9e\n", gps_alm[i].inc);
fprintf( out, "Rate of Right Ascen(R/s): %10.9e\n", gps_alm[i].omegadot);
fprintf( out, "SQRT(A) (m^1/2): %10.9e\n", gps_alm[i].sqra);
fprintf( out, "Right Ascen at TOA(rad): %10.9e\n", gps_alm[i].omega0);
fprintf( out, "Argument of Perigee(rad): %10.9e\n", gps_alm[i].w);
fprintf( out, "Mean Anom(rad): %10.9e\n", gps_alm[i].ma);
fprintf( out, "Af0(s): %10.9e\n", gps_alm[i].af0);
fprintf( out, "Af1(s/s): %10.9e\n", gps_alm[i].af1);
fprintf( out, "week: %4d \n", gps_alm[i].week % 1024);
fprintf( out, "\n");
}
}
fclose(out);
}
/*******************************************************************************
FUNCTION write_ephemeris()
RETURNS None.
PARAMETERS None.
PURPOSE
This function writes the broadcast ephemeris data to a file for later
use. In particular to support a hot start.
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void write_ephemeris()
{
int i;
out = fopen( "current.eph", "w+");
if ( !out)
{
printf( "write_ephemeris: error opening file 'current.eph'\n");
exit(-1);
}
for ( i=1; i<=32; i++)
{
if ( gps_eph[i].inc0 >0.0)
{
fprintf( out, "**** Ephemeris for PRN-%02d ***********\n", i);
fprintf( out, "ID: %3d\n", i);
fprintf( out, "Health: %3d\n", gps_eph[i].health);
fprintf( out, "Week: %4d\n", gps_eph[i].week);
fprintf( out, "E Time of Applic(s): %10.9e\n", gps_eph[i].toe);
fprintf( out, "C Time of Applic(s): %10.9e\n", gps_eph[i].toc);
fprintf( out, "Tgd(s): %10.9e\n", gps_eph[i].tgd);
fprintf( out, "Af0(s): %10.9e\n", gps_eph[i].af0);
fprintf( out, "Af1(s/s): %10.9e\n", gps_eph[i].af1);
fprintf( out, "Af2(s/s/s): %10.9e\n", gps_eph[i].af2);
fprintf( out, "Eccentricity: %10.9e\n", gps_eph[i].ety);
fprintf( out, "Orbital Inclination(rad): %10.9e\n", gps_eph[i].inc0);
fprintf( out, "Rate of Right Ascen(R/s): %10.9e\n", gps_eph[i].omegadot);
fprintf( out, "SQRT(A) (m^1/2): %10.9e\n", gps_eph[i].sqra);
fprintf( out, "Right Ascen at TOE(rad): %10.9e\n", gps_eph[i].omega0);
fprintf( out, "Argument of Perigee(rad): %10.9e\n", gps_eph[i].w);
fprintf( out, "Mean Anom(rad): %10.9e\n", gps_eph[i].ma);
fprintf( out, "Cuc(rad): %10.9e\n", gps_eph[i].cuc);
fprintf( out, "Cus(rad): %10.9e\n", gps_eph[i].cus);
fprintf( out, "Crc(m): %10.9e\n", gps_eph[i].crc);
fprintf( out, "Crs(m): %10.9e\n", gps_eph[i].crs);
fprintf( out, "Cic(rad): %10.9e\n", gps_eph[i].cic);
fprintf( out, "Cis(rad): %10.9e\n", gps_eph[i].cis);
fprintf( out, "\n");
}
}
fclose(out);
}
/*******************************************************************************
FUNCTION write_ion_utc()
RETURNS None.
PARAMETERS None.
PURPOSE
This function writes the broadcast ionospheric correction data and the
parameters to tie GPS time to UTC to a file for later use.
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void write_ion_utc()
{
FILE *out;
out = fopen( "ion_utc.dat", "w+");
if ( !out)
{
printf( "write_ion_utc: error opening file 'ion_utc.dat'\n");
exit(-1);
}
fprintf( out, "al0: %e\n", Iono.al0);
fprintf( out, "al1: %e\n", Iono.al1);
fprintf( out, "al2: %e\n", Iono.al2);
fprintf( out, "al3: %e\n", Iono.al3);
fprintf( out, "b0: %f\n", Iono.b0);
fprintf( out, "b1: %f\n", Iono.b1);
fprintf( out, "b2: %f\n", Iono.b2);
fprintf( out, "b3: %f\n", Iono.b3);
fprintf( out, "a0: %f\n", Utc.a0);
fprintf( out, "a1: %f\n", Utc.a1);
fprintf( out, "dtls: %f\n", Utc.dtls);
fprintf( out, "tot: %f\n", Utc.tot);
fprintf( out, "WNt: %f\n", Utc.WNt);
fprintf( out, "WNlsf: %f\n", Utc.WNlsf);
fprintf( out, "DN: %f\n", Utc.DN);
fprintf( out, "dtlsf: %f\n", Utc.dtlsf);
fclose( out);
return;
}
/*******************************************************************************
FUNCTION fix_sqrt( long x)
RETURNS long integer
PARAMETERS
x long integer
PURPOSE
This function finds the fixed point square root of a long integer
WRITTEN BY
Clifford Kelley
*******************************************************************************/
long fix_sqrt( long x)
{
long xt, scr;
int i;
// --- take care of sqrt(0) --- (G.B. 02.04.08)
if ( !x)
return (0);
i = 0;
xt = x;
do
{
xt = xt >> 1;
i++;
} while( xt>0);
i = (i >> 1) + 1;
xt = x >> i;
do
{
scr = xt * xt;
scr = x - scr;
scr = scr >> 1;
scr = scr / xt;
xt = scr + xt;
} while( scr!=0);
xt = xt << 7;
return (xt);
}
/* ========================= End of File ========================= */