spacecruft
/
strf
1
0
Fork 0
Code Releases Activity

SGDP4 routines

pull/10/head
Cees Bassa 2014-03-23 16:50:23 +01:00
parent 727f062c89
commit 1b775aca7c
8 changed files with 2629 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

924
deep.c 100644
View File

@ -0,0 +1,924 @@
/* > deep.c
*
* 1.00 around 1980 - Felix R. Hoots & Ronald L. Roehrich, from original
* DEEP.FOR used in the SGP deep-space models SDP4
* and SDP8.
*
************************************************************************
*
* Made famous by the spacetrack report No.3:
* "Models for Propogation of NORAD Element Sets"
* Edited and subsequently distributed by Dr. T. S. Kelso.
*
************************************************************************
*
* This conversion by:
* Paul S. Crawford and Andrew R. Brooks
* Dundee University
*
* NOTE !
* This code is supplied "as is" and without warranty of any sort.
*
* (c) 1994-2004, Paul Crawford, Andrew Brooks
*
************************************************************************
*
* 2.00 psc Mon Dec 19 1994 - Translated from FORTRAN into 'C' (of sorts).
*
* 2.01 psc Wed Dec 21 1994 - Re-write of the secular integrator from a
* messy FORTRAN block in to something which
* (hopefully!) is understandable.
*
* 2.02 psc Thu Dec 22 1994 - Final mods and tested against the FORTRAN
* version (using ~12 hour resonant and
* geostationary (~24 hour) elements).
*
* 2.03 psc Mon Jan 02 1995 - Some additional refinements and error-traps.
*
* 3.00 psc Mon May 29 1995 - Cleaned up for general use & distrabution (to
* remove Dundee specific features).
*
* 3.01 psc Mon Jan 12 2004 - Final fix agreed for "Lyddane bug".
* 3.02 psc Mon Jul 03 2006 - Extended range "Lyddane bug" fix.
* 3.03 psc Tue Jul 04 2006 - Bug fix for extended range "Lyddane bug" fix.
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
static const char SCCSid[] = "@(#)deep.c 3.03 (C) 1995 psc SatLib: Deep Space effects";
#ifndef NO_DEEP_SPACE
#include "sgdp4h.h"
extern long Isat;
int Set_LS_zero = 0; /* Set to 1 to zero Lunar-Solar terms at epoch. */
/* ======================= Function prototypes ====================== */
static void dot_terms_calculated(void);
static void compute_LunarSolar(double tsince);
static void thetag(double ep, real *thegr, double *days50);
/* ===================== Strange constants, etc ===================== */
#define ZNS ((real)1.19459e-5)
#define C1SS ((real)2.9864797e-6)
#define ZES ((real)0.01675)
#define ZNL ((real)1.5835218e-4)
#define C1L ((real)4.7968065e-7)
#define ZEL ((real)0.0549)
#define ZCOSIS ((real)0.91744867)
#define ZSINIS ((real)0.39785416)
#define ZCOSGS ((real)0.1945905)
#define ZSINGS ((real)-0.98088458)
#define Q22 ((real)1.7891679e-6)
#define Q31 ((real)2.1460748e-6)
#define Q33 ((real)2.2123015e-7)
#define G22 ((real)5.7686396)
#define G32 ((real)0.95240898)
#define G44 ((real)1.8014998)
#define G52 ((real)1.0508330)
#define G54 ((real)4.4108898)
#define ROOT22 ((real)1.7891679e-6)
#define ROOT32 ((real)3.7393792e-7)
#define ROOT44 ((real)7.3636953e-9)
#define ROOT52 ((real)1.1428639e-7)
#define ROOT54 ((real)2.1765803e-9)
#define THDT ((real)4.37526908801129966e-3)
//#define THDT ((real)0.0043752691)
#define STEP 720.0
#define MAX_INTEGRATE (STEP * 10000)
#define SIN_EPS (real)(1.0e-12)
/* ======= Global variables used by dpsec(), from dpinit(). ======== */
static real eo; /* copy of original eccentricity. */
static real xincl; /* copy of original equatorial inclination. */
static int isynfl=0, iresfl=0;
static double atime, xli, xni, xnq, xfact;
static real ssl, ssg, ssh, sse, ssi;
static real xlamo, omegaq, omgdt, thgr;
static real del1, del2, del3, fasx2, fasx4, fasx6;
static real d2201, d2211, d3210, d3222, d4410, d4422;
static real d5220, d5232, d5421, d5433;
static real xnddt, xndot, xldot; /* Integrator terms. */
static real xnddt0, xndot0, xldot0; /* Integrator at epoch. */
/* ======== Global Variables used by dpper(), from dpinit(). ======= */
static int ilsd=0, ilsz=0;
static real zmos, se2, se3, si2, si3, sl2, sl3, sl4;
static real sgh2, sgh3, sgh4, sh2, sh3;
static real zmol, ee2, e3 ,xi2, xi3, xl2, xl3, xl4;
static real xgh2, xgh3, xgh4, xh2, xh3;
static real pe, pinc, pgh, ph, pl;
static real pgh0, ph0, pe0, pinc0, pl0; /* Added terms to save the epoch values of perturbations. */
/* ==================================================================
----------------- DEEP SPACE INITIALIZATION ----------------------
epoch : Input, epoch time as YYDDD.DDDD as read from 2-line elements.
omegao : Input, argument of perigee from elements, radian.
xnodeo : Input, right asc. for ascn node from elements, radian.
xmo : Input, mean anomaly from elements, radian.
orb_eo : Input, eccentricity from elements, dimentionless.
orb_xincl : Input, equatorial inclination from elements, radian.
aodp : Input, original semi-major axis, earth radii.
xlldot : Input, 1st derivative of "mean anomaly" (xmdot), radian/min.
omgdot : Input, 1st derivative of arg. per., radian/min.
xnodot : Input, 1st derivative of right asc., radian/min.
xnodp : Input, original mean motion, radian/min.
================================================================== */
int SGDP4_dpinit(double epoch, real omegao, real xnodeo, real xmo,
real orb_eo, real orb_xincl, real aodp, double xlldot,
real omgdot, real xnodot, double xnodp)
{
LOCAL_DOUBLE ds50, day, xnodce, bfact=0, gam, c;
LOCAL_REAL ctem, sinq, cosq, aqnv, xmao, stem, eqsq, xnoi, ainv2;
LOCAL_REAL zcosg, zsing, zcosi, zsini, zcosh, zsinh;
LOCAL_REAL cosomo, zcosgl, zcoshl, zcosil, sinomo;
LOCAL_REAL xpidot, zsinil, siniq2, cosiq2;
LOCAL_REAL rteqsq, zsinhl, zsingl;
LOCAL_REAL eoc, sgh, g200, bsq, zmo, xno2;
LOCAL_REAL a1, a2, a3, a4, a5, a6, a7, a8, a9, a10;
LOCAL_REAL x1, x2, x3, x4, x5, x6, x7, x8;
LOCAL_REAL z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33;
LOCAL_REAL s1, s2, s3, s4, s5, s6, s7, cc, ao, eq, se, shdq, si, sl;
LOCAL_REAL zx, zy, ze, zn;
LOCAL_REAL g201, g211, g310, g300, g322, g410, g422, g520, g533, g521, g532;
LOCAL_REAL f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543;
real siniq, cosiq;
real temp0, temp1;
int ls, imode=0;
int ishq;
/*
Copy the supplied orbital elements to "local" (static to this file)
variables and compute common trig values.
*/
eq = eo = orb_eo;
xincl = orb_xincl;
/* Decide on direct or Lyddane Lunar-Solar perturbations. */
ilsd = 0;
if(xincl >= (real)0.2) ilsd = 1;
/* Drop some terms below 3 deg inclination. */
ishq = 0;
#define SHQT 0.052359877
if (xincl >= (real)SHQT) ishq = 1; /* As per reoprt #3. */
SINCOS(omegao, &sinomo, &cosomo);
SINCOS(xnodeo, &sinq, &cosq);
SINCOS(xincl, &siniq, &cosiq);
if (fabs(siniq) <= SIN_EPS)
{
siniq = SIGN(SIN_EPS, siniq);
}
cosiq2 = cosiq * cosiq;
siniq2 = siniq * siniq;
ao = aodp;
omgdt = omgdot;
eqsq = eo * eo;
bsq = (real)1.0 - eqsq;
rteqsq = SQRT(bsq);
thetag(epoch, &thgr, &ds50);
/*printf("# epoch = %.8f ds50 = %.8f thgr = %f\n", epoch, ds50, DEG(thgr));*/
xnq = xnodp;
aqnv = (real)1.0 / ao;
xmao = xmo;
xpidot = omgdt + xnodot;
omegaq = omegao;
/* INITIALIZE LUNAR SOLAR TERMS */
day = ds50 + 18261.5;
xnodce = 4.523602 - day * 9.2422029e-4;
temp0 = (real)fmod(xnodce, TWOPI);
SINCOS(temp0, &stem, &ctem);
zcosil = (real)0.91375164 - ctem * (real)0.03568096;
zsinil = SQRT((real)1.0 - zcosil * zcosil);
zsinhl = stem * (real)0.089683511 / zsinil;
zcoshl = SQRT((real)1.0 - zsinhl * zsinhl);
c = day * 0.2299715 + 4.7199672;
gam = day * 0.001944368 + 5.8351514;
zmol = (real)MOD2PI(c - gam);
zx = stem * (real)0.39785416 / zsinil;
zy = zcoshl * ctem + zsinhl * (real)0.91744867 * stem;
zx = ATAN2(zx, zy);
zx = (real)fmod(gam + zx - xnodce, TWOPI);
SINCOS(zx, &zsingl, &zcosgl);
zmos = (real)MOD2PI(day * 0.017201977 + 6.2565837);
/* DO SOLAR TERMS */
zcosg = ZCOSGS;
zsing = ZSINGS;
zcosi = ZCOSIS;
zsini = ZSINIS;
zcosh = cosq;
zsinh = sinq;
cc = C1SS;
zn = ZNS;
ze = ZES;
zmo = zmos;
xnoi = (real)(1.0 / xnq);
for(ls = 0; ls < 2; ls++)
{
a1 = zcosg * zcosh + zsing * zcosi * zsinh;
a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
a8 = zsing * zsini;
a9 = zsing * zsinh + zcosg * zcosi * zcosh;
a10 = zcosg * zsini;
a2 = cosiq * a7 + siniq * a8;
a4 = cosiq * a9 + siniq * a10;
a5 = -siniq * a7 + cosiq * a8;
a6 = -siniq * a9 + cosiq * a10;
x1 = a1 * cosomo + a2 * sinomo;
x2 = a3 * cosomo + a4 * sinomo;
x3 = -a1 * sinomo + a2 * cosomo;
x4 = -a3 * sinomo + a4 * cosomo;
x5 = a5 * sinomo;
x6 = a6 * sinomo;
x7 = a5 * cosomo;
x8 = a6 * cosomo;
z31 = x1 * (real)12.0 * x1 - x3 * (real)3.0 * x3;
z32 = x1 * (real)24.0 * x2 - x3 * (real)6.0 * x4;
z33 = x2 * (real)12.0 * x2 - x4 * (real)3.0 * x4;
z1 = (a1 * a1 + a2 * a2) * (real)3.0 + z31 * eqsq;
z2 = (a1 * a3 + a2 * a4) * (real)6.0 + z32 * eqsq;
z3 = (a3 * a3 + a4 * a4) * (real)3.0 + z33 * eqsq;
z11 = a1 * (real)-6.0 * a5 + eqsq * (x1 * (real)-24.0 * x7 - x3 *
(real)6.0 * x5);
z12 = (a1 * a6 + a3 * a5) * (real)-6.0 + eqsq * ((x2 * x7 +
x1 * x8) * (real)-24.0 - (x3 * x6 + x4 * x5) * (real)6.0);
z13 = a3 * (real)-6.0 * a6 + eqsq * (x2 * (real)-24.0 * x8 - x4 *
(real)6.0 * x6);
z21 = a2 * (real)6.0 * a5 + eqsq * (x1 * (real)24.0 * x5 -
x3 * (real)6.0 * x7);
z22 = (a4 * a5 + a2 * a6) * (real)6.0 + eqsq * ((x2 * x5 + x1 * x6) *
(real)24.0 - (x4 * x7 + x3 * x8) * (real)6.0);
z23 = a4 * (real)6.0 * a6 + eqsq * (x2 * (real)24.0 * x6 - x4 *
(real)6.0 * x8);
z1 = z1 + z1 + bsq * z31;
z2 = z2 + z2 + bsq * z32;
z3 = z3 + z3 + bsq * z33;
s3 = cc * xnoi;
s2 = s3 * (real)-0.5 / rteqsq;
s4 = s3 * rteqsq;
s1 = eq * (real)-15.0 * s4;
s5 = x1 * x3 + x2 * x4;
s6 = x2 * x3 + x1 * x4;
s7 = x2 * x4 - x1 * x3;
se = s1 * zn * s5;
si = s2 * zn * (z11 + z13);
sl = -zn * s3 * (z1 + z3 - (real)14.0 - eqsq * (real)6.0);
sgh = s4 * zn * (z31 + z33 - (real)6.0);
shdq = 0;
if(ishq)
{
real sh = -zn * s2 * (z21 + z23);
shdq = sh / siniq;
}
ee2 = s1 * (real)2.0 * s6;
e3 = s1 * (real)2.0 * s7;
xi2 = s2 * (real)2.0 * z12;
xi3 = s2 * (real)2.0 * (z13 - z11);
xl2 = s3 * (real)-2.0 * z2;
xl3 = s3 * (real)-2.0 * (z3 - z1);
xl4 = s3 * (real)-2.0 * ((real)-21.0 - eqsq * (real)9.0) * ze;
xgh2 = s4 * (real)2.0 * z32;
xgh3 = s4 * (real)2.0 * (z33 - z31);
xgh4 = s4 * (real)-18.0 * ze;
xh2 = s2 * (real)-2.0 * z22;
xh3 = s2 * (real)-2.0 * (z23 - z21);
if (ls == 1) break;
/* DO LUNAR TERMS */
sse = se;
ssi = si;
ssl = sl;
ssh = shdq;
ssg = sgh - cosiq * ssh;
se2 = ee2;
si2 = xi2;
sl2 = xl2;
sgh2 = xgh2;
sh2 = xh2;
se3 = e3;
si3 = xi3;
sl3 = xl3;
sgh3 = xgh3;
sh3 = xh3;
sl4 = xl4;
sgh4 = xgh4;
zcosg = zcosgl;
zsing = zsingl;
zcosi = zcosil;
zsini = zsinil;
zcosh = zcoshl * cosq + zsinhl * sinq;
zsinh = sinq * zcoshl - cosq * zsinhl;
zn = ZNL;
cc = C1L;
ze = ZEL;
zmo = zmol;
}
sse += se;
ssi += si;
ssl += sl;
ssg += sgh - cosiq * shdq;
ssh += shdq;
if (xnq < 0.0052359877 && xnq > 0.0034906585)
{
/* 24h SYNCHRONOUS RESONANCE TERMS INITIALIZATION */
iresfl = 1;
isynfl = 1;
g200 = eqsq * (eqsq * (real)0.8125 - (real)2.5) + (real)1.0;
g310 = eqsq * (real)2.0 + (real)1.0;
g300 = eqsq * (eqsq * (real)6.60937 - (real)6.0) + (real)1.0;
f220 = (cosiq + (real)1.0) * (real)0.75 * (cosiq + (real)1.0);
f311 = siniq * (real)0.9375 * siniq * (cosiq * (real)3.0 +
(real)1.0) - (cosiq + (real)1.0) * (real)0.75;
f330 = cosiq + (real)1.0;
f330 = f330 * (real)1.875 * f330 * f330;
del1 = (real)3.0 * (real)(xnq * xnq * aqnv * aqnv);
del2 = del1 * (real)2.0 * f220 * g200 * Q22;
del3 = del1 * (real)3.0 * f330 * g300 * Q33 * aqnv;
del1 = del1 * f311 * g310 * Q31 * aqnv;
fasx2 = (real)0.13130908;
fasx4 = (real)2.8843198;
fasx6 = (real)0.37448087;
xlamo = xmao + xnodeo + omegao - thgr;
bfact = xlldot + xpidot - THDT;
bfact += (double)(ssl + ssg + ssh);
}
else if (xnq >= 0.00826 && xnq <= 0.00924 && eq >= (real)0.5)
{
/* GEOPOTENTIAL RESONANCE INITIALIZATION FOR 12 HOUR ORBITS */
iresfl = 1;
isynfl = 0;
eoc = eq * eqsq;
g201 = (real)-0.306 - (eq - (real)0.64) * (real)0.44;
if (eq <= (real)0.65)
{
g211 = (real)3.616 - eq * (real)13.247 + eqsq * (real)16.29;
g310 = eq * (real)117.39 - (real)19.302 - eqsq * (real)228.419 + eoc * (real)156.591;
g322 = eq * (real)109.7927 - (real)18.9068 - eqsq * (real)214.6334 + eoc * (real)146.5816;
g410 = eq * (real)242.694 - (real)41.122 - eqsq * (real)471.094 + eoc * (real)313.953;
g422 = eq * (real)841.88 - (real)146.407 - eqsq * (real)1629.014 + eoc * (real)1083.435;
g520 = eq * (real)3017.977 - (real)532.114 - eqsq * 5740.032 + eoc * (real)3708.276;
}
else
{
g211 = eq * (real)331.819 - (real)72.099 - eqsq * (real)508.738 + eoc * (real)266.724;
g310 = eq * (real)1582.851 - (real)346.844 - eqsq * (real)2415.925 + eoc * (real)1246.113;
g322 = eq * (real)1554.908 - (real)342.585 - eqsq * (real)2366.899 + eoc * (real)1215.972;
g410 = eq * (real)4758.686 - (real)1052.797 - eqsq * (real)7193.992 + eoc * (real)3651.957;
g422 = eq * (real)16178.11 - (real)3581.69 - eqsq * (real)24462.77 + eoc * (real)12422.52;
if (eq <= (real)0.715)
{
g520 = (real)1464.74 - eq * (real)4664.75 + eqsq * (real)3763.64;
}
else
{
g520 = eq * (real)29936.92 - (real)5149.66 - eqsq * (real)54087.36 + eoc * (real)31324.56;
}
}
if (eq < (real)0.7)
{
g533 = eq * (real)4988.61 - (real)919.2277 - eqsq * (real)9064.77 + eoc * (real)5542.21;
g521 = eq * (real)4568.6173 - (real)822.71072 - eqsq * (real)8491.4146 + eoc * (real)5337.524;
g532 = eq * (real)4690.25 - (real)853.666 - eqsq * (real)8624.77 + eoc * (real)5341.4;
}
else
{
g533 = eq * (real)161616.52 - (real)37995.78 - eqsq * (real)229838.2 + eoc * (real)109377.94;
g521 = eq * (real)218913.95 - (real)51752.104 - eqsq * (real)309468.16 + eoc * (real)146349.42;
g532 = eq * (real)170470.89 - (real)40023.88 - eqsq * (real)242699.48 + eoc * (real)115605.82;
}
f220 = (cosiq * (real)2.0 + (real)1.0 + cosiq2) * (real)0.75;
f221 = siniq2 * (real)1.5;
f321 = siniq * (real)1.875 * ((real)1.0 - cosiq * (real)2.0 - cosiq2 * (real)3.0);
f322 = siniq * (real)-1.875 * (cosiq * (real)2.0 + (real)1.0 - cosiq2 * (real)3.0);
f441 = siniq2 * (real)35.0 * f220;
f442 = siniq2 * (real)39.375 * siniq2;
f522 = siniq * (real)9.84375 * (siniq2 * ((real)1.0 - cosiq *
(real)2.0 - cosiq2 * (real)5.0) + (cosiq * (real)4.0 -
(real)2.0 + cosiq2 * (real)6.0) * (real)0.33333333);
f523 = siniq * (siniq2 * (real)4.92187512 * ((real)-2.0 - cosiq *
(real)4.0 + cosiq2 * (real)10.0) + (cosiq * (real)2.0 +
(real)1.0 - cosiq2 * (real)3.0) * (real)6.56250012);
f542 = siniq * (real)29.53125 * ((real)2.0 - cosiq * (real)8.0 +
cosiq2 * (cosiq * (real)8.0 - (real)12.0 + cosiq2 *
(real)10.0));
f543 = siniq * (real)29.53125 * ((real)-2.0 - cosiq * (real)8.0 +
cosiq2 * (cosiq * (real)8.0 + (real)12.0 - cosiq2 *
(real)10.0));
xno2 = (real)(xnq * xnq);
ainv2 = aqnv * aqnv;
temp1 = xno2 * (real)3.0 * ainv2;
temp0 = temp1 * ROOT22;
d2201 = temp0 * f220 * g201;
d2211 = temp0 * f221 * g211;
temp1 *= aqnv;
temp0 = temp1 * ROOT32;
d3210 = temp0 * f321 * g310;
d3222 = temp0 * f322 * g322;
temp1 *= aqnv;
temp0 = temp1 * (real)2.0 * ROOT44;
d4410 = temp0 * f441 * g410;
d4422 = temp0 * f442 * g422;
temp1 *= aqnv;
temp0 = temp1 * ROOT52;
d5220 = temp0 * f522 * g520;
d5232 = temp0 * f523 * g532;
temp0 = temp1 * (real)2.0 * ROOT54;
d5421 = temp0 * f542 * g521;
d5433 = temp0 * f543 * g533;
xlamo = xmao + xnodeo + xnodeo - thgr - thgr;
bfact = xlldot + xnodot + xnodot - THDT - THDT;
bfact += (double)(ssl + ssh + ssh);
}
else
{
/* NON RESONANT ORBITS */
iresfl = 0;
isynfl = 0;
}
if(iresfl == 0)
{
/* Non-resonant orbits. */
imode = SGDP4_DEEP_NORM;
}
else
{
/* INITIALIZE INTEGRATOR */
xfact = bfact - xnq;
xli = (double)xlamo;
xni = xnq;
atime = 0.0;
dot_terms_calculated();
/* Save the "dot" terms for integrator re-start. */
xnddt0 = xnddt;
xndot0 = xndot;
xldot0 = xldot;
if (isynfl)
imode = SGDP4_DEEP_SYNC;
else
imode = SGDP4_DEEP_RESN;
}
/* Set up for original mode (LS terms at epoch non-zero). */
ilsz = 0;
pgh0 = ph0 = pe0 = pinc0 = pl0 = (real)0.0;
if(Set_LS_zero)
{
/* Save the epoch case Lunar-Solar terms to remove this bias for
* actual computations later on.
* Not sure if this is a good idea.
*/
compute_LunarSolar(0.0);
pgh0 = pgh;
ph0 = ph;
pe0 = pe;
pinc0 = pinc;
pl0 = pl;
ilsz = 1;
}
return imode;
} /* SGDP4_dpinit */
/* =====================================================================
------------- ENTRANCE FOR DEEP SPACE SECULAR EFFECTS ---------------
xll : Input/Output, modified "mean anomaly" or "mean longitude".
omgasm : Input/Output, modified argument of perigee.
xnodes : Input/Output, modified right asc of ascn node.
em : Input/Output, modified eccentricity.
xinc : Input/Output, modified inclination.
xn : Output, modified period from 'xnodp'.
tsince : Input, time from epoch (minutes).
===================================================================== */
int SGDP4_dpsec(double *xll, real *omgasm, real *xnodes, real *em,
real *xinc, double *xn, double tsince)
{
LOCAL_DOUBLE delt, ft, xl;
real temp0;
*xll += ssl * tsince;
*omgasm += ssg * tsince;
*xnodes += ssh * tsince;
*em += sse * tsince;
*xinc += ssi * tsince;
if (iresfl == 0) return 0;
/*
* A minor increase in some efficiency can be had by restarting if
* the new time is closer to epoch than to the old integrated
* time. This also forces a re-start on a change in sign (i.e. going
* through zero time) as then we have |tsince - atime| > |tsince|
* as well. Second test is for stepping back towards zero, forcing a restart
* if close enough rather than integrating to zero.
*/
#define AHYST 1.0
/* Most accurate (OK, most _consistant_) method. Restart if need to
* integrate 'backwards' significantly from current point.
*/
if(fabs(tsince) < STEP ||
(atime > 0.0 && tsince < atime - AHYST) ||
(atime < 0.0 && tsince > atime + AHYST))
{
/* Epoch restart if we are at, or have crossed, tsince==0 */
atime = 0.0;
xni = xnq;
xli = (double)xlamo;
/* Restore the old "dot" terms. */
xnddt = xnddt0;
xndot = xndot0;
xldot = xldot0;
}
ft = tsince - atime;
if (fabs(ft) > MAX_INTEGRATE)
{
fatal_error("SGDP4_dpsec: Integration limit reached");
return -1;
}
if (fabs(ft) >= STEP)
{
/*
Do integration if required. Find the step direction to
make 'atime' catch up with 'tsince'.
*/
delt = (tsince >= atime ? STEP : -STEP);
do {
/* INTEGRATOR (using the last "dot" terms). */
xli += delt * (xldot + delt * (real)0.5 * xndot);
xni += delt * (xndot + delt * (real)0.5 * xnddt);
atime += delt;
dot_terms_calculated();
/* Are we close enough now ? */
ft = tsince - atime;
} while (fabs(ft) >= STEP);
}
xl = xli + ft * (xldot + ft * (real)0.5 * xndot);
*xn = xni + ft * (xndot + ft * (real)0.5 * xnddt);
temp0 = -(*xnodes) + thgr + tsince * THDT;
if (isynfl == 0)
*xll = xl + temp0 + temp0;
else
*xll = xl - *omgasm + temp0;
return 0;
} /* SGDP4_dpsec */
/* =====================================================================
Here we do the "dot" terms for the integrator. Separate function so we
can call when initialising and save the atime==0.0 values for later
epoch re-start of the integrator.
===================================================================== */
static void dot_terms_calculated(void)
{
LOCAL_DOUBLE x2li, x2omi, xomi;
/* DOT TERMS CALCULATED */
if (isynfl)
{
xndot = del1 * SIN(xli - fasx2)
+ del2 * SIN((xli - fasx4) * (real)2.0)
+ del3 * SIN((xli - fasx6) * (real)3.0);
xnddt = del1 * COS(xli - fasx2)
+ del2 * COS((xli - fasx4) * (real)2.0) * (real)2.0
+ del3 * COS((xli - fasx6) * (real)3.0) * (real)3.0;
}
else
{
xomi = omegaq + omgdt * atime;
x2omi = xomi + xomi;
x2li = xli + xli;
xndot = d2201 * SIN(x2omi + xli - G22)
+ d2211 * SIN(xli - G22)
+ d3210 * SIN(xomi + xli - G32)
+ d3222 * SIN(-xomi + xli - G32)
+ d5220 * SIN(xomi + xli - G52)
+ d5232 * SIN(-xomi + xli - G52)
+ d4410 * SIN(x2omi + x2li - G44)
+ d4422 * SIN(x2li - G44)
+ d5421 * SIN(xomi + x2li - G54)
+ d5433 * SIN(-xomi + x2li - G54);
xnddt = d2201 * COS(x2omi + xli - G22)
+ d2211 * COS(xli - G22)
+ d3210 * COS(xomi + xli - G32)
+ d3222 * COS(-xomi + xli - G32)
+ d5220 * COS(xomi + xli - G52)
+ d5232 * COS(-xomi + xli - G52)
+ (d4410 * COS(x2omi + x2li - G44)
+ d4422 * COS(x2li - G44)
+ d5421 * COS(xomi + x2li - G54)
+ d5433 * COS(-xomi + x2li - G54)) * (real)2.0;
}
xldot = (real)(xni + xfact);
xnddt *= xldot;
} /* dot_terms_calculated */
/* =====================================================================
---------------- ENTRANCES FOR LUNAR-SOLAR PERIODICS ----------------
em : Input/Output, modified eccentricity.
xinc : Input/Output, modified inclination.
omgasm : Input/Output, modified argument of perigee.
xnodes : Input/Output, modified right asc of ascn node.
xll : Input/Output, modified "mean anomaly" or "mean longitude".
tsince : Input, time from epoch (minutes).
===================================================================== */
int SGDP4_dpper(real *em, real *xinc, real *omgasm, real *xnodes,
double *xll, double tsince)
{
real sinis, cosis;
compute_LunarSolar(tsince);
*xinc += pinc;
*em += pe;
/* Spacetrack report #3 has sin/cos from before perturbations
* added to xinc (oldxinc), but apparently report # 6 has then
* from after they are added.
*/
SINCOS(*xinc, &sinis, &cosis);
if (ilsd)
{
/* APPLY PERIODICS DIRECTLY */
real tmp_ph;
tmp_ph = ph / sinis;
*omgasm += pgh - cosis * tmp_ph;
*xnodes += tmp_ph;
*xll += pl;
}
else
{
/* APPLY PERIODICS WITH LYDDANE MODIFICATION */
LOCAL_REAL alfdp, betdp, dalf, dbet, xls, dls;
LOCAL_REAL sinok, cosok;
int ishift;
real tmp, oldxnode = (*xnodes);
SINCOS(*xnodes, &sinok, &cosok);
alfdp = sinis * sinok;
betdp = sinis * cosok;
dalf = ph * cosok + pinc * cosis * sinok;
dbet = -ph * sinok + pinc * cosis * cosok;
alfdp += dalf;
betdp += dbet;
xls = (real)*xll + *omgasm + cosis * *xnodes;
dls = pl + pgh - pinc * *xnodes * sinis;
xls += dls;
*xnodes = ATAN2(alfdp, betdp);
/* Get perturbed xnodes in to same quadrant as original. */
ishift = NINT((oldxnode - (*xnodes))/TWOPI);
*xnodes += (real)(TWOPI * ishift);
*xll += (double)pl;
*omgasm = xls - (real)*xll - cosis * (*xnodes);
}
return 0;
} /* SGDP4_dpper */
/* =====================================================================
Do the Lunar-Solar terms for the SGDP4_dpper() function (normally only
every 1/2 hour needed. Seperate function so initialisng could save the
epoch terms to zero them. Not sure if this is a good thing (some believe
it the way the equations were intended) as the two-line elements may
be computed to give the right answer with out this (which I would hope
as it would make predictions consistant with the 'official' model
code).
===================================================================== */
static void compute_LunarSolar(double tsince)
{
LOCAL_REAL sinzf, coszf;
LOCAL_REAL f2, f3, zf, zm;
LOCAL_REAL sel, sil, ses, sll, sis, sls;
LOCAL_REAL sghs, shs, sghl, shl;
/* Update Solar terms. */
zm = zmos + ZNS * tsince;
zf = zm + ZES * (real)2.0 * SIN(zm);
SINCOS(zf, &sinzf, &coszf);
f2 = sinzf * (real)0.5 * sinzf - (real)0.25;
f3 = sinzf * (real)-0.5 * coszf;
ses = se2 * f2 + se3 * f3;
sis = si2 * f2 + si3 * f3;
sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf;
sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf;
shs = sh2 * f2 + sh3 * f3;
/* Update Lunar terms. */
zm = zmol + ZNL * tsince;
zf = zm + ZEL * (real)2.0 * SIN(zm);
SINCOS(zf, &sinzf, &coszf);
f2 = sinzf * (real)0.5 * sinzf - (real)0.25;
f3 = sinzf * (real)-0.5 * coszf;
sel = ee2 * f2 + e3 * f3;
sil = xi2 * f2 + xi3 * f3;
sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf;
sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf;
shl = xh2 * f2 + xh3 * f3;
/* Save computed values to calling structure. */
pgh = sghs + sghl;
ph = shs + shl;
pe = ses + sel;
pinc = sis + sil;
pl = sls + sll;
if (ilsz)
{
/* Correct for previously saved epoch terms. */
pgh -= pgh0;
ph -= ph0;
pe -= pe0;
pinc -= pinc0;
pl -= pl0;
}
}
/* =====================================================================
This function converts the epoch time (in the form of YYDDD.DDDDDDDD,
exactly as it appears in the two-line elements) into days from 00:00:00
hours Jan 1st 1950 UTC. Also it computes the right ascencion of Greenwich
at the epoch time, but not in a very accurate manner. However, the same
method is used here to allow exact comparason with the original FORTRAN
versions of the programs. The calling arguments are:
ep : Input, epoch time of elements (as read from 2-line data).
thegr : Output, right ascensionm of Greenwich at epoch, radian.
days50 : Output, days from Jan 1st 1950 00:00:00 UTC.
===================================================================== */
#define THETAG 2
/* Version like sat_code. */
#define J1900 (2451545.5 - 36525. - 1.)
#define SECDAY (86400.0)
#define C1 (1.72027916940703639E-2)
#define C1P2P (C1 + TWOPI)
#define THGR70 (1.7321343856509374)
#define FK5R (5.07551419432269442E-15)
static void thetag(double ep, real *thegr, double *days50)
{
double d;
long n, jy;
double jd, theta;
jy = (long)((ep + 2.0e-7) * 0.001); /* Extract the year. */
d = ep - jy * 1.0e3; /* And then the day of year. */
/* Assume " 8" is 1980, or more sensibly 2008 ? */
/*
if (jy < 10) jy += 80;
*/
if (jy < 50) jy += 100;
if (jy < 70) /* Fix for leap years ? */
n = (jy - 72) / 4;
else
n = (jy - 69) / 4;
*days50 = (jy - 70) * 365.0 + 7305.0 + n + d;
jd = d + J1900 + jy * 365. + ((jy - 1) / 4);
#if THETAG == 0
/* Original report #3 code. */
theta = *days50 * 6.3003880987 + 1.72944494;
#elif THETAG == 1
{
/* Method from project pluto code. */
/* Reference: The 1992 Astronomical Almanac, page B6. */
const double omega_E = 1.00273790934; /* Earth rotations per sidereal day (non-constant) */
const double UT = fmod(jd + 0.5, 1.0);
double t_cen, GMST;
t_cen = (jd - UT - 2451545.0) / 36525.0;
GMST = 24110.54841 + t_cen * (8640184.812866 + t_cen * (0.093104 - t_cen * 6.2E-6));
GMST = fmod( GMST + SECDAY * omega_E * UT, SECDAY);
if(GMST < 0.0) GMST += SECDAY;
theta = TWOPI * GMST / SECDAY;
}
#elif THETAG == 2
{
/* Method from SGP4SUB.F code. */
double ts70, ds70, trfac;
long ids70;
ts70 = (*days50) - 7305.0;
ids70 = (long)(ts70 + 1.0e-8);
ds70 = ids70;
trfac = ts70 - ds70;
/* CALCULATE GREENWICH LOCATION AT EPOCH */
theta = THGR70 + C1*ds70 + C1P2P*trfac + ts70*ts70*FK5R;
}
#else
#error 'Unknown method for theta-G calculation'
#endif
theta = fmod(theta, TWOPI);
if (theta < 0.0) theta += TWOPI;
*thegr = (real)theta;
} /* thetag */
#endif /* !NO_DEEP_SPACE */

