diff --git a/deep.c b/deep.c new file mode 100644 index 0000000..fd8ba57 --- /dev/null +++ b/deep.c @@ -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 +#include +#include + +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 */ diff --git a/ferror.c b/ferror.c new file mode 100644 index 0000000..05090bf --- /dev/null +++ b/ferror.c @@ -0,0 +1,29 @@ +/* > satutl.c + * + */ + + +#include "sgdp4h.h" + +#include + +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); + +} + +/* ===================================================================== */ diff --git a/rfoverlay.c b/rfoverlay.c new file mode 100644 index 0000000..5b8e1af --- /dev/null +++ b/rfoverlay.c @@ -0,0 +1,225 @@ +#include +#include +#include +#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;i0) { + 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;i90.0 && flag==1) { + if (tflag==0) { + tflag=1; + cpgtext((float) i,(float) freq,text); + } + flag=0; + } + } + } + fclose(file); + } + fclose(infile); + + // Free + free(p); + + return; +} diff --git a/rfoverlay.h b/rfoverlay.h new file mode 100644 index 0000000..f5c095f --- /dev/null +++ b/rfoverlay.h @@ -0,0 +1 @@ +void overlay(double *mjd,int n,int site_id); diff --git a/satutl.c b/satutl.c new file mode 100644 index 0000000..1946712 --- /dev/null +++ b/satutl.c @@ -0,0 +1,230 @@ +/* > satutl.c + * + */ + + +#include "sgdp4h.h" + +#include + +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); +} + +/* ====================================================================== */ diff --git a/satutl.h b/satutl.h new file mode 100644 index 0000000..04e1969 --- /dev/null +++ b/satutl.h @@ -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 */ diff --git a/sgdp4.c b/sgdp4.c new file mode 100644 index 0000000..e747250 --- /dev/null +++ b/sgdp4.c @@ -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 +#include +#include + +/* ================ 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 ========================== */ diff --git a/sgdp4h.h b/sgdp4h.h new file mode 100644 index 0000000..d12b7ee --- /dev/null +++ b/sgdp4h.h @@ -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 +#include +#include +#include +#include +#include +#include +#ifdef UNIX +#include +#endif + +#ifdef SUN4 +#include +#endif + +#ifdef sun +#include /* solaris 7 has struct timeval in here */ +#include /* for sincos() which is in libsunmath */ +#endif + +#ifdef linux +#include +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