From 1b775aca7c4fb994760e75107731fff41439cb69 Mon Sep 17 00:00:00 2001 From: Cees Bassa Date: Sun, 23 Mar 2014 16:50:23 +0100 Subject: [PATCH] SGDP4 routines --- deep.c | 924 ++++++++++++++++++++++++++++++++++++++++++++++++++++ ferror.c | 29 ++ rfoverlay.c | 225 +++++++++++++ rfoverlay.h | 1 + satutl.c | 230 +++++++++++++ satutl.h | 30 ++ sgdp4.c | 828 ++++++++++++++++++++++++++++++++++++++++++++++ sgdp4h.h | 362 ++++++++++++++++++++ 8 files changed, 2629 insertions(+) create mode 100644 deep.c create mode 100644 ferror.c create mode 100644 rfoverlay.c create mode 100644 rfoverlay.h create mode 100644 satutl.c create mode 100644 satutl.h create mode 100644 sgdp4.c create mode 100644 sgdp4h.h 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