29
ferror.c 100644
View File

@ -0,0 +1,29 @@
/* > satutl.c
*
*/
#include "sgdp4h.h"
#include <stdarg.h>
void fatal_error(const char *format, ...)
{
va_list arg_ptr;
fflush(stdout);
fprintf(stderr, "\nDundee Satellite Lab fatal run-time error:\n");
va_start(arg_ptr, format);
vfprintf(stderr, format, arg_ptr);
va_end(arg_ptr);
fprintf(stderr, "\nNow terminating the program...\n");
fflush(stderr);
exit(5);
}
/* ===================================================================== */

225
rfoverlay.c 100644
View File

@ -0,0 +1,225 @@
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "sgdp4h.h"
#include "satutl.h"
#include "cpgplot.h"
#define LIM 80
#define D2R M_PI/180.0
#define R2D 180.0/M_PI
#define XKMPER 6378.135 // Earth radius in km
#define XKMPAU 149597879.691 // AU in km
#define FLAT (1.0/298.257)
#define C 299792.458 // Speed of light in km/s
struct point {
xyz_t obspos,obsvel;
};
struct site {
int id;
double lng,lat;
float alt;
char observer[64];
};
// Return x modulo y [0,y)
double modulo(double x,double y)
{
x=fmod(x,y);
if (x<0.0) x+=y;
return x;
}
// Read a line of maximum length int lim from file FILE into string s
int fgetline(FILE *file,char *s,int lim)
{
int c,i=0;
while (--lim > 0 && (c=fgetc(file)) != EOF && c != '\n')
s[i++] = c;
// if (c == '\n')
// s[i++] = c;
s[i] = '\0';
return i;
}
// Greenwich Mean Sidereal Time
double gmst(double mjd)
{
double t,gmst;
t=(mjd-51544.5)/36525.0;
gmst=modulo(280.46061837+360.98564736629*(mjd-51544.5)+t*t*(0.000387933-t/38710000),360.0);
return gmst;
}
// Greenwich Mean Sidereal Time
double dgmst(double mjd)
{
double t,dgmst;
t=(mjd-51544.5)/36525.0;
dgmst=360.98564736629+t*(0.000387933-t/38710000);
return dgmst;
}
// Observer position
void obspos_xyz(double mjd,double lng,double lat,float alt,xyz_t *pos,xyz_t *vel)
{
double ff,gc,gs,theta,s,dtheta;
s=sin(lat*D2R);
ff=sqrt(1.0-FLAT*(2.0-FLAT)*s*s);
gc=1.0/ff+alt/XKMPER;
gs=(1.0-FLAT)*(1.0-FLAT)/ff+alt/XKMPER;
theta=gmst(mjd)+lng;
dtheta=dgmst(mjd)*D2R/86400;
pos->x=gc*cos(lat*D2R)*cos(theta*D2R)*XKMPER;
pos->y=gc*cos(lat*D2R)*sin(theta*D2R)*XKMPER;
pos->z=gs*sin(lat*D2R)*XKMPER;
vel->x=-gc*cos(lat*D2R)*sin(theta*D2R)*XKMPER*dtheta;
vel->y=gc*cos(lat*D2R)*cos(theta*D2R)*XKMPER*dtheta;
vel->z=0.0;
return;
}
// Get observing site
struct site get_site(int site_id)
{
int i=0;
char line[LIM];
FILE *file;
int id;
double lat,lng;
float alt;
char abbrev[3],observer[64];
struct site s;
char *env,filename[LIM];
env=getenv("ST_DATADIR");
sprintf(filename,"%s/data/sites.txt",env);
file=fopen(filename,"r");
if (file==NULL) {
printf("File with site information not found!\n");
return;
}
while (fgets(line,LIM,file)!=NULL) {
// Skip
if (strstr(line,"#")!=NULL)
continue;
// Strip newline
line[strlen(line)-1]='\0';
// Read data
sscanf(line,"%4d %2s %lf %lf %f",
&id,abbrev,&lat,&lng,&alt);
strcpy(observer,line+38);
// Change to km
alt/=1000.0;
// Copy site
if (id==site_id) {
s.lat=lat;
s.lng=lng;
s.alt=alt;
s.id=id;
strcpy(s.observer,observer);
}
}
fclose(file);
return s;
}
// Plot overlay
void overlay(double *mjd,int n,int site_id)
{
int i,imode,flag,satno,tflag;
struct point *p;
struct site s;
FILE *file,*infile;
orbit_t orb;
xyz_t satpos,satvel;
double dx,dy,dz,dvx,dvy,dvz,r,v,za;
double freq,freq0;
char line[LIM],text[8];
// Get site
s=get_site(site_id);
// Allocate
p=(struct point *) malloc(sizeof(struct point)*n);
// Get observer position
for (i=0;i<n;i++)
obspos_xyz(mjd[i],s.lng,s.lat,s.alt,&p[i].obspos,&p[i].obsvel);
infile=fopen("frequencies.txt","r");
while (fgetline(infile,line,LIM)>0) {
sscanf(line,"%d %lf",&satno,&freq0);
sprintf(text," %d",satno);
// Loop over TLEs
file=fopen("/home/bassa/code/c/satellite/sattools/tle/bulk.tle","r");
while (read_twoline(file,satno,&orb)==0) {
// Initialize
imode=init_sgdp4(&orb);
if (imode==SGDP4_ERROR)
printf("Error\n");
// Loop over points
for (i=0,flag=0,tflag=0;i<n;i++) {
// Get satellite position
satpos_xyz(mjd[i]+2400000.5,&satpos,&satvel);
dx=satpos.x-p[i].obspos.x;
dy=satpos.y-p[i].obspos.y;
dz=satpos.z-p[i].obspos.z;
dvx=satvel.x-p[i].obsvel.x;
dvy=satvel.y-p[i].obsvel.y;
dvz=satvel.z-p[i].obsvel.z;
r=sqrt(dx*dx+dy*dy+dz*dz);
v=(dvx*dx+dvy*dy+dvz*dz)/r;
za=acos((p[i].obspos.x*dx+p[i].obspos.y*dy+p[i].obspos.z*dz)/(r*XKMPER))*R2D;
freq=(1.0-v/C)*freq0;
if (flag==0) {
cpgmove((float) i,(float) freq);
flag=1;
} else {
cpgdraw((float) i,(float) freq);
}
if (za<90.0 && flag==0) {
flag=1;
} else if (za>90.0 && flag==1) {
if (tflag==0) {
tflag=1;
cpgtext((float) i,(float) freq,text);
}
flag=0;
}
}
}
fclose(file);
}
fclose(infile);
// Free
free(p);
return;
}

