spacecruft
/
sattools
1
0
Fork 0
Code Releases Activity
sattools/src/sgdp4.c

888 lines
26 KiB
C
Raw Permalink Blame History

/* > 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 Propagation of NORAD Element Sets"
* Edited and subsequently distributed by Dr. T. S. Kelso.
*
************************************************************************
*
* This conversion by:
* (c) Paul Crawford & Andrew Brooks 1994-2019
* University of Dundee
* psc (at) sat.dundee.ac.uk
* arb (at) sat.dundee.ac.uk
*
* Released under the terms of the GNU LGPL V3
* http://www.gnu.org/licenses/lgpl-3.0.html
*
* This software 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.
*
************************************************************************
*
* 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 tweaks and tidied up the
* documentation for more general use.
*
* 3.00 psc Mon May 29 1995 - Cleaned up for general use & distribution (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.
*
* 3.04 psc Sat 15 Jun 2019 - Better 'float' support and minor spelling corrections.
*
*/
static const char SCCSid[] =
"@(#)sgdp4.c 3.04 (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. */
#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 coefficient 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 sgdp4_mode_t 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. */
static double perigee, period, apogee;
int Set_LS_zero = 0; /* Set to 1 to zero Lunar-Solar terms at epoch. */
/* =======================================================================
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 determination 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 meaningful
but such a circular orbit is not either! It is fast though.
Calling 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.
======================================================================= */
sgdp4_mode_t
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 < 1960)
{
/* Assume 0 and 100 both refer to 2000AD */
iyear += (iyear < 60 ? 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
fprintf (stderr, "init_sgdp4: Deep space equations not supported\n");
imode = SGDP4_NEAR_NORM; /* Force operations even if wrong model? */
#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 Centred 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.
======================================================================= */
sgdp4_mode_t
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++)
{
real 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 = (real) 1.0 - ecosE;
/* 1st order Newton-Raphson correction. */
nr = f / df;
/*Sanity check for high eccentricity failure case. */
if (ii == 0 && FABS (nr) > (real) 1.25 * maxnr)
nr = SIGN (maxnr, nr);
#if 1
/* 2nd order Newton-Raphson correction. */
else
nr = f / (df + (real) 0.5 * esinE * nr); /* f/(df - 0.5*d2f*f/df) */
#endif
epw += nr; /* Newton-Raphson correction of -F/DF. */
}
/* 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 position vector, km (NULL if not required).
vel : Pointer to velocity vector, km/sec (NULL if not required).
====================================================================== */
sgdp4_mode_t
satpos_xyz (double jd, xyz_t * pos, xyz_t * vel)
{
kep_t K;
int withvel;
sgdp4_mode_t rv;
double tsince;
tsince = (jd - SGDP4_jd0) * XMNPDA;
#if defined( DEBUG ) && 0
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 ========================== */
View Git Blame Copy Permalink