1
rfoverlay.h 100644
View File

@ -0,0 +1 @@
void overlay(double *mjd,int n,int site_id);

230
satutl.c 100644
View File

@ -0,0 +1,230 @@
/* > satutl.c
*
*/
#include "sgdp4h.h"
#include <ctype.h>
static char *st_start(char *buf);
static long i_read(char *str, int start, int stop);
static double d_read(char *str, int start, int stop);
/* ====================================================================
Read a string from key board, remove CR/LF etc.
==================================================================== */
void read_kb(char *buf)
{
int ii;
fgets(buf, ST_SIZE-1, stdin);
/* Remove the CR/LF etc. */
for(ii = 0; ii < ST_SIZE; ii++)
{
if(buf[ii] == '\r' || buf[ii] == '\n')
{
buf[ii] = '\0';
break;
}
}
}
/* ====================================================================
Read orbit parameters for "satno" in file "filename", return -1 if
failed to find the corresponding data. Call with satno = 0 to get the
next elements of whatever sort.
==================================================================== */
int read_twoline(FILE *fp, long search_satno, orbit_t *orb)
{
static char search[ST_SIZE];
static char line1[ST_SIZE];
static char line2[ST_SIZE];
char *st1, *st2;
int found;
double bm, bx;
st1 = line1;
st2 = line2;
do {
if(fgets(line1, ST_SIZE-1, fp) == NULL) return -1;
st1 = st_start(line1);
} while(st1[0] != '1');
if(search_satno > 0)
{
found = 0;
}
else
{
found = 1;
search_satno = atol(st1+2);
}
sprintf(search, "1 %05ld", search_satno);
do {
st1 = st_start(line1);
if(strncmp(st1, search, 7) == 0)
{
found = 1;
break;
}
} while(fgets(line1, ST_SIZE-1, fp) != NULL);
sprintf(search, "2 %05ld", search_satno);
if(found)
{
fgets(line2, ST_SIZE-1, fp);
st2 = st_start(line2);
}
if(!found || strncmp(st2, search, 7) != 0)
{
return -1;
}
orb->ep_year = (int)i_read(st1, 19, 20);
if(orb->ep_year < 57) orb->ep_year += 2000;
else orb->ep_year += 1900;
orb->ep_day = d_read(st1, 21, 32);
orb->ndot2 = d_read(st1, 34, 43);
bm = d_read(st1, 45, 50) * 1.0e-5;
bx = d_read(st1, 51, 52);
orb->nddot6 = bm * pow(10.0, bx);
bm = d_read(st1, 54, 59) * 1.0e-5;
bx = d_read(st1, 60, 61);
orb->bstar = bm * pow(10.0, bx);
orb->eqinc = RAD(d_read(st2, 9, 16));
orb->ascn = RAD(d_read(st2, 18, 25));
orb->ecc = d_read(st2, 27, 33) * 1.0e-7;
orb->argp = RAD(d_read(st2, 35, 42));
orb->mnan = RAD(d_read(st2, 44, 51));
orb->rev = d_read(st2, 53, 63);
orb->norb = i_read(st2, 64, 68);
orb->satno = search_satno;
sscanf(st1+9,"%s",orb->desig);
return 0;
}
/* ==================================================================
Locate the first non-white space character, return location.
================================================================== */
static char *st_start(char *buf)
{
if(buf == NULL) return buf;
while(*buf != '\0' && isspace(*buf)) buf++;
return buf;
}
/* ==================================================================
Mimick the FORTRAN formatted read (assumes array starts at 1), copy
characters to buffer then convert.
================================================================== */
static long i_read(char *str, int start, int stop)
{
long itmp=0;
char *buf, *tmp;
int ii;
start--; /* 'C' arrays start at 0 */
stop--;
tmp = buf = (char *)vector(stop-start+2, sizeof(char));
for(ii = start; ii <= stop; ii++)
{
*tmp++ = str[ii]; /* Copy the characters. */
}
*tmp = '\0'; /* NUL terminate */
itmp = atol(buf); /* Convert to long integer. */
free(buf);
return itmp;
}
/* ==================================================================
Mimick the FORTRAN formatted read (assumes array starts at 1), copy
characters to buffer then convert.
================================================================== */
static double d_read(char *str, int start, int stop)
{
double dtmp=0;
char *buf, *tmp;
int ii;
start--;
stop--;
tmp = buf = (char *)vector(stop-start+2, sizeof(char));
for(ii = start; ii <= stop; ii++)
{
*tmp++ = str[ii]; /* Copy the characters. */
}
*tmp = '\0'; /* NUL terminate */
dtmp = atof(buf); /* Convert to long integer. */
free(buf);
return dtmp;
}
/* ==================================================================
Allocate and check an all-zero array of memory (storage vector).
================================================================== */
void *vector(size_t num, size_t size)
{
void *ptr;
ptr = calloc(num, size);
if(ptr == NULL)
{
fatal_error("vector: Allocation failed %u * %u", num, size);
}
return ptr;
}
/* ==================================================================
Print out orbital parameters.
================================================================== */
void print_orb(orbit_t *orb)
{
printf("# Satellite ID = %ld\n", (long)orb->satno);
printf("# Satellite designation = %s\n",orb->desig);
printf("# Epoch year = %d day = %.8f\n", orb->ep_year, orb->ep_day);
printf("# Eccentricity = %.7f\n", orb->ecc);
printf("# Equatorial inclination = %.4f deg\n", DEG(orb->eqinc));
printf("# Argument of perigee = %.4f deg\n", DEG(orb->argp));
printf("# Mean anomaly = %.4f deg\n", DEG(orb->mnan));
printf("# Right Ascension of Ascending Node = %.4f deg\n", DEG(orb->ascn));
printf("# Mean Motion (number of rev/day) = %.8f\n", orb->rev);
printf("# First derivative of mean motion = %e\n",orb->ndot2);
printf("# Second derivative of mean motion = %e\n",orb->nddot6);
printf("# BSTAR drag = %.4e\n", orb->bstar);
printf("# Orbit number = %ld\n", orb->norb);
}
/* ====================================================================== */

30
satutl.h 100644
View File

@ -0,0 +1,30 @@
/* > satutl.h
*
*/
#ifndef _SATUTL_H
#define _SATUTL_H
#define ST_SIZE 256
#ifdef __cplusplus
extern "C" {
#endif
/** satutl.c **/
void read_kb(char *buf);
int read_twoline(FILE *fp, long satno, orbit_t *orb);
void *vector(size_t num, size_t size);
void print_orb(orbit_t *orb);
/** aries.c **/
double gha_aries(double jd);
/** ferror.c **/
void fatal_error(const char *format, ...);
#ifdef __cplusplus
}
#endif
#endif /* _SATUTL_H */

828
sgdp4.c 100644
View File

@ -0,0 +1,828 @@
/* > sgdp4.c
*
* 1.00 around 1980 - Felix R. Hoots & Ronald L. Roehrich, from original
* SDP4.FOR and SGP4.FOR
*
************************************************************************
*
* Made famous by the spacetrack report No.3:
* "Models for Propogation of NORAD Element Sets"
* Edited and subsequently distributed by Dr. T. S. Kelso.
*
************************************************************************
*
* This conversion by:
* Paul S. Crawford and Andrew R. Brooks
* Dundee University
*
* NOTE !
* This code is supplied "as is" and without warranty of any sort.
*
* (c) 1994-2004, Paul Crawford, Andrew Brooks
*
************************************************************************
*
* 1.07 arb Oct 1994 - Transcribed by arb Oct 1994 into 'C', then
* modified to fit Dundee systems by psc.
*
* 1.08 psc Mon Nov 7 1994 - replaced original satpos.c with SGP4 model.
*
* 1.09 psc Wed Nov 9 1994 - Corrected a few minor translation errors after
* testing with example two-line elements.
*
* 1.10 psc Mon Nov 21 1994 - A few optimising tweeks.
*
* 1.11 psc Wed Nov 30 1994 - No longer uses eloset() and minor error in the
* SGP4 code corrected.
*
* 2.00 psc Tue Dec 13 1994 - arb discovered the archive.afit.af.mil FTP site
* with the original FORTRAN code in machine form.
* Tidied up and added support for the SDP4 model.
*
* 2.01 psc Fri Dec 23 1994 - Tested out the combined SGP4/SDP4 code against
* the original FORTRAN versions.
*
* 2.02 psc Mon Jan 02 1995 - Few more tweeks and tidied up the
* doccumentation for more general use.
*
* 3.00 psc Mon May 29 1995 - Cleaned up for general use & distrabution (to
* remove Dundee specific features).
*
* 3.01 psc Mon Jan 12 2004 - Minor bug fix for day calculation.
*
* 3.02 psc Mon Jul 10 2006 - Added if(rk < (real)1.0) test for sub-orbital decay.
*
* 3.03 psc Sat Aug 05 2006 - Added trap for divide-by-zero when calculating xlcof.
*
*/
static const char SCCSid[] = "@(#)sgdp4.c 3.03 (C) 1995 psc SatLib: Orbital Model";
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
/* ================ single / double precision fix-ups =============== */
#include "sgdp4h.h"
#define ECC_ZERO ((real)0.0) /* Zero eccentricity case ? */
#define ECC_ALL ((real)1.0e-4) /* For all drag terms in GSFC case. */
#define ECC_EPS ((real)1.0e-6) /* Too low for computing further drops. */
#define ECC_LIMIT_LOW ((real)-1.0e-3) /* Exit point for serious decaying of orbits. */
#define ECC_LIMIT_HIGH ((real)(1.0 - ECC_EPS)) /* Too close to 1 */
#define EPS_COSIO (1.5e-12) /* Minimum divisor allowed for (...)/(1+cos(IO)) */
#define TOTHRD (2.0/3.0)
#if defined( SGDP4_SNGL ) || 0
#define NR_EPS ((real)(1.0e-6)) /* Minimum ~1e-6 min for float. */
#else
#define NR_EPS ((real)(1.0e-12)) /* Minimum ~1e-14 for double. */
//#define NR_EPS ((real)(1.0e-14)) /* Minimum ~1e-14 for double. */
//#define NR_EPS ((real)(1.0e-8)) /* Minimum ~1e-14 for double. */
#endif
#define Q0 ((real)120.0)
#define S0 ((real)78.0)
#define XJ2 ((real)1.082616e-3)
#define XJ3 ((real)-2.53881e-6)
#define XJ4 ((real)-1.65597e-6)
#define XKMPER (6378.135) /* Km per earth radii */
#define XMNPDA (1440.0) /* Minutes per day */
#define AE (1.0) /* Earth radius in "chosen units". */
#if 0
/* Original code constants. */
#define XKE (0.743669161e-1)
#define CK2 ((real)5.413080e-4) /* (0.5 * XJ2 * AE * AE) */
#define CK4 ((real)0.62098875e-6) /* (-0.375 * XJ4 * AE * AE * AE * AE) */
#define QOMS2T ((real)1.88027916e-9) /* (pow((Q0 - S0)*AE/XKMPER, 4.0)) */
#define KS ((real)1.01222928) /* (AE * (1.0 + S0/XKMPER)) */
#else
/* GSFC improved coeficient resolution. */
#define XKE ((real)7.43669161331734132e-2)
#define CK2 ((real)(0.5 * XJ2 * AE * AE))
#define CK4 ((real)(-0.375 * XJ4 * AE * AE * AE * AE))
#define QOMS2T ((real)1.880279159015270643865e-9) /* (pow((Q0 - S0)*AE/XKMPER, 4.0)) */
#define KS ((real)(AE * (1.0 + S0/XKMPER)))
#endif
static const real a3ovk2 = (real)(-XJ3 / CK2 * (AE * AE * AE));
/* ================= Copy of the orbital elements ==================== */
static double xno; /* Mean motion (rad/min) */
static real xmo; /* Mean "mean anomaly" at epoch (rad). */
static real eo; /* Eccentricity. */
static real xincl; /* Equatorial inclination (rad). */
static real omegao; /* Mean argument of perigee at epoch (rad). */
static real xnodeo; /* Mean longitude of ascending node (rad, east). */
static real bstar; /* Drag term. */
double SGDP4_jd0; /* Julian Day for epoch (available to outside functions. */
/* ================== Local "global" variables for SGP4 ================= */
static int imode = SGDP4_NOT_INIT;
static real sinIO, cosIO, sinXMO, cosXMO;
static real c1, c2, c3, c4, c5, d2, d3, d4;
static real omgcof, xmcof, xlcof, aycof;
static real t2cof, t3cof, t4cof, t5cof;
static real xnodcf, delmo, x7thm1, x3thm1, x1mth2;
static real aodp, eta, omgdot, xnodot;
static double xnodp, xmdot;
static long Isat=0; /* 16-bit compilers need 'long' integer for higher space catalogue numbers. */
double perigee, period, apogee;
long Icount = 0;
int MaxNR=0;
extern int Set_LS_zero; /* From deep.c */
/* =======================================================================
The init_sgdp4() function passes all of the required orbital elements to
the sgdp4() function together with the pre-calculated constants. There is
some basic error traps and the detemination of the orbital model is made.
For near-earth satellites (xnodp < 225 minutes according to the NORAD
classification) the SGP4 model is used, with truncated terms for low
perigee heights when the drag terms are high. For deep-space satellites
the SDP4 model is used and the deep-space terms initialised (a slow
process). For orbits with an eccentricity of less than ECC_EPS the model
reverts to a very basic circular model. This is not physically meaningfull
but such a circluar orbit is not either! It is fast though.
Callinr arguments:
orb : Input, structure with the orbital elements from NORAD 2-line
element data in radian form.
The return value indicates the orbital model used.
======================================================================= */
int init_sgdp4(orbit_t *orb)
{
LOCAL_REAL theta2, theta4, xhdot1, x1m5th;
LOCAL_REAL s4, del1, del0;
LOCAL_REAL betao, betao2, coef, coef1;
LOCAL_REAL etasq, eeta, qoms24;
LOCAL_REAL pinvsq, tsi, psisq, c1sq;
LOCAL_DOUBLE a0, a1, epoch;
real temp0, temp1, temp2, temp3;
long iday, iyear;
/* Copy over elements. */
/* Convert year to Gregorian with century as 1994 or 94 type ? */
iyear = (long)orb->ep_year;
if (iyear < 1957)
{
/* Assume 0 and 100 both refer to 2000AD */
iyear += (iyear < 57 ? 2000 : 1900);
}
if (iyear < 1901 || iyear > 2099)
{
fatal_error("init_sgdp4: Satellite ep_year error %ld", iyear);
imode = SGDP4_ERROR;
return imode;
}
Isat = orb->satno;
/* Compute days from 1st Jan 1900 (works 1901 to 2099 only). */
iday = ((iyear - 1901)*1461L)/4L + 364L + 1L;
SGDP4_jd0 = JD1900 + iday + (orb->ep_day - 1.0); /* Julian day number. */
epoch = (iyear - 1900) * 1.0e3 + orb->ep_day; /* YYDDD.DDDD as from 2-line. */
#ifdef DEBUG
fprintf(stderr, "Epoch = %f SGDP4_jd0 = %f\n", epoch, SGDP4_jd0);
#endif
eo = (real)orb->ecc;
xno = (double)orb->rev * TWOPI/XMNPDA; /* Radian / unit time. */
xincl = (real)orb->eqinc;
xnodeo = (real)orb->ascn;
omegao = (real)orb->argp;
xmo = (real)orb->mnan;
bstar = (real)orb->bstar;
/* A few simple error checks here. */
if (eo < (real)0.0 || eo > ECC_LIMIT_HIGH)
{
fatal_error("init_sgdp4: Eccentricity out of range for %ld (%le)", Isat, (double)eo);
imode = SGDP4_ERROR;
return imode;
}
if (xno < 0.035*TWOPI/XMNPDA || xno > 18.0*TWOPI/XMNPDA)
{
fatal_error("init_sgdp4: Mean motion out of range %ld (%le)", Isat, xno);
imode = SGDP4_ERROR;
return imode;
}
if (xincl < (real)0.0 || xincl > (real)PI)
{
fatal_error("init_sgdp4: Equatorial inclination out of range %ld (%le)", Isat, DEG(xincl));
imode = SGDP4_ERROR;
return imode;
}
/* Start the initialisation. */
if (eo < ECC_ZERO)
imode = SGDP4_ZERO_ECC; /* Special mode for "ideal" circular orbit. */
else
imode = SGDP4_NOT_INIT;
/*
Recover original mean motion (xnodp) and semimajor axis (aodp)
from input elements.
*/
SINCOS(xincl, &sinIO, &cosIO);
theta2 = cosIO * cosIO;
theta4 = theta2 * theta2;
x3thm1 = (real)3.0 * theta2 - (real)1.0;
x1mth2 = (real)1.0 - theta2;
x7thm1 = (real)7.0 * theta2 - (real)1.0;
a1 = pow(XKE / xno, TOTHRD);
betao2 = (real)1.0 - eo * eo;
betao = SQRT(betao2);
temp0 = (real)(1.5 * CK2) * x3thm1 / (betao * betao2);
del1 = temp0 / (a1 * a1);
a0 = a1 * (1.0 - del1 * (1.0/3.0 + del1 * (1.0 + del1 * 134.0/81.0)));
del0 = temp0 / (a0 * a0);
xnodp = xno / (1.0 + del0);
aodp = (real)(a0 / (1.0 - del0));
perigee = (aodp * (1.0 - eo) - AE) * XKMPER;
apogee = (aodp * (1.0 + eo) - AE) * XKMPER;
period = (TWOPI * 1440.0 / XMNPDA) / xnodp;
/*
printf("Perigee = %lf km period = %lf min del0 = %e\n",
perigee, period, del0);
*/
if (perigee <= 0.0)
{
fprintf(stderr, "# Satellite %ld sub-orbital (apogee = %.1f km, perigee = %.1f km)\n", Isat, apogee, perigee);
}
if (imode == SGDP4_ZERO_ECC) return imode;
if (period >= 225.0 && Set_LS_zero < 2)
{
imode = SGDP4_DEEP_NORM; /* Deep-Space model(s). */
}
else if (perigee < 220.0)
{
/*
For perigee less than 220 km the imode flag is set so the
equations are truncated to linear variation in sqrt A and
quadratic variation in mean anomaly. Also the c3 term, the
delta omega term and the delta m term are dropped.
*/
imode = SGDP4_NEAR_SIMP; /* Near-space, simplified equations. */
}
else
{
imode = SGDP4_NEAR_NORM; /* Near-space, normal equations. */
}
/* For perigee below 156 km the values of S and QOMS2T are altered */
if (perigee < 156.0)
{
s4 = (real)(perigee - 78.0);
if(s4 < (real)20.0)
{
fprintf(stderr, "# Very low s4 constant for sat %ld (perigee = %.2f)\n", Isat, perigee);
s4 = (real)20.0;
}
else
{
fprintf(stderr, "# Changing s4 constant for sat %ld (perigee = %.2f)\n", Isat, perigee);
}
qoms24 = POW4((real)((120.0 - s4) * (AE / XKMPER)));
s4 = (real)(s4 / XKMPER + AE);
}
else
{
s4 = KS;
qoms24 = QOMS2T;
}
pinvsq = (real)1.0 / (aodp * aodp * betao2 * betao2);
tsi = (real)1.0 / (aodp - s4);
eta = aodp * eo * tsi;
etasq = eta * eta;
eeta = eo * eta;
psisq = FABS((real)1.0 - etasq);
coef = qoms24 * POW4(tsi);
coef1 = coef / POW(psisq, 3.5);
c2 = coef1 * (real)xnodp * (aodp *
((real)1.0 + (real)1.5 * etasq + eeta * ((real)4.0 + etasq)) +
(real)(0.75 * CK2) * tsi / psisq * x3thm1 *
((real)8.0 + (real)3.0 * etasq * ((real)8.0 + etasq)));
c1 = bstar * c2;
c4 = (real)2.0 * (real)xnodp * coef1 * aodp * betao2 * (eta *
((real)2.0 + (real)0.5 * etasq) + eo * ((real)0.5 + (real)2.0 *
etasq) - (real)(2.0 * CK2) * tsi / (aodp * psisq) * ((real)-3.0 *
x3thm1 * ((real)1.0 - (real)2.0 * eeta + etasq *
((real)1.5 - (real)0.5 * eeta)) + (real)0.75 * x1mth2 * ((real)2.0 *
etasq - eeta * ((real)1.0 + etasq)) * COS((real)2.0 * omegao)));
c5 = c3 = omgcof = (real)0.0;
if (imode == SGDP4_NEAR_NORM)
{
/* BSTAR drag terms for normal near-space 'normal' model only. */
c5 = (real)2.0 * coef1 * aodp * betao2 *
((real)1.0 + (real)2.75 * (etasq + eeta) + eeta * etasq);
if(eo > ECC_ALL)
{
c3 = coef * tsi * a3ovk2 * (real)xnodp * (real)AE * sinIO / eo;
}
omgcof = bstar * c3 * COS(omegao);
}
temp1 = (real)(3.0 * CK2) * pinvsq * (real)xnodp;
temp2 = temp1 * CK2 * pinvsq;
temp3 = (real)(1.25 * CK4) * pinvsq * pinvsq * (real)xnodp;
xmdot = xnodp + ((real)0.5 * temp1 * betao * x3thm1 + (real)0.0625 *
temp2 * betao * ((real)13.0 - (real)78.0 * theta2 +
(real)137.0 * theta4));
x1m5th = (real)1.0 - (real)5.0 * theta2;
omgdot = (real)-0.5 * temp1 * x1m5th + (real)0.0625 * temp2 *
((real)7.0 - (real)114.0 * theta2 + (real)395.0 * theta4) +
temp3 * ((real)3.0 - (real)36.0 * theta2 + (real)49.0 * theta4);
xhdot1 = -temp1 * cosIO;
xnodot = xhdot1 + ((real)0.5 * temp2 * ((real)4.0 - (real)19.0 * theta2) +
(real)2.0 * temp3 * ((real)3.0 - (real)7.0 * theta2)) * cosIO;
xmcof = (real)0.0;
if(eo > ECC_ALL)
{
xmcof = (real)(-TOTHRD * AE) * coef * bstar / eeta;
}
xnodcf = (real)3.5 * betao2 * xhdot1 * c1;
t2cof = (real)1.5 * c1;
/* Check for possible divide-by-zero for X/(1+cosIO) when calculating xlcof */
temp0 = (real)1.0 + cosIO;
if(fabs(temp0) < EPS_COSIO) temp0 = (real)SIGN(EPS_COSIO, temp0);
xlcof = (real)0.125 * a3ovk2 * sinIO *
((real)3.0 + (real)5.0 * cosIO) / temp0;
aycof = (real)0.25 * a3ovk2 * sinIO;
SINCOS(xmo, &sinXMO, &cosXMO);
delmo = CUBE((real)1.0 + eta * cosXMO);
if (imode == SGDP4_NEAR_NORM)
{
c1sq = c1 * c1;
d2 = (real)4.0 * aodp * tsi * c1sq;
temp0 = d2 * tsi * c1 / (real)3.0;
d3 = ((real)17.0 * aodp + s4) * temp0;
d4 = (real)0.5 * temp0 * aodp * tsi * ((real)221.0 * aodp +
(real)31.0 * s4) * c1;
t3cof = d2 + (real)2.0 * c1sq;
t4cof = (real)0.25 * ((real)3.0 * d3 + c1 * ((real)12.0 * d2 +
(real)10.0 * c1sq));
t5cof = (real)0.2 * ((real)3.0 * d4 + (real)12.0 * c1 * d3 +
(real)6.0 * d2 * d2 + (real)15.0 * c1sq * ((real)2.0 *
d2 + c1sq));
}
else if (imode == SGDP4_DEEP_NORM)
{
#ifdef NO_DEEP_SPACE
fatal_error("init_sgdp4: Deep space equations not supported");
#else
imode = SGDP4_dpinit(epoch, omegao, xnodeo, xmo, eo, xincl,
aodp, xmdot, omgdot, xnodot, xnodp);
#endif /* !NO_DEEP_SPACE */
}
return imode;
}
/* =======================================================================
The sgdp4() function computes the Keplarian elements that describe the
position and velocity of the satellite. Depending on the initialisation
(and the compile options) the deep-space perturbations are also included
allowing sensible predictions for most satellites. These output elements
can be transformed to Earth Centered Inertial coordinates (X-Y-Z) and/or
to sub-satellite latitude and longitude as required. The terms for the
velocity solution are often not required so the 'withvel' flag can be used
to by-pass that step as required. This function is normally called through
another since the input 'tsince' is the time from epoch.
Calling arguments:
tsince : Input, time from epoch (minutes).
withvel : Input, non-zero if velocity terms required.
kep : Output, the Keplarian position / velocity of the satellite.
The return value indicates the orbital mode used.
======================================================================= */
int sgdp4(double tsince, int withvel, kep_t *kep)
{
LOCAL_REAL rk, uk, xnodek, xinck, em, xinc;
LOCAL_REAL xnode, delm, axn, ayn, omega;
LOCAL_REAL capu, epw, elsq, invR, beta2, betal;
LOCAL_REAL sinu, sin2u, cosu, cos2u;
LOCAL_REAL a, e, r, u, pl;
LOCAL_REAL sinEPW, cosEPW, sinOMG, cosOMG;
LOCAL_DOUBLE xmp, xl, xlt;
const int MAXI = 10;
#ifndef NO_DEEP_SPACE
LOCAL_DOUBLE xn, xmam;
#endif /* !NO_DEEP_SPACE */
real esinE, ecosE, maxnr;
real temp0, temp1, temp2, temp3;
real tempa, tempe, templ;
int ii;
#ifdef SGDP4_SNGL
real ts = (real)tsince;
#else
#define ts tsince
#endif /* ! SGDP4_SNGL */
/* Update for secular gravity and atmospheric drag. */
em = eo;
xinc = xincl;
xmp = (double)xmo + xmdot * tsince;
xnode = xnodeo + ts * (xnodot + ts * xnodcf);
omega = omegao + omgdot * ts;
switch(imode)
{
case SGDP4_ZERO_ECC:
/* Not a "real" orbit but OK for fast computation searches. */
kep->smjaxs = kep->radius = (double)aodp * XKMPER/AE;
kep->theta = fmod(PI + xnodp * tsince, TWOPI) - PI;
kep->eqinc = (double)xincl;
kep->ascn = xnodeo;
kep->argp = 0;
kep->ecc = 0;
kep->rfdotk = 0;
if(withvel)
kep->rfdotk = aodp * xnodp * (XKMPER/AE*XMNPDA/86400.0); /* For km/sec */
else
kep->rfdotk = 0;
return imode;
case SGDP4_NEAR_SIMP:
tempa = (real)1.0 - ts * c1;
tempe = bstar * ts * c4;
templ = ts * ts * t2cof;
a = aodp * tempa * tempa;
e = em - tempe;
xl = xmp + omega + xnode + xnodp * templ;
break;
case SGDP4_NEAR_NORM:
delm = xmcof * (CUBE((real)1.0 + eta * COS(xmp)) - delmo);
temp0 = ts * omgcof + delm;
xmp += (double)temp0;
omega -= temp0;
tempa = (real)1.0 - (ts * (c1 + ts * (d2 + ts * (d3 + ts * d4))));
tempe = bstar * (c4 * ts + c5 * (SIN(xmp) - sinXMO));
templ = ts * ts * (t2cof + ts * (t3cof + ts * (t4cof + ts * t5cof)));
//xmp += (double)temp0;
a = aodp * tempa * tempa;
e = em - tempe;
xl = xmp + omega + xnode + xnodp * templ;
break;
#ifndef NO_DEEP_SPACE
case SGDP4_DEEP_NORM:
case SGDP4_DEEP_RESN:
case SGDP4_DEEP_SYNC:
tempa = (real)1.0 - ts * c1;
tempe = bstar * ts * c4;
templ = ts * ts * t2cof;
xn = xnodp;
SGDP4_dpsec(&xmp, &omega, &xnode, &em, &xinc, &xn, tsince);
a = POW(XKE / xn, TOTHRD) * tempa * tempa;
e = em - tempe;
xmam = xmp + xnodp * templ;
SGDP4_dpper(&e, &xinc, &omega, &xnode, &xmam, tsince);
if (xinc < (real)0.0)
{
xinc = (-xinc);
xnode += (real)PI;
omega -= (real)PI;
}
xl = xmam + omega + xnode;
/* Re-compute the perturbed values. */
SINCOS(xinc, &sinIO, &cosIO);
{
real theta2 = cosIO * cosIO;
x3thm1 = (real)3.0 * theta2 - (real)1.0;
x1mth2 = (real)1.0 - theta2;
x7thm1 = (real)7.0 * theta2 - (real)1.0;
/* Check for possible divide-by-zero for X/(1+cosIO) when calculating xlcof */
temp0 = (real)1.0 + cosIO;
if(fabs(temp0) < EPS_COSIO) temp0 = (real)SIGN(EPS_COSIO, temp0);
xlcof = (real)0.125 * a3ovk2 * sinIO *
((real)3.0 + (real)5.0 * cosIO) / temp0;
aycof = (real)0.25 * a3ovk2 * sinIO;
}
break;
#endif /* ! NO_DEEP_SPACE */
default:
fatal_error("sgdp4: Orbit not initialised");
return SGDP4_ERROR;
}
if(a < (real)1.0)
{
fprintf(stderr, "sgdp4: Satellite %05ld crashed at %.3f (a = %.3f Earth radii)\n", Isat, ts, a);
return SGDP4_ERROR;
}
if(e < ECC_LIMIT_LOW)
{
fprintf(stderr, "sgdp4: Satellite %05ld modified eccentricity too low (ts = %.3f, e = %e < %e)\n", Isat, ts, e, ECC_LIMIT_LOW);
return SGDP4_ERROR;
}
if(e < ECC_EPS)
{
/*fprintf(stderr, "# ecc %f at %.3f for for %05ld\n", e, ts, Isat);*/
e = ECC_EPS;
}
else if(e > ECC_LIMIT_HIGH)
{
/*fprintf(stderr, "# ecc %f at %.3f for for %05ld\n", e, ts, Isat);*/
e = ECC_LIMIT_HIGH;
}
beta2 = (real)1.0 - e * e;
/* Long period periodics */
SINCOS(omega, &sinOMG, &cosOMG);
temp0 = (real)1.0 / (a * beta2);
axn = e * cosOMG;
ayn = e * sinOMG + temp0 * aycof;
xlt = xl + temp0 * xlcof * axn;
elsq = axn * axn + ayn * ayn;
if (elsq >= (real)1.0)
{
fprintf(stderr, "sgdp4: SQR(e) >= 1 (%.3f at tsince = %.3f for sat %05ld)\n", elsq, tsince, Isat);
return SGDP4_ERROR;
}
/* Sensibility check for N-R correction. */
kep->ecc = sqrt(elsq);
/*
* Solve Kepler's equation using Newton-Raphson root solving. Here 'capu' is
* almost the "Mean anomaly", initialise the "Eccentric Anomaly" term 'epw'.
* The fmod() saves reduction of angle to +/-2pi in SINCOS() and prevents
* convergence problems.
*
* Later modified to support 2nd order NR method which saves roughly 1 iteration
* for only a couple of arithmetic operations.
*/
epw = capu = fmod(xlt - xnode, TWOPI);
maxnr = kep->ecc;
for(ii = 0; ii < MAXI; ii++)
{
double nr, f, df;
SINCOS(epw, &sinEPW, &cosEPW);
ecosE = axn * cosEPW + ayn * sinEPW;
esinE = axn * sinEPW - ayn * cosEPW;
f = capu - epw + esinE;
if (fabs(f) < NR_EPS) break;
df = 1.0 - ecosE;
/* 1st order Newton-Raphson correction. */
nr = f / df;
if (ii == 0 && FABS(nr) > 1.25*maxnr)
nr = SIGN(maxnr, nr);
#if 1
/* 2nd order Newton-Raphson correction. */
else
nr = f / (df + 0.5*esinE*nr); /* f/(df - 0.5*d2f*f/df) */
#endif
epw += nr; /* Newton-Raphson correction of -F/DF. */
//if (fabs(nr) < NR_EPS) break;
}
/* Short period preliminary quantities */
temp0 = (real)1.0 - elsq;
betal = SQRT(temp0);
pl = a * temp0;
r = a * ((real)1.0 - ecosE);
invR = (real)1.0 / r;
temp2 = a * invR;
temp3 = (real)1.0 / ((real)1.0 + betal);
cosu = temp2 * (cosEPW - axn + ayn * esinE * temp3);
sinu = temp2 * (sinEPW - ayn - axn * esinE * temp3);
u = ATAN2(sinu, cosu);
sin2u = (real)2.0 * sinu * cosu;
cos2u = (real)2.0 * cosu * cosu - (real)1.0;
temp0 = (real)1.0 / pl;
temp1 = CK2 * temp0;
temp2 = temp1 * temp0;
/* Update for short term periodics to position terms. */
rk = r * ((real)1.0 - (real)1.5 * temp2 * betal * x3thm1) + (real)0.5 * temp1 * x1mth2 * cos2u;
uk = u - (real)0.25 * temp2 * x7thm1 * sin2u;
xnodek = xnode + (real)1.5 * temp2 * cosIO * sin2u;
xinck = xinc + (real)1.5 * temp2 * cosIO * sinIO * cos2u;
if(rk < (real)1.0)
{
#if 1
fprintf(stderr, "sgdp4: Satellite %05ld crashed at %.3f (rk = %.3f Earth radii)\n", Isat, ts, rk);
#endif
return SGDP4_ERROR;
}
kep->radius = rk * XKMPER/AE; /* Into km */
kep->theta = uk;
kep->eqinc = xinck;
kep->ascn = xnodek;
kep->argp = omega;
kep->smjaxs = a * XKMPER/AE;
/* Short period velocity terms ?. */
if (withvel)
{
/* xn = XKE / pow(a, 1.5); */
temp0 = SQRT(a);
temp2 = (real)XKE / (a * temp0);
kep->rdotk = ((real)XKE * temp0 * esinE * invR -
temp2 * temp1 * x1mth2 * sin2u) *
(XKMPER/AE*XMNPDA/86400.0); /* Into km/sec */
kep->rfdotk = ((real)XKE * SQRT(pl) * invR + temp2 * temp1 *
(x1mth2 * cos2u + (real)1.5 * x3thm1)) *
(XKMPER/AE*XMNPDA/86400.0);
}
else
{
kep->rdotk = kep->rfdotk = 0;
}
#ifndef SGDP4_SNGL
#undef ts
#endif
return imode;
}
/* ====================================================================
Transformation from "Kepler" type coordinates to cartesian XYZ form.
Calling arguments:
K : Kepler structure as filled by sgdp4();
pos : XYZ structure for position.
vel : same for velocity.
==================================================================== */
void kep2xyz(kep_t *K, xyz_t *pos, xyz_t *vel)
{
real xmx, xmy;
real ux, uy, uz, vx, vy, vz;
real sinT, cosT, sinI, cosI, sinS, cosS;
/* Orientation vectors for X-Y-Z format. */
SINCOS((real)K->theta, &sinT, &cosT);
SINCOS((real)K->eqinc, &sinI, &cosI);
SINCOS((real)K->ascn, &sinS, &cosS);
xmx = -sinS * cosI;
xmy = cosS * cosI;
ux = xmx * sinT + cosS * cosT;
uy = xmy * sinT + sinS * cosT;
uz = sinI * sinT;
/* Position and velocity */
if(pos != NULL)
{
pos->x = K->radius * ux;
pos->y = K->radius * uy;
pos->z = K->radius * uz;
}
if(vel != NULL)
{
vx = xmx * cosT - cosS * sinT;
vy = xmy * cosT - sinS * sinT;
vz = sinI * cosT;
vel->x = K->rdotk * ux + K->rfdotk * vx;
vel->y = K->rdotk * uy + K->rfdotk * vy;
vel->z = K->rdotk * uz + K->rfdotk * vz;
}
}
/* ======================================================================
Compute the satellite position and/or velocity for a given time (in the
form of Julian day number.)
Calling arguments are:
jd : Time as Julian day number.
pos : Pointer to posiition vector, km (NULL if not required).
vel : Pointer to velocity vector, km/sec (NULL if not required).
====================================================================== */
int satpos_xyz(double jd, xyz_t *pos, xyz_t *vel)
{
kep_t K;
int withvel, rv;
double tsince;
tsince = (jd - SGDP4_jd0) * XMNPDA;
#ifdef DEBUG
fprintf(stderr, "Tsince = %f\n", tsince);
#endif
if(vel != NULL)
withvel = 1;
else
withvel = 0;
rv = sgdp4(tsince, withvel, &K);
kep2xyz(&K, pos, vel);
return rv;
}
/* ==================== End of file sgdp4.c ========================== */

362
sgdp4h.h 100644
View File

@ -0,0 +1,362 @@
/* > sgdp4h.h
*
*
* Paul S. Crawford and Andrew R. Brooks
* Dundee University
*
* NOTE !
* This code is supplied "as is" and without warranty of any sort.
*
* (c) 1994-2004, Paul Crawford, Andrew Brooks
*
*
* 2.00 psc Sun May 28 1995 - Modifed for non-Dundee use.
*
*/
#ifndef _SGDP4H_H
#define _SGDP4H_H
/*
* Set up standard system-dependent names UNIX, LINUX, RISCOS, MSDOS, WIN32
*/
#if defined( unix )
# define UNIX
# if defined( linux ) && !defined( LINUX )
# define LINUX
# endif
#elif defined( __riscos ) && !defined( RISCOS )
# define RISCOS
#elif !defined( MSDOS ) && !defined( WIN32 ) && !defined( __CYGWIN__ )
# define MSDOS
#endif
/*
* Include files
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <memory.h>
#include <time.h>
#include <sys/types.h>
#ifdef UNIX
#include <unistd.h>
#endif
#ifdef SUN4
#include <memory.h>
#endif
#ifdef sun
#include <sys/time.h> /* solaris 7 has struct timeval in here */
#include <sunmath.h> /* for sincos() which is in libsunmath */
#endif
#ifdef linux
#include <stdint.h>
void sincos(double x, double *s, double *c); /* declared where? */
#endif
/*
* ================= SYSTEM SPECIFIC DEFINITIONS =====================
*/
/* Use INLINE keyword when declaring inline functions */
#ifdef WIN32
#define INLINE __inline
#elif defined( MSDOS )
#define INLINE
#else
/*UNIX?*/
#define INLINE inline
#endif
/* Sun C compiler has automatic inline and doesn't understand inline keyword */
#ifdef __SUNPRO_C
#undef INLINE
#define INLINE
#define MACROS_ARE_SAFE
#endif
/* Some very common constants. */
#ifndef M_PI
#define M_PI 3.141592653589793
#endif /* MSDOS */
#ifndef PI
#define PI M_PI
#endif
#define TWOPI (2.0*PI) /* Optimising compiler will deal with this! */
#define PB2 (0.5*PI)
#define PI180 (PI/180.0)
#define SOLAR_DAY (1440.0) /* Minutes per 24 hours */
#define SIDERIAL_DAY (23.0*60.0 + 56.0 + 4.09054/60.0) /* Against stars */
#define EQRAD (6378.137) /* Earth radius at equator, km */
#define LATCON (1.0/298.257) /* Latitude radius constant */
#define ECON ((1.0-LATCON)*(1.0-LATCON))
#define JD1900 2415020.5 /* Julian day number for Jan 1st, 00:00 hours 1900 */
/*
* =============================== MACROS ============================
*
*
* Define macro for sign transfer, double to nearest (long) integer,
* to square an expression (not nested), and A "safe" square, uses test
* to force correct sequence of evaluation when the macro is nested.
*/
/*
* These macros are safe since they make no assignments.
*/
#define SIGN(a, b) ((b) >= 0 ? fabs(a) : -fabs(a))
/* Coordinate conversion macros */
#define DEG(x) ((x)/PI180)
#define RAD(x) ((x)*PI180)
#define GEOC(x) (atan(ECON*tan(x))) /* Geographic to geocentric. */
#define GEOG(x) (atan(tan(x)/ECON))
/*
* All other compilers can have static inline functions.
* (SQR is used badly here: do_cal.c, glat2lat.c, satpos.c, vmath.h).
*/
static INLINE int NINT(double a) { return (int)(a > 0 ? a+0.5 : a-0.5); }
static INLINE long NLONG(double a) { return (long)(a > 0 ? a+0.5 : a-0.5); }
static INLINE double DSQR(double a) { return(a*a); }
static INLINE float FSQR(float a) { return(a*a); }
static INLINE int ISQR(int a) { return(a*a); }
static INLINE double DCUBE(double a) { return(a*a*a); }
static INLINE float FCUBE(float a) { return(a*a*a); }
static INLINE int ICUBE(int a) { return(a*a*a); }
static INLINE double DPOW4(double a) { a*=a; return(a*a); }
static INLINE float FPOW4(float a) { a*=a; return(a*a); }
static INLINE int IPOW4(int a) { a*=a; return(a*a); }
static INLINE double DMAX(double a,double b) { if (a>b) return a; else return b; }
static INLINE float FMAX(float a, float b) { if (a>b) return a; else return b; }
static INLINE int IMAX(int a, int b) { if (a>b) return a; else return b; }
static INLINE double DMIN(double a,double b) { if (a<b) return a; else return b; }
static INLINE float FMIN(float a, float b) { if (a<b) return a; else return b; }
static INLINE int IMIN(int a, int b) { if (a<b) return a; else return b; }
static INLINE double MOD2PI(double a) { a=fmod(a, TWOPI); return a < 0.0 ? a+TWOPI : a; }
static INLINE double MOD360(double a) { a=fmod(a, 360.0); return a < 0.0 ? a+360.0 : a; }
/*
* Unless you have higher than default optimisation the Sun compiler
* would prefer to be told explicitly about inline functions after their
* declaration.
*/
#if defined(__SUNPRO_C) && !defined(MACROS_ARE_SAFE)
#pragma inline_routines(NINT, NLONG, DSQR, FSQR, ISQR, DCUBE, FCUBE, ICUBE, DPOW4, FPOW4, IPOW4)
#pragma inline_routines(DMAX, FMAX, IMAX, DMIN, FMIN, IMIN, MOD2PI, MOD360, S_GEOC, S_GEOG)
#endif
/* ==================================================================== */
typedef struct orbit_s
{
/* Add the epoch time if required. */
int ep_year;/* Year of epoch, e.g. 94 for 1994, 100 for 2000AD */
double ep_day; /* Day of epoch from 00:00 Jan 1st ( = 1.0 ) */
double rev; /* Mean motion, revolutions per day */
double bstar; /* Drag term .*/
double eqinc; /* Equatorial inclination, radians */
double ecc; /* Eccentricity */
double mnan; /* Mean anomaly at epoch from elements, radians */
double argp; /* Argument of perigee, radians */
double ascn; /* Right ascension (ascending node), radians */
double smjaxs; /* Semi-major axis, km */
double ndot2,nddot6; /* Mean motion derivatives */
char desig[10]; /* International designation */
long norb; /* Orbit number, for elements */
int satno; /* Satellite number. */
} orbit_t;
typedef struct xyz_s
{
double x;
double y;
double z;
} xyz_t;
typedef struct kep_s
{
double theta; /* Angle "theta" from equatorial plane (rad) = U. */
double ascn; /* Right ascension (rad). */
double eqinc; /* Equatorial inclination (rad). */
double radius; /* Radius (km). */
double rdotk;
double rfdotk;
/*
* Following are without short-term perturbations but used to
* speed searchs.
*/
double argp; /* Argument of perigee at 'tsince' (rad). */
double smjaxs; /* Semi-major axis at 'tsince' (km). */
double ecc; /* Eccentricity at 'tsince'. */
} kep_t;
/* ================ Single or Double precision options. ================= */
#define DEFAULT_TO_SNGL 0
#if defined( SGDP4_SNGL ) || (DEFAULT_TO_SNGL && !defined( SGDP4_DBLE ))
/* Single precision option. */
typedef float real;
#ifndef SGDP4_SNGL
#define SGDP4_SNGL
#endif
#else
/* Double precision option. */
typedef double real;
#ifndef SGDP4_DBLE
#define SGDP4_DBLE
#endif
#endif /* Single or double choice. */
/* Something silly ? */
#if defined( SGDP4_SNGL ) && defined( SGDP4_DBLE )
#error sgdp4h.h - Cannot have both single and double precision defined
#endif
/* =========== Do we have sincos() functions available or not ? ======= */
/*
We can use the normal ANSI 'C' library functions in sincos() macros, but if
we have sincos() functions they are much faster (25% under some tests). For
DOS programs we use our assembly language functions using the 80387 (and
higher) coprocessor FSINCOS instruction:
void sincos(double x, double *s, double *c);
void sincosf(float x, float *s, float *c);
For the Sun 'C' compiler there is only the system supplied double precision
version of these functions.
*/
#ifdef MACRO_SINCOS
#define sincos(x,s,c) {double sc__tmp=(x);\
*(s)=sin(sc__tmp);\
*(c)=cos(sc__tmp);}
#define SINCOS(x,s,c) {double sc__tmp=(double)(x);\
*(s)=(real)sin(sc__tmp);\
*(c)=(real)cos(sc__tmp);}
#elif !defined( sun )
/* For Microsoft C6.0 compiler, etc. */
#ifdef SGDP4_SNGL
#define SINCOS sincosf
#else
#define SINCOS sincos
#endif /* ! SGDP4_SNGL */
void sincos(double, double *, double *);
void sincosf(float, float *, float *);
#else
/* Sun 'C' compiler. */
#ifdef SGDP4_SNGL
/* Use double function and cast results to single precision. */
#define SINCOS(x,s,c) {double s__tmp, c__tmp;\
sincos((double)(x), &s__tmp, &c__tmp);\
*(s)=(real)s__tmp;\
*(c)=(real)c__tmp);}
#else
#define SINCOS sincos
#endif /* ! SGDP4_SNGL */
#endif /* ! MACRO_SINCOS */
/* ================= Stack space problems ? ======================== */
#if !defined( MSDOS )
/* Automatic variables, faster (?) but needs more stack space. */
#define LOCAL_REAL real
#define LOCAL_DOUBLE double
#else
/* Static variables, slower (?) but little stack space. */
#define LOCAL_REAL static real
#define LOCAL_DOUBLE static double
#endif
/* ======== Macro fixes for float/double in math.h type functions. ===== */
#define SIN(x) (real)sin((double)(x))
#define COS(x) (real)cos((double)(x))
#define SQRT(x) (real)sqrt((double)(x))
#define FABS(x) (real)fabs((double)(x))
#define POW(x,y) (real)pow((double)(x), (double)(y))
#define FMOD(x,y) (real)fmod((double)(x), (double)(y))
#define ATAN2(x,y) (real)atan2((double)(x), (double)(y))
#ifdef SGDP4_SNGL
#define CUBE FCUBE
#define POW4 FPOW4
#else
#define CUBE DCUBE
#define POW4 DPOW4
#endif
/* SGDP4 function return values. */
#define SGDP4_ERROR (-1)
#define SGDP4_NOT_INIT 0
#define SGDP4_ZERO_ECC 1
#define SGDP4_NEAR_SIMP 2
#define SGDP4_NEAR_NORM 3
#define SGDP4_DEEP_NORM 4
#define SGDP4_DEEP_RESN 5
#define SGDP4_DEEP_SYNC 6
#include "satutl.h"
/* ======================= Function prototypes ====================== */
#ifdef __cplusplus
extern "C" {
#endif
/** deep.c **/
int SGDP4_dpinit(double epoch, real omegao, real xnodeo, real xmo,
real orb_eo, real orb_xincl, real aodp, double xmdot,
real omgdot, real xnodot, double xnodp);
int SGDP4_dpsec(double *xll, real *omgasm, real *xnodes, real *em,
real *xinc, double *xn, double tsince);
int SGDP4_dpper(real *em, real *xinc, real *omgasm, real *xnodes,
double *xll, double tsince);
/** sgdp4.c **/
int init_sgdp4(orbit_t *orb);
int sgdp4(double tsince, int withvel, kep_t *kep);
void kep2xyz(kep_t *K, xyz_t *pos, xyz_t *vel);
int satpos_xyz(double jd, xyz_t *pos, xyz_t *vel);
#ifdef __cplusplus
}
#endif
#endif /* !_SGDP4H_H */