Grady Hillhouse / SGP4_vallado

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

sgp4unit.cpp@0:806c1dd6946c, 2015-08-11 (annotated)

Committer:
gradyhillhouse
Date:
Tue Aug 11 22:42:13 2015 +0000
Revision:
0:806c1dd6946c
Port of SGP4 library by Vallado

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gradyhillhouse 0:806c1dd6946c 1 /* ----------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 2 *
gradyhillhouse 0:806c1dd6946c 3 * sgp4unit.cpp
gradyhillhouse 0:806c1dd6946c 4 *
gradyhillhouse 0:806c1dd6946c 5 * this file contains the sgp4 procedures for analytical propagation
gradyhillhouse 0:806c1dd6946c 6 * of a satellite. the code was originally released in the 1980 and 1986
gradyhillhouse 0:806c1dd6946c 7 * spacetrack papers. a detailed discussion of the theory and history
gradyhillhouse 0:806c1dd6946c 8 * may be found in the 2006 aiaa paper by vallado, crawford, hujsak,
gradyhillhouse 0:806c1dd6946c 9 * and kelso.
gradyhillhouse 0:806c1dd6946c 10 *
gradyhillhouse 0:806c1dd6946c 11 * companion code for
gradyhillhouse 0:806c1dd6946c 12 * fundamentals of astrodynamics and applications
gradyhillhouse 0:806c1dd6946c 13 * 2007
gradyhillhouse 0:806c1dd6946c 14 * by david vallado
gradyhillhouse 0:806c1dd6946c 15 *
gradyhillhouse 0:806c1dd6946c 16 * (w) 719-573-2600, email dvallado@agi.com
gradyhillhouse 0:806c1dd6946c 17 *
gradyhillhouse 0:806c1dd6946c 18 * current :
gradyhillhouse 0:806c1dd6946c 19 * 30 Aug 10 david vallado
gradyhillhouse 0:806c1dd6946c 20 * delete unused variables in initl
gradyhillhouse 0:806c1dd6946c 21 * replace pow inetger 2, 3 with multiplies for speed
gradyhillhouse 0:806c1dd6946c 22 * changes :
gradyhillhouse 0:806c1dd6946c 23 * 3 Nov 08 david vallado
gradyhillhouse 0:806c1dd6946c 24 * put returns in for error codes
gradyhillhouse 0:806c1dd6946c 25 * 29 sep 08 david vallado
gradyhillhouse 0:806c1dd6946c 26 * fix atime for faster operation in dspace
gradyhillhouse 0:806c1dd6946c 27 * add operationmode for afspc (a) or improved (i)
gradyhillhouse 0:806c1dd6946c 28 * performance mode
gradyhillhouse 0:806c1dd6946c 29 * 16 jun 08 david vallado
gradyhillhouse 0:806c1dd6946c 30 * update small eccentricity check
gradyhillhouse 0:806c1dd6946c 31 * 16 nov 07 david vallado
gradyhillhouse 0:806c1dd6946c 32 * misc fixes for better compliance
gradyhillhouse 0:806c1dd6946c 33 * 20 apr 07 david vallado
gradyhillhouse 0:806c1dd6946c 34 * misc fixes for constants
gradyhillhouse 0:806c1dd6946c 35 * 11 aug 06 david vallado
gradyhillhouse 0:806c1dd6946c 36 * chg lyddane choice back to strn3, constants, misc doc
gradyhillhouse 0:806c1dd6946c 37 * 15 dec 05 david vallado
gradyhillhouse 0:806c1dd6946c 38 * misc fixes
gradyhillhouse 0:806c1dd6946c 39 * 26 jul 05 david vallado
gradyhillhouse 0:806c1dd6946c 40 * fixes for paper
gradyhillhouse 0:806c1dd6946c 41 * note that each fix is preceded by a
gradyhillhouse 0:806c1dd6946c 42 * comment with "sgp4fix" and an explanation of
gradyhillhouse 0:806c1dd6946c 43 * what was changed
gradyhillhouse 0:806c1dd6946c 44 * 10 aug 04 david vallado
gradyhillhouse 0:806c1dd6946c 45 * 2nd printing baseline working
gradyhillhouse 0:806c1dd6946c 46 * 14 may 01 david vallado
gradyhillhouse 0:806c1dd6946c 47 * 2nd edition baseline
gradyhillhouse 0:806c1dd6946c 48 * 80 norad
gradyhillhouse 0:806c1dd6946c 49 * original baseline
gradyhillhouse 0:806c1dd6946c 50 * ---------------------------------------------------------------- */
gradyhillhouse 0:806c1dd6946c 51
gradyhillhouse 0:806c1dd6946c 52 #include "sgp4unit.h"
gradyhillhouse 0:806c1dd6946c 53
gradyhillhouse 0:806c1dd6946c 54 //const char help = 'n'; //Not sure what this does. Commented out. - Grady Hillhouse 2015
gradyhillhouse 0:806c1dd6946c 55 //FILE *dbgfile;
gradyhillhouse 0:806c1dd6946c 56
gradyhillhouse 0:806c1dd6946c 57
gradyhillhouse 0:806c1dd6946c 58 /* ----------- local functions - only ever used internally by sgp4 ---------- */
gradyhillhouse 0:806c1dd6946c 59 static void dpper
gradyhillhouse 0:806c1dd6946c 60 (
gradyhillhouse 0:806c1dd6946c 61 double e3, double ee2, double peo, double pgho, double pho,
gradyhillhouse 0:806c1dd6946c 62 double pinco, double plo, double se2, double se3, double sgh2,
gradyhillhouse 0:806c1dd6946c 63 double sgh3, double sgh4, double sh2, double sh3, double si2,
gradyhillhouse 0:806c1dd6946c 64 double si3, double sl2, double sl3, double sl4, double t,
gradyhillhouse 0:806c1dd6946c 65 double xgh2, double xgh3, double xgh4, double xh2, double xh3,
gradyhillhouse 0:806c1dd6946c 66 double xi2, double xi3, double xl2, double xl3, double xl4,
gradyhillhouse 0:806c1dd6946c 67 double zmol, double zmos, double inclo,
gradyhillhouse 0:806c1dd6946c 68 char init,
gradyhillhouse 0:806c1dd6946c 69 double& ep, double& inclp, double& nodep, double& argpp, double& mp,
gradyhillhouse 0:806c1dd6946c 70 char opsmode
gradyhillhouse 0:806c1dd6946c 71 );
gradyhillhouse 0:806c1dd6946c 72
gradyhillhouse 0:806c1dd6946c 73 static void dscom
gradyhillhouse 0:806c1dd6946c 74 (
gradyhillhouse 0:806c1dd6946c 75 double epoch, double ep, double argpp, double tc, double inclp,
gradyhillhouse 0:806c1dd6946c 76 double nodep, double np,
gradyhillhouse 0:806c1dd6946c 77 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm,
gradyhillhouse 0:806c1dd6946c 78 double& cosomm,double& day, double& e3, double& ee2, double& em,
gradyhillhouse 0:806c1dd6946c 79 double& emsq, double& gam, double& peo, double& pgho, double& pho,
gradyhillhouse 0:806c1dd6946c 80 double& pinco, double& plo, double& rtemsq, double& se2, double& se3,
gradyhillhouse 0:806c1dd6946c 81 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3,
gradyhillhouse 0:806c1dd6946c 82 double& si2, double& si3, double& sl2, double& sl3, double& sl4,
gradyhillhouse 0:806c1dd6946c 83 double& s1, double& s2, double& s3, double& s4, double& s5,
gradyhillhouse 0:806c1dd6946c 84 double& s6, double& s7, double& ss1, double& ss2, double& ss3,
gradyhillhouse 0:806c1dd6946c 85 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1,
gradyhillhouse 0:806c1dd6946c 86 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13,
gradyhillhouse 0:806c1dd6946c 87 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32,
gradyhillhouse 0:806c1dd6946c 88 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2,
gradyhillhouse 0:806c1dd6946c 89 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3,
gradyhillhouse 0:806c1dd6946c 90 double& xl4, double& nm, double& z1, double& z2, double& z3,
gradyhillhouse 0:806c1dd6946c 91 double& z11, double& z12, double& z13, double& z21, double& z22,
gradyhillhouse 0:806c1dd6946c 92 double& z23, double& z31, double& z32, double& z33, double& zmol,
gradyhillhouse 0:806c1dd6946c 93 double& zmos
gradyhillhouse 0:806c1dd6946c 94 );
gradyhillhouse 0:806c1dd6946c 95
gradyhillhouse 0:806c1dd6946c 96 static void dsinit
gradyhillhouse 0:806c1dd6946c 97 (
gradyhillhouse 0:806c1dd6946c 98 gravconsttype whichconst,
gradyhillhouse 0:806c1dd6946c 99 double cosim, double emsq, double argpo, double s1, double s2,
gradyhillhouse 0:806c1dd6946c 100 double s3, double s4, double s5, double sinim, double ss1,
gradyhillhouse 0:806c1dd6946c 101 double ss2, double ss3, double ss4, double ss5, double sz1,
gradyhillhouse 0:806c1dd6946c 102 double sz3, double sz11, double sz13, double sz21, double sz23,
gradyhillhouse 0:806c1dd6946c 103 double sz31, double sz33, double t, double tc, double gsto,
gradyhillhouse 0:806c1dd6946c 104 double mo, double mdot, double no, double nodeo, double nodedot,
gradyhillhouse 0:806c1dd6946c 105 double xpidot, double z1, double z3, double z11, double z13,
gradyhillhouse 0:806c1dd6946c 106 double z21, double z23, double z31, double z33, double ecco,
gradyhillhouse 0:806c1dd6946c 107 double eccsq, double& em, double& argpm, double& inclm, double& mm,
gradyhillhouse 0:806c1dd6946c 108 double& nm, double& nodem,
gradyhillhouse 0:806c1dd6946c 109 int& irez,
gradyhillhouse 0:806c1dd6946c 110 double& atime, double& d2201, double& d2211, double& d3210, double& d3222,
gradyhillhouse 0:806c1dd6946c 111 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421,
gradyhillhouse 0:806c1dd6946c 112 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt,
gradyhillhouse 0:806c1dd6946c 113 double& dnodt, double& domdt, double& del1, double& del2, double& del3,
gradyhillhouse 0:806c1dd6946c 114 double& xfact, double& xlamo, double& xli, double& xni
gradyhillhouse 0:806c1dd6946c 115 );
gradyhillhouse 0:806c1dd6946c 116
gradyhillhouse 0:806c1dd6946c 117 static void dspace
gradyhillhouse 0:806c1dd6946c 118 (
gradyhillhouse 0:806c1dd6946c 119 int irez,
gradyhillhouse 0:806c1dd6946c 120 double d2201, double d2211, double d3210, double d3222, double d4410,
gradyhillhouse 0:806c1dd6946c 121 double d4422, double d5220, double d5232, double d5421, double d5433,
gradyhillhouse 0:806c1dd6946c 122 double dedt, double del1, double del2, double del3, double didt,
gradyhillhouse 0:806c1dd6946c 123 double dmdt, double dnodt, double domdt, double argpo, double argpdot,
gradyhillhouse 0:806c1dd6946c 124 double t, double tc, double gsto, double xfact, double xlamo,
gradyhillhouse 0:806c1dd6946c 125 double no,
gradyhillhouse 0:806c1dd6946c 126 double& atime, double& em, double& argpm, double& inclm, double& xli,
gradyhillhouse 0:806c1dd6946c 127 double& mm, double& xni, double& nodem, double& dndt, double& nm
gradyhillhouse 0:806c1dd6946c 128 );
gradyhillhouse 0:806c1dd6946c 129
gradyhillhouse 0:806c1dd6946c 130 static void initl
gradyhillhouse 0:806c1dd6946c 131 (
gradyhillhouse 0:806c1dd6946c 132 int satn, gravconsttype whichconst,
gradyhillhouse 0:806c1dd6946c 133 double ecco, double epoch, double inclo, double& no,
gradyhillhouse 0:806c1dd6946c 134 char& method,
gradyhillhouse 0:806c1dd6946c 135 double& ainv, double& ao, double& con41, double& con42, double& cosio,
gradyhillhouse 0:806c1dd6946c 136 double& cosio2,double& eccsq, double& omeosq, double& posq,
gradyhillhouse 0:806c1dd6946c 137 double& rp, double& rteosq,double& sinio , double& gsto, char opsmode
gradyhillhouse 0:806c1dd6946c 138 );
gradyhillhouse 0:806c1dd6946c 139
gradyhillhouse 0:806c1dd6946c 140 /* -----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 141 *
gradyhillhouse 0:806c1dd6946c 142 * procedure dpper
gradyhillhouse 0:806c1dd6946c 143 *
gradyhillhouse 0:806c1dd6946c 144 * this procedure provides deep space long period periodic contributions
gradyhillhouse 0:806c1dd6946c 145 * to the mean elements. by design, these periodics are zero at epoch.
gradyhillhouse 0:806c1dd6946c 146 * this used to be dscom which included initialization, but it's really a
gradyhillhouse 0:806c1dd6946c 147 * recurring function.
gradyhillhouse 0:806c1dd6946c 148 *
gradyhillhouse 0:806c1dd6946c 149 * author : david vallado 719-573-2600 28 jun 2005
gradyhillhouse 0:806c1dd6946c 150 *
gradyhillhouse 0:806c1dd6946c 151 * inputs :
gradyhillhouse 0:806c1dd6946c 152 * e3 -
gradyhillhouse 0:806c1dd6946c 153 * ee2 -
gradyhillhouse 0:806c1dd6946c 154 * peo -
gradyhillhouse 0:806c1dd6946c 155 * pgho -
gradyhillhouse 0:806c1dd6946c 156 * pho -
gradyhillhouse 0:806c1dd6946c 157 * pinco -
gradyhillhouse 0:806c1dd6946c 158 * plo -
gradyhillhouse 0:806c1dd6946c 159 * se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 -
gradyhillhouse 0:806c1dd6946c 160 * t -
gradyhillhouse 0:806c1dd6946c 161 * xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
gradyhillhouse 0:806c1dd6946c 162 * zmol -
gradyhillhouse 0:806c1dd6946c 163 * zmos -
gradyhillhouse 0:806c1dd6946c 164 * ep - eccentricity 0.0 - 1.0
gradyhillhouse 0:806c1dd6946c 165 * inclo - inclination - needed for lyddane modification
gradyhillhouse 0:806c1dd6946c 166 * nodep - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 167 * argpp - argument of perigee
gradyhillhouse 0:806c1dd6946c 168 * mp - mean anomaly
gradyhillhouse 0:806c1dd6946c 169 *
gradyhillhouse 0:806c1dd6946c 170 * outputs :
gradyhillhouse 0:806c1dd6946c 171 * ep - eccentricity 0.0 - 1.0
gradyhillhouse 0:806c1dd6946c 172 * inclp - inclination
gradyhillhouse 0:806c1dd6946c 173 * nodep - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 174 * argpp - argument of perigee
gradyhillhouse 0:806c1dd6946c 175 * mp - mean anomaly
gradyhillhouse 0:806c1dd6946c 176 *
gradyhillhouse 0:806c1dd6946c 177 * locals :
gradyhillhouse 0:806c1dd6946c 178 * alfdp -
gradyhillhouse 0:806c1dd6946c 179 * betdp -
gradyhillhouse 0:806c1dd6946c 180 * cosip , sinip , cosop , sinop ,
gradyhillhouse 0:806c1dd6946c 181 * dalf -
gradyhillhouse 0:806c1dd6946c 182 * dbet -
gradyhillhouse 0:806c1dd6946c 183 * dls -
gradyhillhouse 0:806c1dd6946c 184 * f2, f3 -
gradyhillhouse 0:806c1dd6946c 185 * pe -
gradyhillhouse 0:806c1dd6946c 186 * pgh -
gradyhillhouse 0:806c1dd6946c 187 * ph -
gradyhillhouse 0:806c1dd6946c 188 * pinc -
gradyhillhouse 0:806c1dd6946c 189 * pl -
gradyhillhouse 0:806c1dd6946c 190 * sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis ,
gradyhillhouse 0:806c1dd6946c 191 * sll , sls
gradyhillhouse 0:806c1dd6946c 192 * xls -
gradyhillhouse 0:806c1dd6946c 193 * xnoh -
gradyhillhouse 0:806c1dd6946c 194 * zf -
gradyhillhouse 0:806c1dd6946c 195 * zm -
gradyhillhouse 0:806c1dd6946c 196 *
gradyhillhouse 0:806c1dd6946c 197 * coupling :
gradyhillhouse 0:806c1dd6946c 198 * none.
gradyhillhouse 0:806c1dd6946c 199 *
gradyhillhouse 0:806c1dd6946c 200 * references :
gradyhillhouse 0:806c1dd6946c 201 * hoots, roehrich, norad spacetrack report #3 1980
gradyhillhouse 0:806c1dd6946c 202 * hoots, norad spacetrack report #6 1986
gradyhillhouse 0:806c1dd6946c 203 * hoots, schumacher and glover 2004
gradyhillhouse 0:806c1dd6946c 204 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 205 ----------------------------------------------------------------------------*/
gradyhillhouse 0:806c1dd6946c 206
gradyhillhouse 0:806c1dd6946c 207 static void dpper
gradyhillhouse 0:806c1dd6946c 208 (
gradyhillhouse 0:806c1dd6946c 209 double e3, double ee2, double peo, double pgho, double pho,
gradyhillhouse 0:806c1dd6946c 210 double pinco, double plo, double se2, double se3, double sgh2,
gradyhillhouse 0:806c1dd6946c 211 double sgh3, double sgh4, double sh2, double sh3, double si2,
gradyhillhouse 0:806c1dd6946c 212 double si3, double sl2, double sl3, double sl4, double t,
gradyhillhouse 0:806c1dd6946c 213 double xgh2, double xgh3, double xgh4, double xh2, double xh3,
gradyhillhouse 0:806c1dd6946c 214 double xi2, double xi3, double xl2, double xl3, double xl4,
gradyhillhouse 0:806c1dd6946c 215 double zmol, double zmos, double inclo,
gradyhillhouse 0:806c1dd6946c 216 char init,
gradyhillhouse 0:806c1dd6946c 217 double& ep, double& inclp, double& nodep, double& argpp, double& mp,
gradyhillhouse 0:806c1dd6946c 218 char opsmode
gradyhillhouse 0:806c1dd6946c 219 )
gradyhillhouse 0:806c1dd6946c 220 {
gradyhillhouse 0:806c1dd6946c 221 /* --------------------- local variables ------------------------ */
gradyhillhouse 0:806c1dd6946c 222 const double twopi = 2.0 * pi;
gradyhillhouse 0:806c1dd6946c 223 double alfdp, betdp, cosip, cosop, dalf, dbet, dls,
gradyhillhouse 0:806c1dd6946c 224 f2, f3, pe, pgh, ph, pinc, pl ,
gradyhillhouse 0:806c1dd6946c 225 sel, ses, sghl, sghs, shll, shs, sil,
gradyhillhouse 0:806c1dd6946c 226 sinip, sinop, sinzf, sis, sll, sls, xls,
gradyhillhouse 0:806c1dd6946c 227 xnoh, zf, zm, zel, zes, znl, zns;
gradyhillhouse 0:806c1dd6946c 228
gradyhillhouse 0:806c1dd6946c 229 /* ---------------------- constants ----------------------------- */
gradyhillhouse 0:806c1dd6946c 230 zns = 1.19459e-5;
gradyhillhouse 0:806c1dd6946c 231 zes = 0.01675;
gradyhillhouse 0:806c1dd6946c 232 znl = 1.5835218e-4;
gradyhillhouse 0:806c1dd6946c 233 zel = 0.05490;
gradyhillhouse 0:806c1dd6946c 234
gradyhillhouse 0:806c1dd6946c 235 /* --------------- calculate time varying periodics ----------- */
gradyhillhouse 0:806c1dd6946c 236 zm = zmos + zns * t;
gradyhillhouse 0:806c1dd6946c 237 // be sure that the initial call has time set to zero
gradyhillhouse 0:806c1dd6946c 238 if (init == 'y')
gradyhillhouse 0:806c1dd6946c 239 zm = zmos;
gradyhillhouse 0:806c1dd6946c 240 zf = zm + 2.0 * zes * sin(zm);
gradyhillhouse 0:806c1dd6946c 241 sinzf = sin(zf);
gradyhillhouse 0:806c1dd6946c 242 f2 = 0.5 * sinzf * sinzf - 0.25;
gradyhillhouse 0:806c1dd6946c 243 f3 = -0.5 * sinzf * cos(zf);
gradyhillhouse 0:806c1dd6946c 244 ses = se2* f2 + se3 * f3;
gradyhillhouse 0:806c1dd6946c 245 sis = si2 * f2 + si3 * f3;
gradyhillhouse 0:806c1dd6946c 246 sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf;
gradyhillhouse 0:806c1dd6946c 247 sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf;
gradyhillhouse 0:806c1dd6946c 248 shs = sh2 * f2 + sh3 * f3;
gradyhillhouse 0:806c1dd6946c 249 zm = zmol + znl * t;
gradyhillhouse 0:806c1dd6946c 250 if (init == 'y')
gradyhillhouse 0:806c1dd6946c 251 zm = zmol;
gradyhillhouse 0:806c1dd6946c 252 zf = zm + 2.0 * zel * sin(zm);
gradyhillhouse 0:806c1dd6946c 253 sinzf = sin(zf);
gradyhillhouse 0:806c1dd6946c 254 f2 = 0.5 * sinzf * sinzf - 0.25;
gradyhillhouse 0:806c1dd6946c 255 f3 = -0.5 * sinzf * cos(zf);
gradyhillhouse 0:806c1dd6946c 256 sel = ee2 * f2 + e3 * f3;
gradyhillhouse 0:806c1dd6946c 257 sil = xi2 * f2 + xi3 * f3;
gradyhillhouse 0:806c1dd6946c 258 sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf;
gradyhillhouse 0:806c1dd6946c 259 sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf;
gradyhillhouse 0:806c1dd6946c 260 shll = xh2 * f2 + xh3 * f3;
gradyhillhouse 0:806c1dd6946c 261 pe = ses + sel;
gradyhillhouse 0:806c1dd6946c 262 pinc = sis + sil;
gradyhillhouse 0:806c1dd6946c 263 pl = sls + sll;
gradyhillhouse 0:806c1dd6946c 264 pgh = sghs + sghl;
gradyhillhouse 0:806c1dd6946c 265 ph = shs + shll;
gradyhillhouse 0:806c1dd6946c 266
gradyhillhouse 0:806c1dd6946c 267 if (init == 'n')
gradyhillhouse 0:806c1dd6946c 268 {
gradyhillhouse 0:806c1dd6946c 269 pe = pe - peo;
gradyhillhouse 0:806c1dd6946c 270 pinc = pinc - pinco;
gradyhillhouse 0:806c1dd6946c 271 pl = pl - plo;
gradyhillhouse 0:806c1dd6946c 272 pgh = pgh - pgho;
gradyhillhouse 0:806c1dd6946c 273 ph = ph - pho;
gradyhillhouse 0:806c1dd6946c 274 inclp = inclp + pinc;
gradyhillhouse 0:806c1dd6946c 275 ep = ep + pe;
gradyhillhouse 0:806c1dd6946c 276 sinip = sin(inclp);
gradyhillhouse 0:806c1dd6946c 277 cosip = cos(inclp);
gradyhillhouse 0:806c1dd6946c 278
gradyhillhouse 0:806c1dd6946c 279 /* ----------------- apply periodics directly ------------ */
gradyhillhouse 0:806c1dd6946c 280 // sgp4fix for lyddane choice
gradyhillhouse 0:806c1dd6946c 281 // strn3 used original inclination - this is technically feasible
gradyhillhouse 0:806c1dd6946c 282 // gsfc used perturbed inclination - also technically feasible
gradyhillhouse 0:806c1dd6946c 283 // probably best to readjust the 0.2 limit value and limit discontinuity
gradyhillhouse 0:806c1dd6946c 284 // 0.2 rad = 11.45916 deg
gradyhillhouse 0:806c1dd6946c 285 // use next line for original strn3 approach and original inclination
gradyhillhouse 0:806c1dd6946c 286 // if (inclo >= 0.2)
gradyhillhouse 0:806c1dd6946c 287 // use next line for gsfc version and perturbed inclination
gradyhillhouse 0:806c1dd6946c 288 if (inclp >= 0.2)
gradyhillhouse 0:806c1dd6946c 289 {
gradyhillhouse 0:806c1dd6946c 290 ph = ph / sinip;
gradyhillhouse 0:806c1dd6946c 291 pgh = pgh - cosip * ph;
gradyhillhouse 0:806c1dd6946c 292 argpp = argpp + pgh;
gradyhillhouse 0:806c1dd6946c 293 nodep = nodep + ph;
gradyhillhouse 0:806c1dd6946c 294 mp = mp + pl;
gradyhillhouse 0:806c1dd6946c 295 }
gradyhillhouse 0:806c1dd6946c 296 else
gradyhillhouse 0:806c1dd6946c 297 {
gradyhillhouse 0:806c1dd6946c 298 /* ---- apply periodics with lyddane modification ---- */
gradyhillhouse 0:806c1dd6946c 299 sinop = sin(nodep);
gradyhillhouse 0:806c1dd6946c 300 cosop = cos(nodep);
gradyhillhouse 0:806c1dd6946c 301 alfdp = sinip * sinop;
gradyhillhouse 0:806c1dd6946c 302 betdp = sinip * cosop;
gradyhillhouse 0:806c1dd6946c 303 dalf = ph * cosop + pinc * cosip * sinop;
gradyhillhouse 0:806c1dd6946c 304 dbet = -ph * sinop + pinc * cosip * cosop;
gradyhillhouse 0:806c1dd6946c 305 alfdp = alfdp + dalf;
gradyhillhouse 0:806c1dd6946c 306 betdp = betdp + dbet;
gradyhillhouse 0:806c1dd6946c 307 nodep = fmod(nodep, twopi);
gradyhillhouse 0:806c1dd6946c 308 // sgp4fix for afspc written intrinsic functions
gradyhillhouse 0:806c1dd6946c 309 // nodep used without a trigonometric function ahead
gradyhillhouse 0:806c1dd6946c 310 if ((nodep < 0.0) && (opsmode == 'a'))
gradyhillhouse 0:806c1dd6946c 311 nodep = nodep + twopi;
gradyhillhouse 0:806c1dd6946c 312 xls = mp + argpp + cosip * nodep;
gradyhillhouse 0:806c1dd6946c 313 dls = pl + pgh - pinc * nodep * sinip;
gradyhillhouse 0:806c1dd6946c 314 xls = xls + dls;
gradyhillhouse 0:806c1dd6946c 315 xnoh = nodep;
gradyhillhouse 0:806c1dd6946c 316 nodep = atan2(alfdp, betdp);
gradyhillhouse 0:806c1dd6946c 317 // sgp4fix for afspc written intrinsic functions
gradyhillhouse 0:806c1dd6946c 318 // nodep used without a trigonometric function ahead
gradyhillhouse 0:806c1dd6946c 319 if ((nodep < 0.0) && (opsmode == 'a'))
gradyhillhouse 0:806c1dd6946c 320 nodep = nodep + twopi;
gradyhillhouse 0:806c1dd6946c 321 if (fabs(xnoh - nodep) > pi)
gradyhillhouse 0:806c1dd6946c 322 if (nodep < xnoh)
gradyhillhouse 0:806c1dd6946c 323 nodep = nodep + twopi;
gradyhillhouse 0:806c1dd6946c 324 else
gradyhillhouse 0:806c1dd6946c 325 nodep = nodep - twopi;
gradyhillhouse 0:806c1dd6946c 326 mp = mp + pl;
gradyhillhouse 0:806c1dd6946c 327 argpp = xls - mp - cosip * nodep;
gradyhillhouse 0:806c1dd6946c 328 }
gradyhillhouse 0:806c1dd6946c 329 } // if init == 'n'
gradyhillhouse 0:806c1dd6946c 330
gradyhillhouse 0:806c1dd6946c 331 //#include "debug1.cpp"
gradyhillhouse 0:806c1dd6946c 332 } // end dpper
gradyhillhouse 0:806c1dd6946c 333
gradyhillhouse 0:806c1dd6946c 334 /*-----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 335 *
gradyhillhouse 0:806c1dd6946c 336 * procedure dscom
gradyhillhouse 0:806c1dd6946c 337 *
gradyhillhouse 0:806c1dd6946c 338 * this procedure provides deep space common items used by both the secular
gradyhillhouse 0:806c1dd6946c 339 * and periodics subroutines. input is provided as shown. this routine
gradyhillhouse 0:806c1dd6946c 340 * used to be called dpper, but the functions inside weren't well organized.
gradyhillhouse 0:806c1dd6946c 341 *
gradyhillhouse 0:806c1dd6946c 342 * author : david vallado 719-573-2600 28 jun 2005
gradyhillhouse 0:806c1dd6946c 343 *
gradyhillhouse 0:806c1dd6946c 344 * inputs :
gradyhillhouse 0:806c1dd6946c 345 * epoch -
gradyhillhouse 0:806c1dd6946c 346 * ep - eccentricity
gradyhillhouse 0:806c1dd6946c 347 * argpp - argument of perigee
gradyhillhouse 0:806c1dd6946c 348 * tc -
gradyhillhouse 0:806c1dd6946c 349 * inclp - inclination
gradyhillhouse 0:806c1dd6946c 350 * nodep - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 351 * np - mean motion
gradyhillhouse 0:806c1dd6946c 352 *
gradyhillhouse 0:806c1dd6946c 353 * outputs :
gradyhillhouse 0:806c1dd6946c 354 * sinim , cosim , sinomm , cosomm , snodm , cnodm
gradyhillhouse 0:806c1dd6946c 355 * day -
gradyhillhouse 0:806c1dd6946c 356 * e3 -
gradyhillhouse 0:806c1dd6946c 357 * ee2 -
gradyhillhouse 0:806c1dd6946c 358 * em - eccentricity
gradyhillhouse 0:806c1dd6946c 359 * emsq - eccentricity squared
gradyhillhouse 0:806c1dd6946c 360 * gam -
gradyhillhouse 0:806c1dd6946c 361 * peo -
gradyhillhouse 0:806c1dd6946c 362 * pgho -
gradyhillhouse 0:806c1dd6946c 363 * pho -
gradyhillhouse 0:806c1dd6946c 364 * pinco -
gradyhillhouse 0:806c1dd6946c 365 * plo -
gradyhillhouse 0:806c1dd6946c 366 * rtemsq -
gradyhillhouse 0:806c1dd6946c 367 * se2, se3 -
gradyhillhouse 0:806c1dd6946c 368 * sgh2, sgh3, sgh4 -
gradyhillhouse 0:806c1dd6946c 369 * sh2, sh3, si2, si3, sl2, sl3, sl4 -
gradyhillhouse 0:806c1dd6946c 370 * s1, s2, s3, s4, s5, s6, s7 -
gradyhillhouse 0:806c1dd6946c 371 * ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 -
gradyhillhouse 0:806c1dd6946c 372 * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 -
gradyhillhouse 0:806c1dd6946c 373 * xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
gradyhillhouse 0:806c1dd6946c 374 * nm - mean motion
gradyhillhouse 0:806c1dd6946c 375 * z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 -
gradyhillhouse 0:806c1dd6946c 376 * zmol -
gradyhillhouse 0:806c1dd6946c 377 * zmos -
gradyhillhouse 0:806c1dd6946c 378 *
gradyhillhouse 0:806c1dd6946c 379 * locals :
gradyhillhouse 0:806c1dd6946c 380 * a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 -
gradyhillhouse 0:806c1dd6946c 381 * betasq -
gradyhillhouse 0:806c1dd6946c 382 * cc -
gradyhillhouse 0:806c1dd6946c 383 * ctem, stem -
gradyhillhouse 0:806c1dd6946c 384 * x1, x2, x3, x4, x5, x6, x7, x8 -
gradyhillhouse 0:806c1dd6946c 385 * xnodce -
gradyhillhouse 0:806c1dd6946c 386 * xnoi -
gradyhillhouse 0:806c1dd6946c 387 * zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl ,
gradyhillhouse 0:806c1dd6946c 388 * zcosi , zsini , zcosil , zsinil ,
gradyhillhouse 0:806c1dd6946c 389 * zx -
gradyhillhouse 0:806c1dd6946c 390 * zy -
gradyhillhouse 0:806c1dd6946c 391 *
gradyhillhouse 0:806c1dd6946c 392 * coupling :
gradyhillhouse 0:806c1dd6946c 393 * none.
gradyhillhouse 0:806c1dd6946c 394 *
gradyhillhouse 0:806c1dd6946c 395 * references :
gradyhillhouse 0:806c1dd6946c 396 * hoots, roehrich, norad spacetrack report #3 1980
gradyhillhouse 0:806c1dd6946c 397 * hoots, norad spacetrack report #6 1986
gradyhillhouse 0:806c1dd6946c 398 * hoots, schumacher and glover 2004
gradyhillhouse 0:806c1dd6946c 399 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 400 ----------------------------------------------------------------------------*/
gradyhillhouse 0:806c1dd6946c 401
gradyhillhouse 0:806c1dd6946c 402 static void dscom
gradyhillhouse 0:806c1dd6946c 403 (
gradyhillhouse 0:806c1dd6946c 404 double epoch, double ep, double argpp, double tc, double inclp,
gradyhillhouse 0:806c1dd6946c 405 double nodep, double np,
gradyhillhouse 0:806c1dd6946c 406 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm,
gradyhillhouse 0:806c1dd6946c 407 double& cosomm,double& day, double& e3, double& ee2, double& em,
gradyhillhouse 0:806c1dd6946c 408 double& emsq, double& gam, double& peo, double& pgho, double& pho,
gradyhillhouse 0:806c1dd6946c 409 double& pinco, double& plo, double& rtemsq, double& se2, double& se3,
gradyhillhouse 0:806c1dd6946c 410 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3,
gradyhillhouse 0:806c1dd6946c 411 double& si2, double& si3, double& sl2, double& sl3, double& sl4,
gradyhillhouse 0:806c1dd6946c 412 double& s1, double& s2, double& s3, double& s4, double& s5,
gradyhillhouse 0:806c1dd6946c 413 double& s6, double& s7, double& ss1, double& ss2, double& ss3,
gradyhillhouse 0:806c1dd6946c 414 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1,
gradyhillhouse 0:806c1dd6946c 415 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13,
gradyhillhouse 0:806c1dd6946c 416 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32,
gradyhillhouse 0:806c1dd6946c 417 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2,
gradyhillhouse 0:806c1dd6946c 418 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3,
gradyhillhouse 0:806c1dd6946c 419 double& xl4, double& nm, double& z1, double& z2, double& z3,
gradyhillhouse 0:806c1dd6946c 420 double& z11, double& z12, double& z13, double& z21, double& z22,
gradyhillhouse 0:806c1dd6946c 421 double& z23, double& z31, double& z32, double& z33, double& zmol,
gradyhillhouse 0:806c1dd6946c 422 double& zmos
gradyhillhouse 0:806c1dd6946c 423 )
gradyhillhouse 0:806c1dd6946c 424 {
gradyhillhouse 0:806c1dd6946c 425 /* -------------------------- constants ------------------------- */
gradyhillhouse 0:806c1dd6946c 426 const double zes = 0.01675;
gradyhillhouse 0:806c1dd6946c 427 const double zel = 0.05490;
gradyhillhouse 0:806c1dd6946c 428 const double c1ss = 2.9864797e-6;
gradyhillhouse 0:806c1dd6946c 429 const double c1l = 4.7968065e-7;
gradyhillhouse 0:806c1dd6946c 430 const double zsinis = 0.39785416;
gradyhillhouse 0:806c1dd6946c 431 const double zcosis = 0.91744867;
gradyhillhouse 0:806c1dd6946c 432 const double zcosgs = 0.1945905;
gradyhillhouse 0:806c1dd6946c 433 const double zsings = -0.98088458;
gradyhillhouse 0:806c1dd6946c 434 const double twopi = 2.0 * pi;
gradyhillhouse 0:806c1dd6946c 435
gradyhillhouse 0:806c1dd6946c 436 /* --------------------- local variables ------------------------ */
gradyhillhouse 0:806c1dd6946c 437 int lsflg;
gradyhillhouse 0:806c1dd6946c 438 double a1 , a2 , a3 , a4 , a5 , a6 , a7 ,
gradyhillhouse 0:806c1dd6946c 439 a8 , a9 , a10 , betasq, cc , ctem , stem ,
gradyhillhouse 0:806c1dd6946c 440 x1 , x2 , x3 , x4 , x5 , x6 , x7 ,
gradyhillhouse 0:806c1dd6946c 441 x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl,
gradyhillhouse 0:806c1dd6946c 442 zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini ,
gradyhillhouse 0:806c1dd6946c 443 zsinil, zx , zy;
gradyhillhouse 0:806c1dd6946c 444
gradyhillhouse 0:806c1dd6946c 445 nm = np;
gradyhillhouse 0:806c1dd6946c 446 em = ep;
gradyhillhouse 0:806c1dd6946c 447 snodm = sin(nodep);
gradyhillhouse 0:806c1dd6946c 448 cnodm = cos(nodep);
gradyhillhouse 0:806c1dd6946c 449 sinomm = sin(argpp);
gradyhillhouse 0:806c1dd6946c 450 cosomm = cos(argpp);
gradyhillhouse 0:806c1dd6946c 451 sinim = sin(inclp);
gradyhillhouse 0:806c1dd6946c 452 cosim = cos(inclp);
gradyhillhouse 0:806c1dd6946c 453 emsq = em * em;
gradyhillhouse 0:806c1dd6946c 454 betasq = 1.0 - emsq;
gradyhillhouse 0:806c1dd6946c 455 rtemsq = sqrt(betasq);
gradyhillhouse 0:806c1dd6946c 456
gradyhillhouse 0:806c1dd6946c 457 /* ----------------- initialize lunar solar terms --------------- */
gradyhillhouse 0:806c1dd6946c 458 peo = 0.0;
gradyhillhouse 0:806c1dd6946c 459 pinco = 0.0;
gradyhillhouse 0:806c1dd6946c 460 plo = 0.0;
gradyhillhouse 0:806c1dd6946c 461 pgho = 0.0;
gradyhillhouse 0:806c1dd6946c 462 pho = 0.0;
gradyhillhouse 0:806c1dd6946c 463 day = epoch + 18261.5 + tc / 1440.0;
gradyhillhouse 0:806c1dd6946c 464 xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi);
gradyhillhouse 0:806c1dd6946c 465 stem = sin(xnodce);
gradyhillhouse 0:806c1dd6946c 466 ctem = cos(xnodce);
gradyhillhouse 0:806c1dd6946c 467 zcosil = 0.91375164 - 0.03568096 * ctem;
gradyhillhouse 0:806c1dd6946c 468 zsinil = sqrt(1.0 - zcosil * zcosil);
gradyhillhouse 0:806c1dd6946c 469 zsinhl = 0.089683511 * stem / zsinil;
gradyhillhouse 0:806c1dd6946c 470 zcoshl = sqrt(1.0 - zsinhl * zsinhl);
gradyhillhouse 0:806c1dd6946c 471 gam = 5.8351514 + 0.0019443680 * day;
gradyhillhouse 0:806c1dd6946c 472 zx = 0.39785416 * stem / zsinil;
gradyhillhouse 0:806c1dd6946c 473 zy = zcoshl * ctem + 0.91744867 * zsinhl * stem;
gradyhillhouse 0:806c1dd6946c 474 zx = atan2(zx, zy);
gradyhillhouse 0:806c1dd6946c 475 zx = gam + zx - xnodce;
gradyhillhouse 0:806c1dd6946c 476 zcosgl = cos(zx);
gradyhillhouse 0:806c1dd6946c 477 zsingl = sin(zx);
gradyhillhouse 0:806c1dd6946c 478
gradyhillhouse 0:806c1dd6946c 479 /* ------------------------- do solar terms --------------------- */
gradyhillhouse 0:806c1dd6946c 480 zcosg = zcosgs;
gradyhillhouse 0:806c1dd6946c 481 zsing = zsings;
gradyhillhouse 0:806c1dd6946c 482 zcosi = zcosis;
gradyhillhouse 0:806c1dd6946c 483 zsini = zsinis;
gradyhillhouse 0:806c1dd6946c 484 zcosh = cnodm;
gradyhillhouse 0:806c1dd6946c 485 zsinh = snodm;
gradyhillhouse 0:806c1dd6946c 486 cc = c1ss;
gradyhillhouse 0:806c1dd6946c 487 xnoi = 1.0 / nm;
gradyhillhouse 0:806c1dd6946c 488
gradyhillhouse 0:806c1dd6946c 489 for (lsflg = 1; lsflg <= 2; lsflg++)
gradyhillhouse 0:806c1dd6946c 490 {
gradyhillhouse 0:806c1dd6946c 491 a1 = zcosg * zcosh + zsing * zcosi * zsinh;
gradyhillhouse 0:806c1dd6946c 492 a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
gradyhillhouse 0:806c1dd6946c 493 a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
gradyhillhouse 0:806c1dd6946c 494 a8 = zsing * zsini;
gradyhillhouse 0:806c1dd6946c 495 a9 = zsing * zsinh + zcosg * zcosi * zcosh;
gradyhillhouse 0:806c1dd6946c 496 a10 = zcosg * zsini;
gradyhillhouse 0:806c1dd6946c 497 a2 = cosim * a7 + sinim * a8;
gradyhillhouse 0:806c1dd6946c 498 a4 = cosim * a9 + sinim * a10;
gradyhillhouse 0:806c1dd6946c 499 a5 = -sinim * a7 + cosim * a8;
gradyhillhouse 0:806c1dd6946c 500 a6 = -sinim * a9 + cosim * a10;
gradyhillhouse 0:806c1dd6946c 501
gradyhillhouse 0:806c1dd6946c 502 x1 = a1 * cosomm + a2 * sinomm;
gradyhillhouse 0:806c1dd6946c 503 x2 = a3 * cosomm + a4 * sinomm;
gradyhillhouse 0:806c1dd6946c 504 x3 = -a1 * sinomm + a2 * cosomm;
gradyhillhouse 0:806c1dd6946c 505 x4 = -a3 * sinomm + a4 * cosomm;
gradyhillhouse 0:806c1dd6946c 506 x5 = a5 * sinomm;
gradyhillhouse 0:806c1dd6946c 507 x6 = a6 * sinomm;
gradyhillhouse 0:806c1dd6946c 508 x7 = a5 * cosomm;
gradyhillhouse 0:806c1dd6946c 509 x8 = a6 * cosomm;
gradyhillhouse 0:806c1dd6946c 510
gradyhillhouse 0:806c1dd6946c 511 z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3;
gradyhillhouse 0:806c1dd6946c 512 z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4;
gradyhillhouse 0:806c1dd6946c 513 z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4;
gradyhillhouse 0:806c1dd6946c 514 z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq;
gradyhillhouse 0:806c1dd6946c 515 z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq;
gradyhillhouse 0:806c1dd6946c 516 z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq;
gradyhillhouse 0:806c1dd6946c 517 z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5);
gradyhillhouse 0:806c1dd6946c 518 z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq *
gradyhillhouse 0:806c1dd6946c 519 (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5));
gradyhillhouse 0:806c1dd6946c 520 z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6);
gradyhillhouse 0:806c1dd6946c 521 z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7);
gradyhillhouse 0:806c1dd6946c 522 z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq *
gradyhillhouse 0:806c1dd6946c 523 (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8));
gradyhillhouse 0:806c1dd6946c 524 z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8);
gradyhillhouse 0:806c1dd6946c 525 z1 = z1 + z1 + betasq * z31;
gradyhillhouse 0:806c1dd6946c 526 z2 = z2 + z2 + betasq * z32;
gradyhillhouse 0:806c1dd6946c 527 z3 = z3 + z3 + betasq * z33;
gradyhillhouse 0:806c1dd6946c 528 s3 = cc * xnoi;
gradyhillhouse 0:806c1dd6946c 529 s2 = -0.5 * s3 / rtemsq;
gradyhillhouse 0:806c1dd6946c 530 s4 = s3 * rtemsq;
gradyhillhouse 0:806c1dd6946c 531 s1 = -15.0 * em * s4;
gradyhillhouse 0:806c1dd6946c 532 s5 = x1 * x3 + x2 * x4;
gradyhillhouse 0:806c1dd6946c 533 s6 = x2 * x3 + x1 * x4;
gradyhillhouse 0:806c1dd6946c 534 s7 = x2 * x4 - x1 * x3;
gradyhillhouse 0:806c1dd6946c 535
gradyhillhouse 0:806c1dd6946c 536 /* ----------------------- do lunar terms ------------------- */
gradyhillhouse 0:806c1dd6946c 537 if (lsflg == 1)
gradyhillhouse 0:806c1dd6946c 538 {
gradyhillhouse 0:806c1dd6946c 539 ss1 = s1;
gradyhillhouse 0:806c1dd6946c 540 ss2 = s2;
gradyhillhouse 0:806c1dd6946c 541 ss3 = s3;
gradyhillhouse 0:806c1dd6946c 542 ss4 = s4;
gradyhillhouse 0:806c1dd6946c 543 ss5 = s5;
gradyhillhouse 0:806c1dd6946c 544 ss6 = s6;
gradyhillhouse 0:806c1dd6946c 545 ss7 = s7;
gradyhillhouse 0:806c1dd6946c 546 sz1 = z1;
gradyhillhouse 0:806c1dd6946c 547 sz2 = z2;
gradyhillhouse 0:806c1dd6946c 548 sz3 = z3;
gradyhillhouse 0:806c1dd6946c 549 sz11 = z11;
gradyhillhouse 0:806c1dd6946c 550 sz12 = z12;
gradyhillhouse 0:806c1dd6946c 551 sz13 = z13;
gradyhillhouse 0:806c1dd6946c 552 sz21 = z21;
gradyhillhouse 0:806c1dd6946c 553 sz22 = z22;
gradyhillhouse 0:806c1dd6946c 554 sz23 = z23;
gradyhillhouse 0:806c1dd6946c 555 sz31 = z31;
gradyhillhouse 0:806c1dd6946c 556 sz32 = z32;
gradyhillhouse 0:806c1dd6946c 557 sz33 = z33;
gradyhillhouse 0:806c1dd6946c 558 zcosg = zcosgl;
gradyhillhouse 0:806c1dd6946c 559 zsing = zsingl;
gradyhillhouse 0:806c1dd6946c 560 zcosi = zcosil;
gradyhillhouse 0:806c1dd6946c 561 zsini = zsinil;
gradyhillhouse 0:806c1dd6946c 562 zcosh = zcoshl * cnodm + zsinhl * snodm;
gradyhillhouse 0:806c1dd6946c 563 zsinh = snodm * zcoshl - cnodm * zsinhl;
gradyhillhouse 0:806c1dd6946c 564 cc = c1l;
gradyhillhouse 0:806c1dd6946c 565 }
gradyhillhouse 0:806c1dd6946c 566 }
gradyhillhouse 0:806c1dd6946c 567
gradyhillhouse 0:806c1dd6946c 568 zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi);
gradyhillhouse 0:806c1dd6946c 569 zmos = fmod(6.2565837 + 0.017201977 * day, twopi);
gradyhillhouse 0:806c1dd6946c 570
gradyhillhouse 0:806c1dd6946c 571 /* ------------------------ do solar terms ---------------------- */
gradyhillhouse 0:806c1dd6946c 572 se2 = 2.0 * ss1 * ss6;
gradyhillhouse 0:806c1dd6946c 573 se3 = 2.0 * ss1 * ss7;
gradyhillhouse 0:806c1dd6946c 574 si2 = 2.0 * ss2 * sz12;
gradyhillhouse 0:806c1dd6946c 575 si3 = 2.0 * ss2 * (sz13 - sz11);
gradyhillhouse 0:806c1dd6946c 576 sl2 = -2.0 * ss3 * sz2;
gradyhillhouse 0:806c1dd6946c 577 sl3 = -2.0 * ss3 * (sz3 - sz1);
gradyhillhouse 0:806c1dd6946c 578 sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes;
gradyhillhouse 0:806c1dd6946c 579 sgh2 = 2.0 * ss4 * sz32;
gradyhillhouse 0:806c1dd6946c 580 sgh3 = 2.0 * ss4 * (sz33 - sz31);
gradyhillhouse 0:806c1dd6946c 581 sgh4 = -18.0 * ss4 * zes;
gradyhillhouse 0:806c1dd6946c 582 sh2 = -2.0 * ss2 * sz22;
gradyhillhouse 0:806c1dd6946c 583 sh3 = -2.0 * ss2 * (sz23 - sz21);
gradyhillhouse 0:806c1dd6946c 584
gradyhillhouse 0:806c1dd6946c 585 /* ------------------------ do lunar terms ---------------------- */
gradyhillhouse 0:806c1dd6946c 586 ee2 = 2.0 * s1 * s6;
gradyhillhouse 0:806c1dd6946c 587 e3 = 2.0 * s1 * s7;
gradyhillhouse 0:806c1dd6946c 588 xi2 = 2.0 * s2 * z12;
gradyhillhouse 0:806c1dd6946c 589 xi3 = 2.0 * s2 * (z13 - z11);
gradyhillhouse 0:806c1dd6946c 590 xl2 = -2.0 * s3 * z2;
gradyhillhouse 0:806c1dd6946c 591 xl3 = -2.0 * s3 * (z3 - z1);
gradyhillhouse 0:806c1dd6946c 592 xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel;
gradyhillhouse 0:806c1dd6946c 593 xgh2 = 2.0 * s4 * z32;
gradyhillhouse 0:806c1dd6946c 594 xgh3 = 2.0 * s4 * (z33 - z31);
gradyhillhouse 0:806c1dd6946c 595 xgh4 = -18.0 * s4 * zel;
gradyhillhouse 0:806c1dd6946c 596 xh2 = -2.0 * s2 * z22;
gradyhillhouse 0:806c1dd6946c 597 xh3 = -2.0 * s2 * (z23 - z21);
gradyhillhouse 0:806c1dd6946c 598
gradyhillhouse 0:806c1dd6946c 599 //#include "debug2.cpp"
gradyhillhouse 0:806c1dd6946c 600 } // end dscom
gradyhillhouse 0:806c1dd6946c 601
gradyhillhouse 0:806c1dd6946c 602 /*-----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 603 *
gradyhillhouse 0:806c1dd6946c 604 * procedure dsinit
gradyhillhouse 0:806c1dd6946c 605 *
gradyhillhouse 0:806c1dd6946c 606 * this procedure provides deep space contributions to mean motion dot due
gradyhillhouse 0:806c1dd6946c 607 * to geopotential resonance with half day and one day orbits.
gradyhillhouse 0:806c1dd6946c 608 *
gradyhillhouse 0:806c1dd6946c 609 * author : david vallado 719-573-2600 28 jun 2005
gradyhillhouse 0:806c1dd6946c 610 *
gradyhillhouse 0:806c1dd6946c 611 * inputs :
gradyhillhouse 0:806c1dd6946c 612 * cosim, sinim-
gradyhillhouse 0:806c1dd6946c 613 * emsq - eccentricity squared
gradyhillhouse 0:806c1dd6946c 614 * argpo - argument of perigee
gradyhillhouse 0:806c1dd6946c 615 * s1, s2, s3, s4, s5 -
gradyhillhouse 0:806c1dd6946c 616 * ss1, ss2, ss3, ss4, ss5 -
gradyhillhouse 0:806c1dd6946c 617 * sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 -
gradyhillhouse 0:806c1dd6946c 618 * t - time
gradyhillhouse 0:806c1dd6946c 619 * tc -
gradyhillhouse 0:806c1dd6946c 620 * gsto - greenwich sidereal time rad
gradyhillhouse 0:806c1dd6946c 621 * mo - mean anomaly
gradyhillhouse 0:806c1dd6946c 622 * mdot - mean anomaly dot (rate)
gradyhillhouse 0:806c1dd6946c 623 * no - mean motion
gradyhillhouse 0:806c1dd6946c 624 * nodeo - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 625 * nodedot - right ascension of ascending node dot (rate)
gradyhillhouse 0:806c1dd6946c 626 * xpidot -
gradyhillhouse 0:806c1dd6946c 627 * z1, z3, z11, z13, z21, z23, z31, z33 -
gradyhillhouse 0:806c1dd6946c 628 * eccm - eccentricity
gradyhillhouse 0:806c1dd6946c 629 * argpm - argument of perigee
gradyhillhouse 0:806c1dd6946c 630 * inclm - inclination
gradyhillhouse 0:806c1dd6946c 631 * mm - mean anomaly
gradyhillhouse 0:806c1dd6946c 632 * xn - mean motion
gradyhillhouse 0:806c1dd6946c 633 * nodem - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 634 *
gradyhillhouse 0:806c1dd6946c 635 * outputs :
gradyhillhouse 0:806c1dd6946c 636 * em - eccentricity
gradyhillhouse 0:806c1dd6946c 637 * argpm - argument of perigee
gradyhillhouse 0:806c1dd6946c 638 * inclm - inclination
gradyhillhouse 0:806c1dd6946c 639 * mm - mean anomaly
gradyhillhouse 0:806c1dd6946c 640 * nm - mean motion
gradyhillhouse 0:806c1dd6946c 641 * nodem - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 642 * irez - flag for resonance 0-none, 1-one day, 2-half day
gradyhillhouse 0:806c1dd6946c 643 * atime -
gradyhillhouse 0:806c1dd6946c 644 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 -
gradyhillhouse 0:806c1dd6946c 645 * dedt -
gradyhillhouse 0:806c1dd6946c 646 * didt -
gradyhillhouse 0:806c1dd6946c 647 * dmdt -
gradyhillhouse 0:806c1dd6946c 648 * dndt -
gradyhillhouse 0:806c1dd6946c 649 * dnodt -
gradyhillhouse 0:806c1dd6946c 650 * domdt -
gradyhillhouse 0:806c1dd6946c 651 * del1, del2, del3 -
gradyhillhouse 0:806c1dd6946c 652 * ses , sghl , sghs , sgs , shl , shs , sis , sls
gradyhillhouse 0:806c1dd6946c 653 * theta -
gradyhillhouse 0:806c1dd6946c 654 * xfact -
gradyhillhouse 0:806c1dd6946c 655 * xlamo -
gradyhillhouse 0:806c1dd6946c 656 * xli -
gradyhillhouse 0:806c1dd6946c 657 * xni
gradyhillhouse 0:806c1dd6946c 658 *
gradyhillhouse 0:806c1dd6946c 659 * locals :
gradyhillhouse 0:806c1dd6946c 660 * ainv2 -
gradyhillhouse 0:806c1dd6946c 661 * aonv -
gradyhillhouse 0:806c1dd6946c 662 * cosisq -
gradyhillhouse 0:806c1dd6946c 663 * eoc -
gradyhillhouse 0:806c1dd6946c 664 * f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 -
gradyhillhouse 0:806c1dd6946c 665 * g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 -
gradyhillhouse 0:806c1dd6946c 666 * sini2 -
gradyhillhouse 0:806c1dd6946c 667 * temp -
gradyhillhouse 0:806c1dd6946c 668 * temp1 -
gradyhillhouse 0:806c1dd6946c 669 * theta -
gradyhillhouse 0:806c1dd6946c 670 * xno2 -
gradyhillhouse 0:806c1dd6946c 671 *
gradyhillhouse 0:806c1dd6946c 672 * coupling :
gradyhillhouse 0:806c1dd6946c 673 * getgravconst
gradyhillhouse 0:806c1dd6946c 674 *
gradyhillhouse 0:806c1dd6946c 675 * references :
gradyhillhouse 0:806c1dd6946c 676 * hoots, roehrich, norad spacetrack report #3 1980
gradyhillhouse 0:806c1dd6946c 677 * hoots, norad spacetrack report #6 1986
gradyhillhouse 0:806c1dd6946c 678 * hoots, schumacher and glover 2004
gradyhillhouse 0:806c1dd6946c 679 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 680 ----------------------------------------------------------------------------*/
gradyhillhouse 0:806c1dd6946c 681
gradyhillhouse 0:806c1dd6946c 682 static void dsinit
gradyhillhouse 0:806c1dd6946c 683 (
gradyhillhouse 0:806c1dd6946c 684 gravconsttype whichconst,
gradyhillhouse 0:806c1dd6946c 685 double cosim, double emsq, double argpo, double s1, double s2,
gradyhillhouse 0:806c1dd6946c 686 double s3, double s4, double s5, double sinim, double ss1,
gradyhillhouse 0:806c1dd6946c 687 double ss2, double ss3, double ss4, double ss5, double sz1,
gradyhillhouse 0:806c1dd6946c 688 double sz3, double sz11, double sz13, double sz21, double sz23,
gradyhillhouse 0:806c1dd6946c 689 double sz31, double sz33, double t, double tc, double gsto,
gradyhillhouse 0:806c1dd6946c 690 double mo, double mdot, double no, double nodeo, double nodedot,
gradyhillhouse 0:806c1dd6946c 691 double xpidot, double z1, double z3, double z11, double z13,
gradyhillhouse 0:806c1dd6946c 692 double z21, double z23, double z31, double z33, double ecco,
gradyhillhouse 0:806c1dd6946c 693 double eccsq, double& em, double& argpm, double& inclm, double& mm,
gradyhillhouse 0:806c1dd6946c 694 double& nm, double& nodem,
gradyhillhouse 0:806c1dd6946c 695 int& irez,
gradyhillhouse 0:806c1dd6946c 696 double& atime, double& d2201, double& d2211, double& d3210, double& d3222,
gradyhillhouse 0:806c1dd6946c 697 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421,
gradyhillhouse 0:806c1dd6946c 698 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt,
gradyhillhouse 0:806c1dd6946c 699 double& dnodt, double& domdt, double& del1, double& del2, double& del3,
gradyhillhouse 0:806c1dd6946c 700 double& xfact, double& xlamo, double& xli, double& xni
gradyhillhouse 0:806c1dd6946c 701 )
gradyhillhouse 0:806c1dd6946c 702 {
gradyhillhouse 0:806c1dd6946c 703 /* --------------------- local variables ------------------------ */
gradyhillhouse 0:806c1dd6946c 704 const double twopi = 2.0 * pi;
gradyhillhouse 0:806c1dd6946c 705
gradyhillhouse 0:806c1dd6946c 706 double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 ,
gradyhillhouse 0:806c1dd6946c 707 f321 , f322 , f330 , f441 , f442 , f522 , f523 ,
gradyhillhouse 0:806c1dd6946c 708 f542 , f543 , g200 , g201 , g211 , g300 , g310 ,
gradyhillhouse 0:806c1dd6946c 709 g322 , g410 , g422 , g520 , g521 , g532 , g533 ,
gradyhillhouse 0:806c1dd6946c 710 ses , sgs , sghl , sghs , shs , shll , sis ,
gradyhillhouse 0:806c1dd6946c 711 sini2 , sls , temp , temp1 , theta , xno2 , q22 ,
gradyhillhouse 0:806c1dd6946c 712 q31 , q33 , root22, root44, root54, rptim , root32,
gradyhillhouse 0:806c1dd6946c 713 root52, x2o3 , xke , znl , emo , zns , emsqo,
gradyhillhouse 0:806c1dd6946c 714 tumin, mu, radiusearthkm, j2, j3, j4, j3oj2;
gradyhillhouse 0:806c1dd6946c 715
gradyhillhouse 0:806c1dd6946c 716 q22 = 1.7891679e-6;
gradyhillhouse 0:806c1dd6946c 717 q31 = 2.1460748e-6;
gradyhillhouse 0:806c1dd6946c 718 q33 = 2.2123015e-7;
gradyhillhouse 0:806c1dd6946c 719 root22 = 1.7891679e-6;
gradyhillhouse 0:806c1dd6946c 720 root44 = 7.3636953e-9;
gradyhillhouse 0:806c1dd6946c 721 root54 = 2.1765803e-9;
gradyhillhouse 0:806c1dd6946c 722 rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
gradyhillhouse 0:806c1dd6946c 723 root32 = 3.7393792e-7;
gradyhillhouse 0:806c1dd6946c 724 root52 = 1.1428639e-7;
gradyhillhouse 0:806c1dd6946c 725 x2o3 = 2.0 / 3.0;
gradyhillhouse 0:806c1dd6946c 726 znl = 1.5835218e-4;
gradyhillhouse 0:806c1dd6946c 727 zns = 1.19459e-5;
gradyhillhouse 0:806c1dd6946c 728
gradyhillhouse 0:806c1dd6946c 729 // sgp4fix identify constants and allow alternate values
gradyhillhouse 0:806c1dd6946c 730 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
gradyhillhouse 0:806c1dd6946c 731
gradyhillhouse 0:806c1dd6946c 732 /* -------------------- deep space initialization ------------ */
gradyhillhouse 0:806c1dd6946c 733 irez = 0;
gradyhillhouse 0:806c1dd6946c 734 if ((nm < 0.0052359877) && (nm > 0.0034906585))
gradyhillhouse 0:806c1dd6946c 735 irez = 1;
gradyhillhouse 0:806c1dd6946c 736 if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5))
gradyhillhouse 0:806c1dd6946c 737 irez = 2;
gradyhillhouse 0:806c1dd6946c 738
gradyhillhouse 0:806c1dd6946c 739 /* ------------------------ do solar terms ------------------- */
gradyhillhouse 0:806c1dd6946c 740 ses = ss1 * zns * ss5;
gradyhillhouse 0:806c1dd6946c 741 sis = ss2 * zns * (sz11 + sz13);
gradyhillhouse 0:806c1dd6946c 742 sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq);
gradyhillhouse 0:806c1dd6946c 743 sghs = ss4 * zns * (sz31 + sz33 - 6.0);
gradyhillhouse 0:806c1dd6946c 744 shs = -zns * ss2 * (sz21 + sz23);
gradyhillhouse 0:806c1dd6946c 745 // sgp4fix for 180 deg incl
gradyhillhouse 0:806c1dd6946c 746 if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2))
gradyhillhouse 0:806c1dd6946c 747 shs = 0.0;
gradyhillhouse 0:806c1dd6946c 748 if (sinim != 0.0)
gradyhillhouse 0:806c1dd6946c 749 shs = shs / sinim;
gradyhillhouse 0:806c1dd6946c 750 sgs = sghs - cosim * shs;
gradyhillhouse 0:806c1dd6946c 751
gradyhillhouse 0:806c1dd6946c 752 /* ------------------------- do lunar terms ------------------ */
gradyhillhouse 0:806c1dd6946c 753 dedt = ses + s1 * znl * s5;
gradyhillhouse 0:806c1dd6946c 754 didt = sis + s2 * znl * (z11 + z13);
gradyhillhouse 0:806c1dd6946c 755 dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq);
gradyhillhouse 0:806c1dd6946c 756 sghl = s4 * znl * (z31 + z33 - 6.0);
gradyhillhouse 0:806c1dd6946c 757 shll = -znl * s2 * (z21 + z23);
gradyhillhouse 0:806c1dd6946c 758 // sgp4fix for 180 deg incl
gradyhillhouse 0:806c1dd6946c 759 if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2))
gradyhillhouse 0:806c1dd6946c 760 shll = 0.0;
gradyhillhouse 0:806c1dd6946c 761 domdt = sgs + sghl;
gradyhillhouse 0:806c1dd6946c 762 dnodt = shs;
gradyhillhouse 0:806c1dd6946c 763 if (sinim != 0.0)
gradyhillhouse 0:806c1dd6946c 764 {
gradyhillhouse 0:806c1dd6946c 765 domdt = domdt - cosim / sinim * shll;
gradyhillhouse 0:806c1dd6946c 766 dnodt = dnodt + shll / sinim;
gradyhillhouse 0:806c1dd6946c 767 }
gradyhillhouse 0:806c1dd6946c 768
gradyhillhouse 0:806c1dd6946c 769 /* ----------- calculate deep space resonance effects -------- */
gradyhillhouse 0:806c1dd6946c 770 dndt = 0.0;
gradyhillhouse 0:806c1dd6946c 771 theta = fmod(gsto + tc * rptim, twopi);
gradyhillhouse 0:806c1dd6946c 772 em = em + dedt * t;
gradyhillhouse 0:806c1dd6946c 773 inclm = inclm + didt * t;
gradyhillhouse 0:806c1dd6946c 774 argpm = argpm + domdt * t;
gradyhillhouse 0:806c1dd6946c 775 nodem = nodem + dnodt * t;
gradyhillhouse 0:806c1dd6946c 776 mm = mm + dmdt * t;
gradyhillhouse 0:806c1dd6946c 777 // sgp4fix for negative inclinations
gradyhillhouse 0:806c1dd6946c 778 // the following if statement should be commented out
gradyhillhouse 0:806c1dd6946c 779 //if (inclm < 0.0)
gradyhillhouse 0:806c1dd6946c 780 // {
gradyhillhouse 0:806c1dd6946c 781 // inclm = -inclm;
gradyhillhouse 0:806c1dd6946c 782 // argpm = argpm - pi;
gradyhillhouse 0:806c1dd6946c 783 // nodem = nodem + pi;
gradyhillhouse 0:806c1dd6946c 784 // }
gradyhillhouse 0:806c1dd6946c 785
gradyhillhouse 0:806c1dd6946c 786 /* -------------- initialize the resonance terms ------------- */
gradyhillhouse 0:806c1dd6946c 787 if (irez != 0)
gradyhillhouse 0:806c1dd6946c 788 {
gradyhillhouse 0:806c1dd6946c 789 aonv = pow(nm / xke, x2o3);
gradyhillhouse 0:806c1dd6946c 790
gradyhillhouse 0:806c1dd6946c 791 /* ---------- geopotential resonance for 12 hour orbits ------ */
gradyhillhouse 0:806c1dd6946c 792 if (irez == 2)
gradyhillhouse 0:806c1dd6946c 793 {
gradyhillhouse 0:806c1dd6946c 794 cosisq = cosim * cosim;
gradyhillhouse 0:806c1dd6946c 795 emo = em;
gradyhillhouse 0:806c1dd6946c 796 em = ecco;
gradyhillhouse 0:806c1dd6946c 797 emsqo = emsq;
gradyhillhouse 0:806c1dd6946c 798 emsq = eccsq;
gradyhillhouse 0:806c1dd6946c 799 eoc = em * emsq;
gradyhillhouse 0:806c1dd6946c 800 g201 = -0.306 - (em - 0.64) * 0.440;
gradyhillhouse 0:806c1dd6946c 801
gradyhillhouse 0:806c1dd6946c 802 if (em <= 0.65)
gradyhillhouse 0:806c1dd6946c 803 {
gradyhillhouse 0:806c1dd6946c 804 g211 = 3.616 - 13.2470 * em + 16.2900 * emsq;
gradyhillhouse 0:806c1dd6946c 805 g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc;
gradyhillhouse 0:806c1dd6946c 806 g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc;
gradyhillhouse 0:806c1dd6946c 807 g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc;
gradyhillhouse 0:806c1dd6946c 808 g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc;
gradyhillhouse 0:806c1dd6946c 809 g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc;
gradyhillhouse 0:806c1dd6946c 810 }
gradyhillhouse 0:806c1dd6946c 811 else
gradyhillhouse 0:806c1dd6946c 812 {
gradyhillhouse 0:806c1dd6946c 813 g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc;
gradyhillhouse 0:806c1dd6946c 814 g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc;
gradyhillhouse 0:806c1dd6946c 815 g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc;
gradyhillhouse 0:806c1dd6946c 816 g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc;
gradyhillhouse 0:806c1dd6946c 817 g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc;
gradyhillhouse 0:806c1dd6946c 818 if (em > 0.715)
gradyhillhouse 0:806c1dd6946c 819 g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc;
gradyhillhouse 0:806c1dd6946c 820 else
gradyhillhouse 0:806c1dd6946c 821 g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq;
gradyhillhouse 0:806c1dd6946c 822 }
gradyhillhouse 0:806c1dd6946c 823 if (em < 0.7)
gradyhillhouse 0:806c1dd6946c 824 {
gradyhillhouse 0:806c1dd6946c 825 g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc;
gradyhillhouse 0:806c1dd6946c 826 g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc;
gradyhillhouse 0:806c1dd6946c 827 g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc;
gradyhillhouse 0:806c1dd6946c 828 }
gradyhillhouse 0:806c1dd6946c 829 else
gradyhillhouse 0:806c1dd6946c 830 {
gradyhillhouse 0:806c1dd6946c 831 g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc;
gradyhillhouse 0:806c1dd6946c 832 g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc;
gradyhillhouse 0:806c1dd6946c 833 g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc;
gradyhillhouse 0:806c1dd6946c 834 }
gradyhillhouse 0:806c1dd6946c 835
gradyhillhouse 0:806c1dd6946c 836 sini2= sinim * sinim;
gradyhillhouse 0:806c1dd6946c 837 f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq);
gradyhillhouse 0:806c1dd6946c 838 f221 = 1.5 * sini2;
gradyhillhouse 0:806c1dd6946c 839 f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq);
gradyhillhouse 0:806c1dd6946c 840 f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq);
gradyhillhouse 0:806c1dd6946c 841 f441 = 35.0 * sini2 * f220;
gradyhillhouse 0:806c1dd6946c 842 f442 = 39.3750 * sini2 * sini2;
gradyhillhouse 0:806c1dd6946c 843 f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) +
gradyhillhouse 0:806c1dd6946c 844 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) );
gradyhillhouse 0:806c1dd6946c 845 f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim +
gradyhillhouse 0:806c1dd6946c 846 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq));
gradyhillhouse 0:806c1dd6946c 847 f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq *
gradyhillhouse 0:806c1dd6946c 848 (-12.0 + 8.0 * cosim + 10.0 * cosisq));
gradyhillhouse 0:806c1dd6946c 849 f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq *
gradyhillhouse 0:806c1dd6946c 850 (12.0 + 8.0 * cosim - 10.0 * cosisq));
gradyhillhouse 0:806c1dd6946c 851 xno2 = nm * nm;
gradyhillhouse 0:806c1dd6946c 852 ainv2 = aonv * aonv;
gradyhillhouse 0:806c1dd6946c 853 temp1 = 3.0 * xno2 * ainv2;
gradyhillhouse 0:806c1dd6946c 854 temp = temp1 * root22;
gradyhillhouse 0:806c1dd6946c 855 d2201 = temp * f220 * g201;
gradyhillhouse 0:806c1dd6946c 856 d2211 = temp * f221 * g211;
gradyhillhouse 0:806c1dd6946c 857 temp1 = temp1 * aonv;
gradyhillhouse 0:806c1dd6946c 858 temp = temp1 * root32;
gradyhillhouse 0:806c1dd6946c 859 d3210 = temp * f321 * g310;
gradyhillhouse 0:806c1dd6946c 860 d3222 = temp * f322 * g322;
gradyhillhouse 0:806c1dd6946c 861 temp1 = temp1 * aonv;
gradyhillhouse 0:806c1dd6946c 862 temp = 2.0 * temp1 * root44;
gradyhillhouse 0:806c1dd6946c 863 d4410 = temp * f441 * g410;
gradyhillhouse 0:806c1dd6946c 864 d4422 = temp * f442 * g422;
gradyhillhouse 0:806c1dd6946c 865 temp1 = temp1 * aonv;
gradyhillhouse 0:806c1dd6946c 866 temp = temp1 * root52;
gradyhillhouse 0:806c1dd6946c 867 d5220 = temp * f522 * g520;
gradyhillhouse 0:806c1dd6946c 868 d5232 = temp * f523 * g532;
gradyhillhouse 0:806c1dd6946c 869 temp = 2.0 * temp1 * root54;
gradyhillhouse 0:806c1dd6946c 870 d5421 = temp * f542 * g521;
gradyhillhouse 0:806c1dd6946c 871 d5433 = temp * f543 * g533;
gradyhillhouse 0:806c1dd6946c 872 xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi);
gradyhillhouse 0:806c1dd6946c 873 xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no;
gradyhillhouse 0:806c1dd6946c 874 em = emo;
gradyhillhouse 0:806c1dd6946c 875 emsq = emsqo;
gradyhillhouse 0:806c1dd6946c 876 }
gradyhillhouse 0:806c1dd6946c 877
gradyhillhouse 0:806c1dd6946c 878 /* ---------------- synchronous resonance terms -------------- */
gradyhillhouse 0:806c1dd6946c 879 if (irez == 1)
gradyhillhouse 0:806c1dd6946c 880 {
gradyhillhouse 0:806c1dd6946c 881 g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq);
gradyhillhouse 0:806c1dd6946c 882 g310 = 1.0 + 2.0 * emsq;
gradyhillhouse 0:806c1dd6946c 883 g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq);
gradyhillhouse 0:806c1dd6946c 884 f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim);
gradyhillhouse 0:806c1dd6946c 885 f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim);
gradyhillhouse 0:806c1dd6946c 886 f330 = 1.0 + cosim;
gradyhillhouse 0:806c1dd6946c 887 f330 = 1.875 * f330 * f330 * f330;
gradyhillhouse 0:806c1dd6946c 888 del1 = 3.0 * nm * nm * aonv * aonv;
gradyhillhouse 0:806c1dd6946c 889 del2 = 2.0 * del1 * f220 * g200 * q22;
gradyhillhouse 0:806c1dd6946c 890 del3 = 3.0 * del1 * f330 * g300 * q33 * aonv;
gradyhillhouse 0:806c1dd6946c 891 del1 = del1 * f311 * g310 * q31 * aonv;
gradyhillhouse 0:806c1dd6946c 892 xlamo = fmod(mo + nodeo + argpo - theta, twopi);
gradyhillhouse 0:806c1dd6946c 893 xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no;
gradyhillhouse 0:806c1dd6946c 894 }
gradyhillhouse 0:806c1dd6946c 895
gradyhillhouse 0:806c1dd6946c 896 /* ------------ for sgp4, initialize the integrator ---------- */
gradyhillhouse 0:806c1dd6946c 897 xli = xlamo;
gradyhillhouse 0:806c1dd6946c 898 xni = no;
gradyhillhouse 0:806c1dd6946c 899 atime = 0.0;
gradyhillhouse 0:806c1dd6946c 900 nm = no + dndt;
gradyhillhouse 0:806c1dd6946c 901 }
gradyhillhouse 0:806c1dd6946c 902
gradyhillhouse 0:806c1dd6946c 903 //#include "debug3.cpp"
gradyhillhouse 0:806c1dd6946c 904 } // end dsinit
gradyhillhouse 0:806c1dd6946c 905
gradyhillhouse 0:806c1dd6946c 906 /*-----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 907 *
gradyhillhouse 0:806c1dd6946c 908 * procedure dspace
gradyhillhouse 0:806c1dd6946c 909 *
gradyhillhouse 0:806c1dd6946c 910 * this procedure provides deep space contributions to mean elements for
gradyhillhouse 0:806c1dd6946c 911 * perturbing third body. these effects have been averaged over one
gradyhillhouse 0:806c1dd6946c 912 * revolution of the sun and moon. for earth resonance effects, the
gradyhillhouse 0:806c1dd6946c 913 * effects have been averaged over no revolutions of the satellite.
gradyhillhouse 0:806c1dd6946c 914 * (mean motion)
gradyhillhouse 0:806c1dd6946c 915 *
gradyhillhouse 0:806c1dd6946c 916 * author : david vallado 719-573-2600 28 jun 2005
gradyhillhouse 0:806c1dd6946c 917 *
gradyhillhouse 0:806c1dd6946c 918 * inputs :
gradyhillhouse 0:806c1dd6946c 919 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 -
gradyhillhouse 0:806c1dd6946c 920 * dedt -
gradyhillhouse 0:806c1dd6946c 921 * del1, del2, del3 -
gradyhillhouse 0:806c1dd6946c 922 * didt -
gradyhillhouse 0:806c1dd6946c 923 * dmdt -
gradyhillhouse 0:806c1dd6946c 924 * dnodt -
gradyhillhouse 0:806c1dd6946c 925 * domdt -
gradyhillhouse 0:806c1dd6946c 926 * irez - flag for resonance 0-none, 1-one day, 2-half day
gradyhillhouse 0:806c1dd6946c 927 * argpo - argument of perigee
gradyhillhouse 0:806c1dd6946c 928 * argpdot - argument of perigee dot (rate)
gradyhillhouse 0:806c1dd6946c 929 * t - time
gradyhillhouse 0:806c1dd6946c 930 * tc -
gradyhillhouse 0:806c1dd6946c 931 * gsto - gst
gradyhillhouse 0:806c1dd6946c 932 * xfact -
gradyhillhouse 0:806c1dd6946c 933 * xlamo -
gradyhillhouse 0:806c1dd6946c 934 * no - mean motion
gradyhillhouse 0:806c1dd6946c 935 * atime -
gradyhillhouse 0:806c1dd6946c 936 * em - eccentricity
gradyhillhouse 0:806c1dd6946c 937 * ft -
gradyhillhouse 0:806c1dd6946c 938 * argpm - argument of perigee
gradyhillhouse 0:806c1dd6946c 939 * inclm - inclination
gradyhillhouse 0:806c1dd6946c 940 * xli -
gradyhillhouse 0:806c1dd6946c 941 * mm - mean anomaly
gradyhillhouse 0:806c1dd6946c 942 * xni - mean motion
gradyhillhouse 0:806c1dd6946c 943 * nodem - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 944 *
gradyhillhouse 0:806c1dd6946c 945 * outputs :
gradyhillhouse 0:806c1dd6946c 946 * atime -
gradyhillhouse 0:806c1dd6946c 947 * em - eccentricity
gradyhillhouse 0:806c1dd6946c 948 * argpm - argument of perigee
gradyhillhouse 0:806c1dd6946c 949 * inclm - inclination
gradyhillhouse 0:806c1dd6946c 950 * xli -
gradyhillhouse 0:806c1dd6946c 951 * mm - mean anomaly
gradyhillhouse 0:806c1dd6946c 952 * xni -
gradyhillhouse 0:806c1dd6946c 953 * nodem - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 954 * dndt -
gradyhillhouse 0:806c1dd6946c 955 * nm - mean motion
gradyhillhouse 0:806c1dd6946c 956 *
gradyhillhouse 0:806c1dd6946c 957 * locals :
gradyhillhouse 0:806c1dd6946c 958 * delt -
gradyhillhouse 0:806c1dd6946c 959 * ft -
gradyhillhouse 0:806c1dd6946c 960 * theta -
gradyhillhouse 0:806c1dd6946c 961 * x2li -
gradyhillhouse 0:806c1dd6946c 962 * x2omi -
gradyhillhouse 0:806c1dd6946c 963 * xl -
gradyhillhouse 0:806c1dd6946c 964 * xldot -
gradyhillhouse 0:806c1dd6946c 965 * xnddt -
gradyhillhouse 0:806c1dd6946c 966 * xndt -
gradyhillhouse 0:806c1dd6946c 967 * xomi -
gradyhillhouse 0:806c1dd6946c 968 *
gradyhillhouse 0:806c1dd6946c 969 * coupling :
gradyhillhouse 0:806c1dd6946c 970 * none -
gradyhillhouse 0:806c1dd6946c 971 *
gradyhillhouse 0:806c1dd6946c 972 * references :
gradyhillhouse 0:806c1dd6946c 973 * hoots, roehrich, norad spacetrack report #3 1980
gradyhillhouse 0:806c1dd6946c 974 * hoots, norad spacetrack report #6 1986
gradyhillhouse 0:806c1dd6946c 975 * hoots, schumacher and glover 2004
gradyhillhouse 0:806c1dd6946c 976 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 977 ----------------------------------------------------------------------------*/
gradyhillhouse 0:806c1dd6946c 978
gradyhillhouse 0:806c1dd6946c 979 static void dspace
gradyhillhouse 0:806c1dd6946c 980 (
gradyhillhouse 0:806c1dd6946c 981 int irez,
gradyhillhouse 0:806c1dd6946c 982 double d2201, double d2211, double d3210, double d3222, double d4410,
gradyhillhouse 0:806c1dd6946c 983 double d4422, double d5220, double d5232, double d5421, double d5433,
gradyhillhouse 0:806c1dd6946c 984 double dedt, double del1, double del2, double del3, double didt,
gradyhillhouse 0:806c1dd6946c 985 double dmdt, double dnodt, double domdt, double argpo, double argpdot,
gradyhillhouse 0:806c1dd6946c 986 double t, double tc, double gsto, double xfact, double xlamo,
gradyhillhouse 0:806c1dd6946c 987 double no,
gradyhillhouse 0:806c1dd6946c 988 double& atime, double& em, double& argpm, double& inclm, double& xli,
gradyhillhouse 0:806c1dd6946c 989 double& mm, double& xni, double& nodem, double& dndt, double& nm
gradyhillhouse 0:806c1dd6946c 990 )
gradyhillhouse 0:806c1dd6946c 991 {
gradyhillhouse 0:806c1dd6946c 992 const double twopi = 2.0 * pi;
gradyhillhouse 0:806c1dd6946c 993 int iretn, iret;
gradyhillhouse 0:806c1dd6946c 994 double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32,
gradyhillhouse 0:806c1dd6946c 995 g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp;
gradyhillhouse 0:806c1dd6946c 996
gradyhillhouse 0:806c1dd6946c 997 fasx2 = 0.13130908;
gradyhillhouse 0:806c1dd6946c 998 fasx4 = 2.8843198;
gradyhillhouse 0:806c1dd6946c 999 fasx6 = 0.37448087;
gradyhillhouse 0:806c1dd6946c 1000 g22 = 5.7686396;
gradyhillhouse 0:806c1dd6946c 1001 g32 = 0.95240898;
gradyhillhouse 0:806c1dd6946c 1002 g44 = 1.8014998;
gradyhillhouse 0:806c1dd6946c 1003 g52 = 1.0508330;
gradyhillhouse 0:806c1dd6946c 1004 g54 = 4.4108898;
gradyhillhouse 0:806c1dd6946c 1005 rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
gradyhillhouse 0:806c1dd6946c 1006 stepp = 720.0;
gradyhillhouse 0:806c1dd6946c 1007 stepn = -720.0;
gradyhillhouse 0:806c1dd6946c 1008 step2 = 259200.0;
gradyhillhouse 0:806c1dd6946c 1009
gradyhillhouse 0:806c1dd6946c 1010 /* ----------- calculate deep space resonance effects ----------- */
gradyhillhouse 0:806c1dd6946c 1011 dndt = 0.0;
gradyhillhouse 0:806c1dd6946c 1012 theta = fmod(gsto + tc * rptim, twopi);
gradyhillhouse 0:806c1dd6946c 1013 em = em + dedt * t;
gradyhillhouse 0:806c1dd6946c 1014
gradyhillhouse 0:806c1dd6946c 1015 inclm = inclm + didt * t;
gradyhillhouse 0:806c1dd6946c 1016 argpm = argpm + domdt * t;
gradyhillhouse 0:806c1dd6946c 1017 nodem = nodem + dnodt * t;
gradyhillhouse 0:806c1dd6946c 1018 mm = mm + dmdt * t;
gradyhillhouse 0:806c1dd6946c 1019
gradyhillhouse 0:806c1dd6946c 1020 // sgp4fix for negative inclinations
gradyhillhouse 0:806c1dd6946c 1021 // the following if statement should be commented out
gradyhillhouse 0:806c1dd6946c 1022 // if (inclm < 0.0)
gradyhillhouse 0:806c1dd6946c 1023 // {
gradyhillhouse 0:806c1dd6946c 1024 // inclm = -inclm;
gradyhillhouse 0:806c1dd6946c 1025 // argpm = argpm - pi;
gradyhillhouse 0:806c1dd6946c 1026 // nodem = nodem + pi;
gradyhillhouse 0:806c1dd6946c 1027 // }
gradyhillhouse 0:806c1dd6946c 1028
gradyhillhouse 0:806c1dd6946c 1029 /* - update resonances : numerical (euler-maclaurin) integration - */
gradyhillhouse 0:806c1dd6946c 1030 /* ------------------------- epoch restart ---------------------- */
gradyhillhouse 0:806c1dd6946c 1031 // sgp4fix for propagator problems
gradyhillhouse 0:806c1dd6946c 1032 // the following integration works for negative time steps and periods
gradyhillhouse 0:806c1dd6946c 1033 // the specific changes are unknown because the original code was so convoluted
gradyhillhouse 0:806c1dd6946c 1034
gradyhillhouse 0:806c1dd6946c 1035 // sgp4fix take out atime = 0.0 and fix for faster operation
gradyhillhouse 0:806c1dd6946c 1036 ft = 0.0;
gradyhillhouse 0:806c1dd6946c 1037 if (irez != 0)
gradyhillhouse 0:806c1dd6946c 1038 {
gradyhillhouse 0:806c1dd6946c 1039 // sgp4fix streamline check
gradyhillhouse 0:806c1dd6946c 1040 if ((atime == 0.0) || (t * atime <= 0.0) || (fabs(t) < fabs(atime)) )
gradyhillhouse 0:806c1dd6946c 1041 {
gradyhillhouse 0:806c1dd6946c 1042 atime = 0.0;
gradyhillhouse 0:806c1dd6946c 1043 xni = no;
gradyhillhouse 0:806c1dd6946c 1044 xli = xlamo;
gradyhillhouse 0:806c1dd6946c 1045 }
gradyhillhouse 0:806c1dd6946c 1046 // sgp4fix move check outside loop
gradyhillhouse 0:806c1dd6946c 1047 if (t > 0.0)
gradyhillhouse 0:806c1dd6946c 1048 delt = stepp;
gradyhillhouse 0:806c1dd6946c 1049 else
gradyhillhouse 0:806c1dd6946c 1050 delt = stepn;
gradyhillhouse 0:806c1dd6946c 1051
gradyhillhouse 0:806c1dd6946c 1052 iretn = 381; // added for do loop
gradyhillhouse 0:806c1dd6946c 1053 iret = 0; // added for loop
gradyhillhouse 0:806c1dd6946c 1054 while (iretn == 381)
gradyhillhouse 0:806c1dd6946c 1055 {
gradyhillhouse 0:806c1dd6946c 1056 /* ------------------- dot terms calculated ------------- */
gradyhillhouse 0:806c1dd6946c 1057 /* ----------- near - synchronous resonance terms ------- */
gradyhillhouse 0:806c1dd6946c 1058 if (irez != 2)
gradyhillhouse 0:806c1dd6946c 1059 {
gradyhillhouse 0:806c1dd6946c 1060 xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) +
gradyhillhouse 0:806c1dd6946c 1061 del3 * sin(3.0 * (xli - fasx6));
gradyhillhouse 0:806c1dd6946c 1062 xldot = xni + xfact;
gradyhillhouse 0:806c1dd6946c 1063 xnddt = del1 * cos(xli - fasx2) +
gradyhillhouse 0:806c1dd6946c 1064 2.0 * del2 * cos(2.0 * (xli - fasx4)) +
gradyhillhouse 0:806c1dd6946c 1065 3.0 * del3 * cos(3.0 * (xli - fasx6));
gradyhillhouse 0:806c1dd6946c 1066 xnddt = xnddt * xldot;
gradyhillhouse 0:806c1dd6946c 1067 }
gradyhillhouse 0:806c1dd6946c 1068 else
gradyhillhouse 0:806c1dd6946c 1069 {
gradyhillhouse 0:806c1dd6946c 1070 /* --------- near - half-day resonance terms -------- */
gradyhillhouse 0:806c1dd6946c 1071 xomi = argpo + argpdot * atime;
gradyhillhouse 0:806c1dd6946c 1072 x2omi = xomi + xomi;
gradyhillhouse 0:806c1dd6946c 1073 x2li = xli + xli;
gradyhillhouse 0:806c1dd6946c 1074 xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) +
gradyhillhouse 0:806c1dd6946c 1075 d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+
gradyhillhouse 0:806c1dd6946c 1076 d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) +
gradyhillhouse 0:806c1dd6946c 1077 d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+
gradyhillhouse 0:806c1dd6946c 1078 d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54);
gradyhillhouse 0:806c1dd6946c 1079 xldot = xni + xfact;
gradyhillhouse 0:806c1dd6946c 1080 xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) +
gradyhillhouse 0:806c1dd6946c 1081 d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) +
gradyhillhouse 0:806c1dd6946c 1082 d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) +
gradyhillhouse 0:806c1dd6946c 1083 2.0 * (d4410 * cos(x2omi + x2li - g44) +
gradyhillhouse 0:806c1dd6946c 1084 d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) +
gradyhillhouse 0:806c1dd6946c 1085 d5433 * cos(-xomi + x2li - g54));
gradyhillhouse 0:806c1dd6946c 1086 xnddt = xnddt * xldot;
gradyhillhouse 0:806c1dd6946c 1087 }
gradyhillhouse 0:806c1dd6946c 1088
gradyhillhouse 0:806c1dd6946c 1089 /* ----------------------- integrator ------------------- */
gradyhillhouse 0:806c1dd6946c 1090 // sgp4fix move end checks to end of routine
gradyhillhouse 0:806c1dd6946c 1091 if (fabs(t - atime) >= stepp)
gradyhillhouse 0:806c1dd6946c 1092 {
gradyhillhouse 0:806c1dd6946c 1093 iret = 0;
gradyhillhouse 0:806c1dd6946c 1094 iretn = 381;
gradyhillhouse 0:806c1dd6946c 1095 }
gradyhillhouse 0:806c1dd6946c 1096 else // exit here
gradyhillhouse 0:806c1dd6946c 1097 {
gradyhillhouse 0:806c1dd6946c 1098 ft = t - atime;
gradyhillhouse 0:806c1dd6946c 1099 iretn = 0;
gradyhillhouse 0:806c1dd6946c 1100 }
gradyhillhouse 0:806c1dd6946c 1101
gradyhillhouse 0:806c1dd6946c 1102 if (iretn == 381)
gradyhillhouse 0:806c1dd6946c 1103 {
gradyhillhouse 0:806c1dd6946c 1104 xli = xli + xldot * delt + xndt * step2;
gradyhillhouse 0:806c1dd6946c 1105 xni = xni + xndt * delt + xnddt * step2;
gradyhillhouse 0:806c1dd6946c 1106 atime = atime + delt;
gradyhillhouse 0:806c1dd6946c 1107 }
gradyhillhouse 0:806c1dd6946c 1108 } // while iretn = 381
gradyhillhouse 0:806c1dd6946c 1109
gradyhillhouse 0:806c1dd6946c 1110 nm = xni + xndt * ft + xnddt * ft * ft * 0.5;
gradyhillhouse 0:806c1dd6946c 1111 xl = xli + xldot * ft + xndt * ft * ft * 0.5;
gradyhillhouse 0:806c1dd6946c 1112 if (irez != 1)
gradyhillhouse 0:806c1dd6946c 1113 {
gradyhillhouse 0:806c1dd6946c 1114 mm = xl - 2.0 * nodem + 2.0 * theta;
gradyhillhouse 0:806c1dd6946c 1115 dndt = nm - no;
gradyhillhouse 0:806c1dd6946c 1116 }
gradyhillhouse 0:806c1dd6946c 1117 else
gradyhillhouse 0:806c1dd6946c 1118 {
gradyhillhouse 0:806c1dd6946c 1119 mm = xl - nodem - argpm + theta;
gradyhillhouse 0:806c1dd6946c 1120 dndt = nm - no;
gradyhillhouse 0:806c1dd6946c 1121 }
gradyhillhouse 0:806c1dd6946c 1122 nm = no + dndt;
gradyhillhouse 0:806c1dd6946c 1123 }
gradyhillhouse 0:806c1dd6946c 1124
gradyhillhouse 0:806c1dd6946c 1125 //#include "debug4.cpp"
gradyhillhouse 0:806c1dd6946c 1126 } // end dsspace
gradyhillhouse 0:806c1dd6946c 1127
gradyhillhouse 0:806c1dd6946c 1128 /*-----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 1129 *
gradyhillhouse 0:806c1dd6946c 1130 * procedure initl
gradyhillhouse 0:806c1dd6946c 1131 *
gradyhillhouse 0:806c1dd6946c 1132 * this procedure initializes the spg4 propagator. all the initialization is
gradyhillhouse 0:806c1dd6946c 1133 * consolidated here instead of having multiple loops inside other routines.
gradyhillhouse 0:806c1dd6946c 1134 *
gradyhillhouse 0:806c1dd6946c 1135 * author : david vallado 719-573-2600 28 jun 2005
gradyhillhouse 0:806c1dd6946c 1136 *
gradyhillhouse 0:806c1dd6946c 1137 * inputs :
gradyhillhouse 0:806c1dd6946c 1138 * ecco - eccentricity 0.0 - 1.0
gradyhillhouse 0:806c1dd6946c 1139 * epoch - epoch time in days from jan 0, 1950. 0 hr
gradyhillhouse 0:806c1dd6946c 1140 * inclo - inclination of satellite
gradyhillhouse 0:806c1dd6946c 1141 * no - mean motion of satellite
gradyhillhouse 0:806c1dd6946c 1142 * satn - satellite number
gradyhillhouse 0:806c1dd6946c 1143 *
gradyhillhouse 0:806c1dd6946c 1144 * outputs :
gradyhillhouse 0:806c1dd6946c 1145 * ainv - 1.0 / a
gradyhillhouse 0:806c1dd6946c 1146 * ao - semi major axis
gradyhillhouse 0:806c1dd6946c 1147 * con41 -
gradyhillhouse 0:806c1dd6946c 1148 * con42 - 1.0 - 5.0 cos(i)
gradyhillhouse 0:806c1dd6946c 1149 * cosio - cosine of inclination
gradyhillhouse 0:806c1dd6946c 1150 * cosio2 - cosio squared
gradyhillhouse 0:806c1dd6946c 1151 * eccsq - eccentricity squared
gradyhillhouse 0:806c1dd6946c 1152 * method - flag for deep space 'd', 'n'
gradyhillhouse 0:806c1dd6946c 1153 * omeosq - 1.0 - ecco * ecco
gradyhillhouse 0:806c1dd6946c 1154 * posq - semi-parameter squared
gradyhillhouse 0:806c1dd6946c 1155 * rp - radius of perigee
gradyhillhouse 0:806c1dd6946c 1156 * rteosq - square root of (1.0 - ecco*ecco)
gradyhillhouse 0:806c1dd6946c 1157 * sinio - sine of inclination
gradyhillhouse 0:806c1dd6946c 1158 * gsto - gst at time of observation rad
gradyhillhouse 0:806c1dd6946c 1159 * no - mean motion of satellite
gradyhillhouse 0:806c1dd6946c 1160 *
gradyhillhouse 0:806c1dd6946c 1161 * locals :
gradyhillhouse 0:806c1dd6946c 1162 * ak -
gradyhillhouse 0:806c1dd6946c 1163 * d1 -
gradyhillhouse 0:806c1dd6946c 1164 * del -
gradyhillhouse 0:806c1dd6946c 1165 * adel -
gradyhillhouse 0:806c1dd6946c 1166 * po -
gradyhillhouse 0:806c1dd6946c 1167 *
gradyhillhouse 0:806c1dd6946c 1168 * coupling :
gradyhillhouse 0:806c1dd6946c 1169 * getgravconst
gradyhillhouse 0:806c1dd6946c 1170 * gstime - find greenwich sidereal time from the julian date
gradyhillhouse 0:806c1dd6946c 1171 *
gradyhillhouse 0:806c1dd6946c 1172 * references :
gradyhillhouse 0:806c1dd6946c 1173 * hoots, roehrich, norad spacetrack report #3 1980
gradyhillhouse 0:806c1dd6946c 1174 * hoots, norad spacetrack report #6 1986
gradyhillhouse 0:806c1dd6946c 1175 * hoots, schumacher and glover 2004
gradyhillhouse 0:806c1dd6946c 1176 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 1177 ----------------------------------------------------------------------------*/
gradyhillhouse 0:806c1dd6946c 1178
gradyhillhouse 0:806c1dd6946c 1179 static void initl
gradyhillhouse 0:806c1dd6946c 1180 (
gradyhillhouse 0:806c1dd6946c 1181 int satn, gravconsttype whichconst,
gradyhillhouse 0:806c1dd6946c 1182 double ecco, double epoch, double inclo, double& no,
gradyhillhouse 0:806c1dd6946c 1183 char& method,
gradyhillhouse 0:806c1dd6946c 1184 double& ainv, double& ao, double& con41, double& con42, double& cosio,
gradyhillhouse 0:806c1dd6946c 1185 double& cosio2,double& eccsq, double& omeosq, double& posq,
gradyhillhouse 0:806c1dd6946c 1186 double& rp, double& rteosq,double& sinio , double& gsto,
gradyhillhouse 0:806c1dd6946c 1187 char opsmode
gradyhillhouse 0:806c1dd6946c 1188 )
gradyhillhouse 0:806c1dd6946c 1189 {
gradyhillhouse 0:806c1dd6946c 1190 /* --------------------- local variables ------------------------ */
gradyhillhouse 0:806c1dd6946c 1191 double ak, d1, del, adel, po, x2o3, j2, xke,
gradyhillhouse 0:806c1dd6946c 1192 tumin, mu, radiusearthkm, j3, j4, j3oj2;
gradyhillhouse 0:806c1dd6946c 1193
gradyhillhouse 0:806c1dd6946c 1194 // sgp4fix use old way of finding gst
gradyhillhouse 0:806c1dd6946c 1195 double ds70;
gradyhillhouse 0:806c1dd6946c 1196 double ts70, tfrac, c1, thgr70, fk5r, c1p2p;
gradyhillhouse 0:806c1dd6946c 1197 const double twopi = 2.0 * pi;
gradyhillhouse 0:806c1dd6946c 1198
gradyhillhouse 0:806c1dd6946c 1199 /* ----------------------- earth constants ---------------------- */
gradyhillhouse 0:806c1dd6946c 1200 // sgp4fix identify constants and allow alternate values
gradyhillhouse 0:806c1dd6946c 1201 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
gradyhillhouse 0:806c1dd6946c 1202 x2o3 = 2.0 / 3.0;
gradyhillhouse 0:806c1dd6946c 1203
gradyhillhouse 0:806c1dd6946c 1204 /* ------------- calculate auxillary epoch quantities ---------- */
gradyhillhouse 0:806c1dd6946c 1205 eccsq = ecco * ecco;
gradyhillhouse 0:806c1dd6946c 1206 omeosq = 1.0 - eccsq;
gradyhillhouse 0:806c1dd6946c 1207 rteosq = sqrt(omeosq);
gradyhillhouse 0:806c1dd6946c 1208 cosio = cos(inclo);
gradyhillhouse 0:806c1dd6946c 1209 cosio2 = cosio * cosio;
gradyhillhouse 0:806c1dd6946c 1210
gradyhillhouse 0:806c1dd6946c 1211 /* ------------------ un-kozai the mean motion ----------------- */
gradyhillhouse 0:806c1dd6946c 1212 ak = pow(xke / no, x2o3);
gradyhillhouse 0:806c1dd6946c 1213 d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq);
gradyhillhouse 0:806c1dd6946c 1214 del = d1 / (ak * ak);
gradyhillhouse 0:806c1dd6946c 1215 adel = ak * (1.0 - del * del - del *
gradyhillhouse 0:806c1dd6946c 1216 (1.0 / 3.0 + 134.0 * del * del / 81.0));
gradyhillhouse 0:806c1dd6946c 1217 del = d1/(adel * adel);
gradyhillhouse 0:806c1dd6946c 1218 no = no / (1.0 + del);
gradyhillhouse 0:806c1dd6946c 1219
gradyhillhouse 0:806c1dd6946c 1220 ao = pow(xke / no, x2o3);
gradyhillhouse 0:806c1dd6946c 1221 sinio = sin(inclo);
gradyhillhouse 0:806c1dd6946c 1222 po = ao * omeosq;
gradyhillhouse 0:806c1dd6946c 1223 con42 = 1.0 - 5.0 * cosio2;
gradyhillhouse 0:806c1dd6946c 1224 con41 = -con42-cosio2-cosio2;
gradyhillhouse 0:806c1dd6946c 1225 ainv = 1.0 / ao;
gradyhillhouse 0:806c1dd6946c 1226 posq = po * po;
gradyhillhouse 0:806c1dd6946c 1227 rp = ao * (1.0 - ecco);
gradyhillhouse 0:806c1dd6946c 1228 method = 'n';
gradyhillhouse 0:806c1dd6946c 1229
gradyhillhouse 0:806c1dd6946c 1230 // sgp4fix modern approach to finding sidereal time
gradyhillhouse 0:806c1dd6946c 1231 if (opsmode == 'a')
gradyhillhouse 0:806c1dd6946c 1232 {
gradyhillhouse 0:806c1dd6946c 1233 // sgp4fix use old way of finding gst
gradyhillhouse 0:806c1dd6946c 1234 // count integer number of days from 0 jan 1970
gradyhillhouse 0:806c1dd6946c 1235 ts70 = epoch - 7305.0;
gradyhillhouse 0:806c1dd6946c 1236 ds70 = floor(ts70 + 1.0e-8);
gradyhillhouse 0:806c1dd6946c 1237 tfrac = ts70 - ds70;
gradyhillhouse 0:806c1dd6946c 1238 // find greenwich location at epoch
gradyhillhouse 0:806c1dd6946c 1239 c1 = 1.72027916940703639e-2;
gradyhillhouse 0:806c1dd6946c 1240 thgr70= 1.7321343856509374;
gradyhillhouse 0:806c1dd6946c 1241 fk5r = 5.07551419432269442e-15;
gradyhillhouse 0:806c1dd6946c 1242 c1p2p = c1 + twopi;
gradyhillhouse 0:806c1dd6946c 1243 gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi);
gradyhillhouse 0:806c1dd6946c 1244 if ( gsto < 0.0 )
gradyhillhouse 0:806c1dd6946c 1245 gsto = gsto + twopi;
gradyhillhouse 0:806c1dd6946c 1246 }
gradyhillhouse 0:806c1dd6946c 1247 else
gradyhillhouse 0:806c1dd6946c 1248 gsto = gstime(epoch + 2433281.5);
gradyhillhouse 0:806c1dd6946c 1249
gradyhillhouse 0:806c1dd6946c 1250
gradyhillhouse 0:806c1dd6946c 1251 //#include "debug5.cpp"
gradyhillhouse 0:806c1dd6946c 1252 } // end initl
gradyhillhouse 0:806c1dd6946c 1253
gradyhillhouse 0:806c1dd6946c 1254 /*-----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 1255 *
gradyhillhouse 0:806c1dd6946c 1256 * procedure sgp4init
gradyhillhouse 0:806c1dd6946c 1257 *
gradyhillhouse 0:806c1dd6946c 1258 * this procedure initializes variables for sgp4.
gradyhillhouse 0:806c1dd6946c 1259 *
gradyhillhouse 0:806c1dd6946c 1260 * author : david vallado 719-573-2600 28 jun 2005
gradyhillhouse 0:806c1dd6946c 1261 *
gradyhillhouse 0:806c1dd6946c 1262 * inputs :
gradyhillhouse 0:806c1dd6946c 1263 * opsmode - mode of operation afspc or improved 'a', 'i'
gradyhillhouse 0:806c1dd6946c 1264 * whichconst - which set of constants to use 72, 84
gradyhillhouse 0:806c1dd6946c 1265 * satn - satellite number
gradyhillhouse 0:806c1dd6946c 1266 * bstar - sgp4 type drag coefficient kg/m2er
gradyhillhouse 0:806c1dd6946c 1267 * ecco - eccentricity
gradyhillhouse 0:806c1dd6946c 1268 * epoch - epoch time in days from jan 0, 1950. 0 hr
gradyhillhouse 0:806c1dd6946c 1269 * argpo - argument of perigee (output if ds)
gradyhillhouse 0:806c1dd6946c 1270 * inclo - inclination
gradyhillhouse 0:806c1dd6946c 1271 * mo - mean anomaly (output if ds)
gradyhillhouse 0:806c1dd6946c 1272 * no - mean motion
gradyhillhouse 0:806c1dd6946c 1273 * nodeo - right ascension of ascending node
gradyhillhouse 0:806c1dd6946c 1274 *
gradyhillhouse 0:806c1dd6946c 1275 * outputs :
gradyhillhouse 0:806c1dd6946c 1276 * satrec - common values for subsequent calls
gradyhillhouse 0:806c1dd6946c 1277 * return code - non-zero on error.
gradyhillhouse 0:806c1dd6946c 1278 * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
gradyhillhouse 0:806c1dd6946c 1279 * 2 - mean motion less than 0.0
gradyhillhouse 0:806c1dd6946c 1280 * 3 - pert elements, ecc < 0.0 or ecc > 1.0
gradyhillhouse 0:806c1dd6946c 1281 * 4 - semi-latus rectum < 0.0
gradyhillhouse 0:806c1dd6946c 1282 * 5 - epoch elements are sub-orbital
gradyhillhouse 0:806c1dd6946c 1283 * 6 - satellite has decayed
gradyhillhouse 0:806c1dd6946c 1284 *
gradyhillhouse 0:806c1dd6946c 1285 * locals :
gradyhillhouse 0:806c1dd6946c 1286 * cnodm , snodm , cosim , sinim , cosomm , sinomm
gradyhillhouse 0:806c1dd6946c 1287 * cc1sq , cc2 , cc3
gradyhillhouse 0:806c1dd6946c 1288 * coef , coef1
gradyhillhouse 0:806c1dd6946c 1289 * cosio4 -
gradyhillhouse 0:806c1dd6946c 1290 * day -
gradyhillhouse 0:806c1dd6946c 1291 * dndt -
gradyhillhouse 0:806c1dd6946c 1292 * em - eccentricity
gradyhillhouse 0:806c1dd6946c 1293 * emsq - eccentricity squared
gradyhillhouse 0:806c1dd6946c 1294 * eeta -
gradyhillhouse 0:806c1dd6946c 1295 * etasq -
gradyhillhouse 0:806c1dd6946c 1296 * gam -
gradyhillhouse 0:806c1dd6946c 1297 * argpm - argument of perigee
gradyhillhouse 0:806c1dd6946c 1298 * nodem -
gradyhillhouse 0:806c1dd6946c 1299 * inclm - inclination
gradyhillhouse 0:806c1dd6946c 1300 * mm - mean anomaly
gradyhillhouse 0:806c1dd6946c 1301 * nm - mean motion
gradyhillhouse 0:806c1dd6946c 1302 * perige - perigee
gradyhillhouse 0:806c1dd6946c 1303 * pinvsq -
gradyhillhouse 0:806c1dd6946c 1304 * psisq -
gradyhillhouse 0:806c1dd6946c 1305 * qzms24 -
gradyhillhouse 0:806c1dd6946c 1306 * rtemsq -
gradyhillhouse 0:806c1dd6946c 1307 * s1, s2, s3, s4, s5, s6, s7 -
gradyhillhouse 0:806c1dd6946c 1308 * sfour -
gradyhillhouse 0:806c1dd6946c 1309 * ss1, ss2, ss3, ss4, ss5, ss6, ss7 -
gradyhillhouse 0:806c1dd6946c 1310 * sz1, sz2, sz3
gradyhillhouse 0:806c1dd6946c 1311 * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 -
gradyhillhouse 0:806c1dd6946c 1312 * tc -
gradyhillhouse 0:806c1dd6946c 1313 * temp -
gradyhillhouse 0:806c1dd6946c 1314 * temp1, temp2, temp3 -
gradyhillhouse 0:806c1dd6946c 1315 * tsi -
gradyhillhouse 0:806c1dd6946c 1316 * xpidot -
gradyhillhouse 0:806c1dd6946c 1317 * xhdot1 -
gradyhillhouse 0:806c1dd6946c 1318 * z1, z2, z3 -
gradyhillhouse 0:806c1dd6946c 1319 * z11, z12, z13, z21, z22, z23, z31, z32, z33 -
gradyhillhouse 0:806c1dd6946c 1320 *
gradyhillhouse 0:806c1dd6946c 1321 * coupling :
gradyhillhouse 0:806c1dd6946c 1322 * getgravconst-
gradyhillhouse 0:806c1dd6946c 1323 * initl -
gradyhillhouse 0:806c1dd6946c 1324 * dscom -
gradyhillhouse 0:806c1dd6946c 1325 * dpper -
gradyhillhouse 0:806c1dd6946c 1326 * dsinit -
gradyhillhouse 0:806c1dd6946c 1327 * sgp4 -
gradyhillhouse 0:806c1dd6946c 1328 *
gradyhillhouse 0:806c1dd6946c 1329 * references :
gradyhillhouse 0:806c1dd6946c 1330 * hoots, roehrich, norad spacetrack report #3 1980
gradyhillhouse 0:806c1dd6946c 1331 * hoots, norad spacetrack report #6 1986
gradyhillhouse 0:806c1dd6946c 1332 * hoots, schumacher and glover 2004
gradyhillhouse 0:806c1dd6946c 1333 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 1334 ----------------------------------------------------------------------------*/
gradyhillhouse 0:806c1dd6946c 1335
gradyhillhouse 0:806c1dd6946c 1336 bool sgp4init
gradyhillhouse 0:806c1dd6946c 1337 (
gradyhillhouse 0:806c1dd6946c 1338 gravconsttype whichconst, char opsmode, const int satn, const double epoch,
gradyhillhouse 0:806c1dd6946c 1339 const double xbstar, const double xecco, const double xargpo,
gradyhillhouse 0:806c1dd6946c 1340 const double xinclo, const double xmo, const double xno,
gradyhillhouse 0:806c1dd6946c 1341 const double xnodeo, elsetrec& satrec
gradyhillhouse 0:806c1dd6946c 1342 )
gradyhillhouse 0:806c1dd6946c 1343 {
gradyhillhouse 0:806c1dd6946c 1344 /* --------------------- local variables ------------------------ */
gradyhillhouse 0:806c1dd6946c 1345 double ao, ainv, con42, cosio, sinio, cosio2, eccsq,
gradyhillhouse 0:806c1dd6946c 1346 omeosq, posq, rp, rteosq,
gradyhillhouse 0:806c1dd6946c 1347 cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq ,
gradyhillhouse 0:806c1dd6946c 1348 cc2 , cc3 , coef , coef1 , cosio4, day , dndt ,
gradyhillhouse 0:806c1dd6946c 1349 em , emsq , eeta , etasq , gam , argpm , nodem ,
gradyhillhouse 0:806c1dd6946c 1350 inclm , mm , nm , perige, pinvsq, psisq , qzms24,
gradyhillhouse 0:806c1dd6946c 1351 rtemsq, s1 , s2 , s3 , s4 , s5 , s6 ,
gradyhillhouse 0:806c1dd6946c 1352 s7 , sfour , ss1 , ss2 , ss3 , ss4 , ss5 ,
gradyhillhouse 0:806c1dd6946c 1353 ss6 , ss7 , sz1 , sz2 , sz3 , sz11 , sz12 ,
gradyhillhouse 0:806c1dd6946c 1354 sz13 , sz21 , sz22 , sz23 , sz31 , sz32 , sz33 ,
gradyhillhouse 0:806c1dd6946c 1355 tc , temp , temp1 , temp2 , temp3 , tsi , xpidot,
gradyhillhouse 0:806c1dd6946c 1356 xhdot1, z1 , z2 , z3 , z11 , z12 , z13 ,
gradyhillhouse 0:806c1dd6946c 1357 z21 , z22 , z23 , z31 , z32 , z33,
gradyhillhouse 0:806c1dd6946c 1358 qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3],
gradyhillhouse 0:806c1dd6946c 1359 tumin, mu, radiusearthkm, xke, j3, delmotemp, qzms2ttemp, qzms24temp;
gradyhillhouse 0:806c1dd6946c 1360
gradyhillhouse 0:806c1dd6946c 1361 /* ------------------------ initialization --------------------- */
gradyhillhouse 0:806c1dd6946c 1362 // sgp4fix divisor for divide by zero check on inclination
gradyhillhouse 0:806c1dd6946c 1363 // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
gradyhillhouse 0:806c1dd6946c 1364 // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
gradyhillhouse 0:806c1dd6946c 1365 const double temp4 = 1.5e-12;
gradyhillhouse 0:806c1dd6946c 1366
gradyhillhouse 0:806c1dd6946c 1367 /* ----------- set all near earth variables to zero ------------ */
gradyhillhouse 0:806c1dd6946c 1368 satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0;
gradyhillhouse 0:806c1dd6946c 1369 satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0;
gradyhillhouse 0:806c1dd6946c 1370 satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0;
gradyhillhouse 0:806c1dd6946c 1371 satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0;
gradyhillhouse 0:806c1dd6946c 1372 satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0;
gradyhillhouse 0:806c1dd6946c 1373 satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0;
gradyhillhouse 0:806c1dd6946c 1374 satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0;
gradyhillhouse 0:806c1dd6946c 1375 satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0;
gradyhillhouse 0:806c1dd6946c 1376 satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0;
gradyhillhouse 0:806c1dd6946c 1377
gradyhillhouse 0:806c1dd6946c 1378 /* ----------- set all deep space variables to zero ------------ */
gradyhillhouse 0:806c1dd6946c 1379 satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0;
gradyhillhouse 0:806c1dd6946c 1380 satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0;
gradyhillhouse 0:806c1dd6946c 1381 satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0;
gradyhillhouse 0:806c1dd6946c 1382 satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0;
gradyhillhouse 0:806c1dd6946c 1383 satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0;
gradyhillhouse 0:806c1dd6946c 1384 satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0;
gradyhillhouse 0:806c1dd6946c 1385 satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0;
gradyhillhouse 0:806c1dd6946c 1386 satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0;
gradyhillhouse 0:806c1dd6946c 1387 satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0;
gradyhillhouse 0:806c1dd6946c 1388 satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0;
gradyhillhouse 0:806c1dd6946c 1389 satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0;
gradyhillhouse 0:806c1dd6946c 1390 satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0;
gradyhillhouse 0:806c1dd6946c 1391 satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0;
gradyhillhouse 0:806c1dd6946c 1392 satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0;
gradyhillhouse 0:806c1dd6946c 1393 satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0;
gradyhillhouse 0:806c1dd6946c 1394 satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0;
gradyhillhouse 0:806c1dd6946c 1395 satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0;
gradyhillhouse 0:806c1dd6946c 1396 satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0;
gradyhillhouse 0:806c1dd6946c 1397 satrec.xli = 0.0; satrec.xni = 0.0;
gradyhillhouse 0:806c1dd6946c 1398
gradyhillhouse 0:806c1dd6946c 1399 // sgp4fix - note the following variables are also passed directly via satrec.
gradyhillhouse 0:806c1dd6946c 1400 // it is possible to streamline the sgp4init call by deleting the "x"
gradyhillhouse 0:806c1dd6946c 1401 // variables, but the user would need to set the satrec.* values first. we
gradyhillhouse 0:806c1dd6946c 1402 // include the additional assignments in case twoline2rv is not used.
gradyhillhouse 0:806c1dd6946c 1403 satrec.bstar = xbstar;
gradyhillhouse 0:806c1dd6946c 1404 satrec.ecco = xecco;
gradyhillhouse 0:806c1dd6946c 1405 satrec.argpo = xargpo;
gradyhillhouse 0:806c1dd6946c 1406 satrec.inclo = xinclo;
gradyhillhouse 0:806c1dd6946c 1407 satrec.mo = xmo;
gradyhillhouse 0:806c1dd6946c 1408 satrec.no = xno;
gradyhillhouse 0:806c1dd6946c 1409 satrec.nodeo = xnodeo;
gradyhillhouse 0:806c1dd6946c 1410
gradyhillhouse 0:806c1dd6946c 1411 // sgp4fix add opsmode
gradyhillhouse 0:806c1dd6946c 1412 satrec.operationmode = opsmode;
gradyhillhouse 0:806c1dd6946c 1413
gradyhillhouse 0:806c1dd6946c 1414 /* ------------------------ earth constants ----------------------- */
gradyhillhouse 0:806c1dd6946c 1415 // sgp4fix identify constants and allow alternate values
gradyhillhouse 0:806c1dd6946c 1416 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
gradyhillhouse 0:806c1dd6946c 1417 ss = 78.0 / radiusearthkm + 1.0;
gradyhillhouse 0:806c1dd6946c 1418 // sgp4fix use multiply for speed instead of pow
gradyhillhouse 0:806c1dd6946c 1419 qzms2ttemp = (120.0 - 78.0) / radiusearthkm;
gradyhillhouse 0:806c1dd6946c 1420 qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp;
gradyhillhouse 0:806c1dd6946c 1421 x2o3 = 2.0 / 3.0;
gradyhillhouse 0:806c1dd6946c 1422
gradyhillhouse 0:806c1dd6946c 1423 satrec.init = 'y';
gradyhillhouse 0:806c1dd6946c 1424 satrec.t = 0.0;
gradyhillhouse 0:806c1dd6946c 1425
gradyhillhouse 0:806c1dd6946c 1426 initl
gradyhillhouse 0:806c1dd6946c 1427 (
gradyhillhouse 0:806c1dd6946c 1428 satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method,
gradyhillhouse 0:806c1dd6946c 1429 ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq,
gradyhillhouse 0:806c1dd6946c 1430 posq, rp, rteosq, sinio, satrec.gsto, satrec.operationmode
gradyhillhouse 0:806c1dd6946c 1431 );
gradyhillhouse 0:806c1dd6946c 1432 satrec.error = 0;
gradyhillhouse 0:806c1dd6946c 1433
gradyhillhouse 0:806c1dd6946c 1434 // sgp4fix remove this check as it is unnecessary
gradyhillhouse 0:806c1dd6946c 1435 // the mrt check in sgp4 handles decaying satellite cases even if the starting
gradyhillhouse 0:806c1dd6946c 1436 // condition is below the surface of te earth
gradyhillhouse 0:806c1dd6946c 1437 // if (rp < 1.0)
gradyhillhouse 0:806c1dd6946c 1438 // {
gradyhillhouse 0:806c1dd6946c 1439 // printf("# *** satn%d epoch elts sub-orbital ***\n", satn);
gradyhillhouse 0:806c1dd6946c 1440 // satrec.error = 5;
gradyhillhouse 0:806c1dd6946c 1441 // }
gradyhillhouse 0:806c1dd6946c 1442
gradyhillhouse 0:806c1dd6946c 1443 if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0))
gradyhillhouse 0:806c1dd6946c 1444 {
gradyhillhouse 0:806c1dd6946c 1445 satrec.isimp = 0;
gradyhillhouse 0:806c1dd6946c 1446 if (rp < (220.0 / radiusearthkm + 1.0))
gradyhillhouse 0:806c1dd6946c 1447 satrec.isimp = 1;
gradyhillhouse 0:806c1dd6946c 1448 sfour = ss;
gradyhillhouse 0:806c1dd6946c 1449 qzms24 = qzms2t;
gradyhillhouse 0:806c1dd6946c 1450 perige = (rp - 1.0) * radiusearthkm;
gradyhillhouse 0:806c1dd6946c 1451
gradyhillhouse 0:806c1dd6946c 1452 /* - for perigees below 156 km, s and qoms2t are altered - */
gradyhillhouse 0:806c1dd6946c 1453 if (perige < 156.0)
gradyhillhouse 0:806c1dd6946c 1454 {
gradyhillhouse 0:806c1dd6946c 1455 sfour = perige - 78.0;
gradyhillhouse 0:806c1dd6946c 1456 if (perige < 98.0)
gradyhillhouse 0:806c1dd6946c 1457 sfour = 20.0;
gradyhillhouse 0:806c1dd6946c 1458 // sgp4fix use multiply for speed instead of pow
gradyhillhouse 0:806c1dd6946c 1459 qzms24temp = (120.0 - sfour) / radiusearthkm;
gradyhillhouse 0:806c1dd6946c 1460 qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp;
gradyhillhouse 0:806c1dd6946c 1461 sfour = sfour / radiusearthkm + 1.0;
gradyhillhouse 0:806c1dd6946c 1462 }
gradyhillhouse 0:806c1dd6946c 1463 pinvsq = 1.0 / posq;
gradyhillhouse 0:806c1dd6946c 1464
gradyhillhouse 0:806c1dd6946c 1465 tsi = 1.0 / (ao - sfour);
gradyhillhouse 0:806c1dd6946c 1466 satrec.eta = ao * satrec.ecco * tsi;
gradyhillhouse 0:806c1dd6946c 1467 etasq = satrec.eta * satrec.eta;
gradyhillhouse 0:806c1dd6946c 1468 eeta = satrec.ecco * satrec.eta;
gradyhillhouse 0:806c1dd6946c 1469 psisq = fabs(1.0 - etasq);
gradyhillhouse 0:806c1dd6946c 1470 coef = qzms24 * pow(tsi, 4.0);
gradyhillhouse 0:806c1dd6946c 1471 coef1 = coef / pow(psisq, 3.5);
gradyhillhouse 0:806c1dd6946c 1472 cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta *
gradyhillhouse 0:806c1dd6946c 1473 (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 *
gradyhillhouse 0:806c1dd6946c 1474 (8.0 + 3.0 * etasq * (8.0 + etasq)));
gradyhillhouse 0:806c1dd6946c 1475 satrec.cc1 = satrec.bstar * cc2;
gradyhillhouse 0:806c1dd6946c 1476 cc3 = 0.0;
gradyhillhouse 0:806c1dd6946c 1477 if (satrec.ecco > 1.0e-4)
gradyhillhouse 0:806c1dd6946c 1478 cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco;
gradyhillhouse 0:806c1dd6946c 1479 satrec.x1mth2 = 1.0 - cosio2;
gradyhillhouse 0:806c1dd6946c 1480 satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq *
gradyhillhouse 0:806c1dd6946c 1481 (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco *
gradyhillhouse 0:806c1dd6946c 1482 (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) *
gradyhillhouse 0:806c1dd6946c 1483 (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq *
gradyhillhouse 0:806c1dd6946c 1484 (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 *
gradyhillhouse 0:806c1dd6946c 1485 (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo)));
gradyhillhouse 0:806c1dd6946c 1486 satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 *
gradyhillhouse 0:806c1dd6946c 1487 (etasq + eeta) + eeta * etasq);
gradyhillhouse 0:806c1dd6946c 1488 cosio4 = cosio2 * cosio2;
gradyhillhouse 0:806c1dd6946c 1489 temp1 = 1.5 * j2 * pinvsq * satrec.no;
gradyhillhouse 0:806c1dd6946c 1490 temp2 = 0.5 * temp1 * j2 * pinvsq;
gradyhillhouse 0:806c1dd6946c 1491 temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no;
gradyhillhouse 0:806c1dd6946c 1492 satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 *
gradyhillhouse 0:806c1dd6946c 1493 temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4);
gradyhillhouse 0:806c1dd6946c 1494 satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 *
gradyhillhouse 0:806c1dd6946c 1495 (7.0 - 114.0 * cosio2 + 395.0 * cosio4) +
gradyhillhouse 0:806c1dd6946c 1496 temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4);
gradyhillhouse 0:806c1dd6946c 1497 xhdot1 = -temp1 * cosio;
gradyhillhouse 0:806c1dd6946c 1498 satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) +
gradyhillhouse 0:806c1dd6946c 1499 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio;
gradyhillhouse 0:806c1dd6946c 1500 xpidot = satrec.argpdot+ satrec.nodedot;
gradyhillhouse 0:806c1dd6946c 1501 satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo);
gradyhillhouse 0:806c1dd6946c 1502 satrec.xmcof = 0.0;
gradyhillhouse 0:806c1dd6946c 1503 if (satrec.ecco > 1.0e-4)
gradyhillhouse 0:806c1dd6946c 1504 satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta;
gradyhillhouse 0:806c1dd6946c 1505 satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1;
gradyhillhouse 0:806c1dd6946c 1506 satrec.t2cof = 1.5 * satrec.cc1;
gradyhillhouse 0:806c1dd6946c 1507 // sgp4fix for divide by zero with xinco = 180 deg
gradyhillhouse 0:806c1dd6946c 1508 if (fabs(cosio+1.0) > 1.5e-12)
gradyhillhouse 0:806c1dd6946c 1509 satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio);
gradyhillhouse 0:806c1dd6946c 1510 else
gradyhillhouse 0:806c1dd6946c 1511 satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4;
gradyhillhouse 0:806c1dd6946c 1512 satrec.aycof = -0.5 * j3oj2 * sinio;
gradyhillhouse 0:806c1dd6946c 1513 // sgp4fix use multiply for speed instead of pow
gradyhillhouse 0:806c1dd6946c 1514 delmotemp = 1.0 + satrec.eta * cos(satrec.mo);
gradyhillhouse 0:806c1dd6946c 1515 satrec.delmo = delmotemp * delmotemp * delmotemp;
gradyhillhouse 0:806c1dd6946c 1516 satrec.sinmao = sin(satrec.mo);
gradyhillhouse 0:806c1dd6946c 1517 satrec.x7thm1 = 7.0 * cosio2 - 1.0;
gradyhillhouse 0:806c1dd6946c 1518
gradyhillhouse 0:806c1dd6946c 1519 /* --------------- deep space initialization ------------- */
gradyhillhouse 0:806c1dd6946c 1520 if ((2*pi / satrec.no) >= 225.0)
gradyhillhouse 0:806c1dd6946c 1521 {
gradyhillhouse 0:806c1dd6946c 1522 satrec.method = 'd';
gradyhillhouse 0:806c1dd6946c 1523 satrec.isimp = 1;
gradyhillhouse 0:806c1dd6946c 1524 tc = 0.0;
gradyhillhouse 0:806c1dd6946c 1525 inclm = satrec.inclo;
gradyhillhouse 0:806c1dd6946c 1526
gradyhillhouse 0:806c1dd6946c 1527 dscom
gradyhillhouse 0:806c1dd6946c 1528 (
gradyhillhouse 0:806c1dd6946c 1529 epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo,
gradyhillhouse 0:806c1dd6946c 1530 satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm,
gradyhillhouse 0:806c1dd6946c 1531 day, satrec.e3, satrec.ee2, em, emsq, gam,
gradyhillhouse 0:806c1dd6946c 1532 satrec.peo, satrec.pgho, satrec.pho, satrec.pinco,
gradyhillhouse 0:806c1dd6946c 1533 satrec.plo, rtemsq, satrec.se2, satrec.se3,
gradyhillhouse 0:806c1dd6946c 1534 satrec.sgh2, satrec.sgh3, satrec.sgh4,
gradyhillhouse 0:806c1dd6946c 1535 satrec.sh2, satrec.sh3, satrec.si2, satrec.si3,
gradyhillhouse 0:806c1dd6946c 1536 satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5,
gradyhillhouse 0:806c1dd6946c 1537 s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3,
gradyhillhouse 0:806c1dd6946c 1538 sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33,
gradyhillhouse 0:806c1dd6946c 1539 satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2,
gradyhillhouse 0:806c1dd6946c 1540 satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2,
gradyhillhouse 0:806c1dd6946c 1541 satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11,
gradyhillhouse 0:806c1dd6946c 1542 z12, z13, z21, z22, z23, z31, z32, z33,
gradyhillhouse 0:806c1dd6946c 1543 satrec.zmol, satrec.zmos
gradyhillhouse 0:806c1dd6946c 1544 );
gradyhillhouse 0:806c1dd6946c 1545 dpper
gradyhillhouse 0:806c1dd6946c 1546 (
gradyhillhouse 0:806c1dd6946c 1547 satrec.e3, satrec.ee2, satrec.peo, satrec.pgho,
gradyhillhouse 0:806c1dd6946c 1548 satrec.pho, satrec.pinco, satrec.plo, satrec.se2,
gradyhillhouse 0:806c1dd6946c 1549 satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4,
gradyhillhouse 0:806c1dd6946c 1550 satrec.sh2, satrec.sh3, satrec.si2, satrec.si3,
gradyhillhouse 0:806c1dd6946c 1551 satrec.sl2, satrec.sl3, satrec.sl4, satrec.t,
gradyhillhouse 0:806c1dd6946c 1552 satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2,
gradyhillhouse 0:806c1dd6946c 1553 satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2,
gradyhillhouse 0:806c1dd6946c 1554 satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init,
gradyhillhouse 0:806c1dd6946c 1555 satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo,
gradyhillhouse 0:806c1dd6946c 1556 satrec.operationmode
gradyhillhouse 0:806c1dd6946c 1557 );
gradyhillhouse 0:806c1dd6946c 1558
gradyhillhouse 0:806c1dd6946c 1559 argpm = 0.0;
gradyhillhouse 0:806c1dd6946c 1560 nodem = 0.0;
gradyhillhouse 0:806c1dd6946c 1561 mm = 0.0;
gradyhillhouse 0:806c1dd6946c 1562
gradyhillhouse 0:806c1dd6946c 1563 dsinit
gradyhillhouse 0:806c1dd6946c 1564 (
gradyhillhouse 0:806c1dd6946c 1565 whichconst,
gradyhillhouse 0:806c1dd6946c 1566 cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4,
gradyhillhouse 0:806c1dd6946c 1567 ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc,
gradyhillhouse 0:806c1dd6946c 1568 satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo,
gradyhillhouse 0:806c1dd6946c 1569 satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33,
gradyhillhouse 0:806c1dd6946c 1570 satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem,
gradyhillhouse 0:806c1dd6946c 1571 satrec.irez, satrec.atime,
gradyhillhouse 0:806c1dd6946c 1572 satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 ,
gradyhillhouse 0:806c1dd6946c 1573 satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232,
gradyhillhouse 0:806c1dd6946c 1574 satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt,
gradyhillhouse 0:806c1dd6946c 1575 satrec.dmdt, dndt, satrec.dnodt, satrec.domdt ,
gradyhillhouse 0:806c1dd6946c 1576 satrec.del1, satrec.del2, satrec.del3, satrec.xfact,
gradyhillhouse 0:806c1dd6946c 1577 satrec.xlamo, satrec.xli, satrec.xni
gradyhillhouse 0:806c1dd6946c 1578 );
gradyhillhouse 0:806c1dd6946c 1579 }
gradyhillhouse 0:806c1dd6946c 1580
gradyhillhouse 0:806c1dd6946c 1581 /* ----------- set variables if not deep space ----------- */
gradyhillhouse 0:806c1dd6946c 1582 if (satrec.isimp != 1)
gradyhillhouse 0:806c1dd6946c 1583 {
gradyhillhouse 0:806c1dd6946c 1584 cc1sq = satrec.cc1 * satrec.cc1;
gradyhillhouse 0:806c1dd6946c 1585 satrec.d2 = 4.0 * ao * tsi * cc1sq;
gradyhillhouse 0:806c1dd6946c 1586 temp = satrec.d2 * tsi * satrec.cc1 / 3.0;
gradyhillhouse 0:806c1dd6946c 1587 satrec.d3 = (17.0 * ao + sfour) * temp;
gradyhillhouse 0:806c1dd6946c 1588 satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) *
gradyhillhouse 0:806c1dd6946c 1589 satrec.cc1;
gradyhillhouse 0:806c1dd6946c 1590 satrec.t3cof = satrec.d2 + 2.0 * cc1sq;
gradyhillhouse 0:806c1dd6946c 1591 satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 *
gradyhillhouse 0:806c1dd6946c 1592 (12.0 * satrec.d2 + 10.0 * cc1sq));
gradyhillhouse 0:806c1dd6946c 1593 satrec.t5cof = 0.2 * (3.0 * satrec.d4 +
gradyhillhouse 0:806c1dd6946c 1594 12.0 * satrec.cc1 * satrec.d3 +
gradyhillhouse 0:806c1dd6946c 1595 6.0 * satrec.d2 * satrec.d2 +
gradyhillhouse 0:806c1dd6946c 1596 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq));
gradyhillhouse 0:806c1dd6946c 1597 }
gradyhillhouse 0:806c1dd6946c 1598 } // if omeosq = 0 ...
gradyhillhouse 0:806c1dd6946c 1599
gradyhillhouse 0:806c1dd6946c 1600 /* finally propogate to zero epoch to initialize all others. */
gradyhillhouse 0:806c1dd6946c 1601 // sgp4fix take out check to let satellites process until they are actually below earth surface
gradyhillhouse 0:806c1dd6946c 1602 // if(satrec.error == 0)
gradyhillhouse 0:806c1dd6946c 1603 sgp4(whichconst, satrec, 0.0, r, v);
gradyhillhouse 0:806c1dd6946c 1604
gradyhillhouse 0:806c1dd6946c 1605 satrec.init = 'n';
gradyhillhouse 0:806c1dd6946c 1606
gradyhillhouse 0:806c1dd6946c 1607 //#include "debug6.cpp"
gradyhillhouse 0:806c1dd6946c 1608 //sgp4fix return boolean. satrec.error contains any error codes
gradyhillhouse 0:806c1dd6946c 1609 return true;
gradyhillhouse 0:806c1dd6946c 1610 } // end sgp4init
gradyhillhouse 0:806c1dd6946c 1611
gradyhillhouse 0:806c1dd6946c 1612 /*-----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 1613 *
gradyhillhouse 0:806c1dd6946c 1614 * procedure sgp4
gradyhillhouse 0:806c1dd6946c 1615 *
gradyhillhouse 0:806c1dd6946c 1616 * this procedure is the sgp4 prediction model from space command. this is an
gradyhillhouse 0:806c1dd6946c 1617 * updated and combined version of sgp4 and sdp4, which were originally
gradyhillhouse 0:806c1dd6946c 1618 * published separately in spacetrack report #3. this version follows the
gradyhillhouse 0:806c1dd6946c 1619 * methodology from the aiaa paper (2006) describing the history and
gradyhillhouse 0:806c1dd6946c 1620 * development of the code.
gradyhillhouse 0:806c1dd6946c 1621 *
gradyhillhouse 0:806c1dd6946c 1622 * author : david vallado 719-573-2600 28 jun 2005
gradyhillhouse 0:806c1dd6946c 1623 *
gradyhillhouse 0:806c1dd6946c 1624 * inputs :
gradyhillhouse 0:806c1dd6946c 1625 * satrec - initialised structure from sgp4init() call.
gradyhillhouse 0:806c1dd6946c 1626 * tsince - time eince epoch (minutes)
gradyhillhouse 0:806c1dd6946c 1627 *
gradyhillhouse 0:806c1dd6946c 1628 * outputs :
gradyhillhouse 0:806c1dd6946c 1629 * r - position vector km
gradyhillhouse 0:806c1dd6946c 1630 * v - velocity km/sec
gradyhillhouse 0:806c1dd6946c 1631 * return code - non-zero on error.
gradyhillhouse 0:806c1dd6946c 1632 * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
gradyhillhouse 0:806c1dd6946c 1633 * 2 - mean motion less than 0.0
gradyhillhouse 0:806c1dd6946c 1634 * 3 - pert elements, ecc < 0.0 or ecc > 1.0
gradyhillhouse 0:806c1dd6946c 1635 * 4 - semi-latus rectum < 0.0
gradyhillhouse 0:806c1dd6946c 1636 * 5 - epoch elements are sub-orbital
gradyhillhouse 0:806c1dd6946c 1637 * 6 - satellite has decayed
gradyhillhouse 0:806c1dd6946c 1638 *
gradyhillhouse 0:806c1dd6946c 1639 * locals :
gradyhillhouse 0:806c1dd6946c 1640 * am -
gradyhillhouse 0:806c1dd6946c 1641 * axnl, aynl -
gradyhillhouse 0:806c1dd6946c 1642 * betal -
gradyhillhouse 0:806c1dd6946c 1643 * cosim , sinim , cosomm , sinomm , cnod , snod , cos2u ,
gradyhillhouse 0:806c1dd6946c 1644 * sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip ,
gradyhillhouse 0:806c1dd6946c 1645 * cosisq , cossu , sinsu , cosu , sinu
gradyhillhouse 0:806c1dd6946c 1646 * delm -
gradyhillhouse 0:806c1dd6946c 1647 * delomg -
gradyhillhouse 0:806c1dd6946c 1648 * dndt -
gradyhillhouse 0:806c1dd6946c 1649 * eccm -
gradyhillhouse 0:806c1dd6946c 1650 * emsq -
gradyhillhouse 0:806c1dd6946c 1651 * ecose -
gradyhillhouse 0:806c1dd6946c 1652 * el2 -
gradyhillhouse 0:806c1dd6946c 1653 * eo1 -
gradyhillhouse 0:806c1dd6946c 1654 * eccp -
gradyhillhouse 0:806c1dd6946c 1655 * esine -
gradyhillhouse 0:806c1dd6946c 1656 * argpm -
gradyhillhouse 0:806c1dd6946c 1657 * argpp -
gradyhillhouse 0:806c1dd6946c 1658 * omgadf -
gradyhillhouse 0:806c1dd6946c 1659 * pl -
gradyhillhouse 0:806c1dd6946c 1660 * r -
gradyhillhouse 0:806c1dd6946c 1661 * rtemsq -
gradyhillhouse 0:806c1dd6946c 1662 * rdotl -
gradyhillhouse 0:806c1dd6946c 1663 * rl -
gradyhillhouse 0:806c1dd6946c 1664 * rvdot -
gradyhillhouse 0:806c1dd6946c 1665 * rvdotl -
gradyhillhouse 0:806c1dd6946c 1666 * su -
gradyhillhouse 0:806c1dd6946c 1667 * t2 , t3 , t4 , tc
gradyhillhouse 0:806c1dd6946c 1668 * tem5, temp , temp1 , temp2 , tempa , tempe , templ
gradyhillhouse 0:806c1dd6946c 1669 * u , ux , uy , uz , vx , vy , vz
gradyhillhouse 0:806c1dd6946c 1670 * inclm - inclination
gradyhillhouse 0:806c1dd6946c 1671 * mm - mean anomaly
gradyhillhouse 0:806c1dd6946c 1672 * nm - mean motion
gradyhillhouse 0:806c1dd6946c 1673 * nodem - right asc of ascending node
gradyhillhouse 0:806c1dd6946c 1674 * xinc -
gradyhillhouse 0:806c1dd6946c 1675 * xincp -
gradyhillhouse 0:806c1dd6946c 1676 * xl -
gradyhillhouse 0:806c1dd6946c 1677 * xlm -
gradyhillhouse 0:806c1dd6946c 1678 * mp -
gradyhillhouse 0:806c1dd6946c 1679 * xmdf -
gradyhillhouse 0:806c1dd6946c 1680 * xmx -
gradyhillhouse 0:806c1dd6946c 1681 * xmy -
gradyhillhouse 0:806c1dd6946c 1682 * nodedf -
gradyhillhouse 0:806c1dd6946c 1683 * xnode -
gradyhillhouse 0:806c1dd6946c 1684 * nodep -
gradyhillhouse 0:806c1dd6946c 1685 * np -
gradyhillhouse 0:806c1dd6946c 1686 *
gradyhillhouse 0:806c1dd6946c 1687 * coupling :
gradyhillhouse 0:806c1dd6946c 1688 * getgravconst-
gradyhillhouse 0:806c1dd6946c 1689 * dpper
gradyhillhouse 0:806c1dd6946c 1690 * dpspace
gradyhillhouse 0:806c1dd6946c 1691 *
gradyhillhouse 0:806c1dd6946c 1692 * references :
gradyhillhouse 0:806c1dd6946c 1693 * hoots, roehrich, norad spacetrack report #3 1980
gradyhillhouse 0:806c1dd6946c 1694 * hoots, norad spacetrack report #6 1986
gradyhillhouse 0:806c1dd6946c 1695 * hoots, schumacher and glover 2004
gradyhillhouse 0:806c1dd6946c 1696 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 1697 ----------------------------------------------------------------------------*/
gradyhillhouse 0:806c1dd6946c 1698
gradyhillhouse 0:806c1dd6946c 1699 bool sgp4
gradyhillhouse 0:806c1dd6946c 1700 (
gradyhillhouse 0:806c1dd6946c 1701 gravconsttype whichconst, elsetrec& satrec, double tsince,
gradyhillhouse 0:806c1dd6946c 1702 double r[3], double v[3]
gradyhillhouse 0:806c1dd6946c 1703 )
gradyhillhouse 0:806c1dd6946c 1704 {
gradyhillhouse 0:806c1dd6946c 1705 double am , axnl , aynl , betal , cosim , cnod ,
gradyhillhouse 0:806c1dd6946c 1706 cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu,
gradyhillhouse 0:806c1dd6946c 1707 delm , delomg, em , emsq , ecose , el2 , eo1 ,
gradyhillhouse 0:806c1dd6946c 1708 ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0,
gradyhillhouse 0:806c1dd6946c 1709 mvt , rdotl , rl , rvdot , rvdotl, sinim ,
gradyhillhouse 0:806c1dd6946c 1710 sin2u, sineo1, sini , sinip , sinsu , sinu ,
gradyhillhouse 0:806c1dd6946c 1711 snod , su , t2 , t3 , t4 , tem5 , temp,
gradyhillhouse 0:806c1dd6946c 1712 temp1, temp2 , tempa, tempe , templ , u , ux ,
gradyhillhouse 0:806c1dd6946c 1713 uy , uz , vx , vy , vz , inclm , mm ,
gradyhillhouse 0:806c1dd6946c 1714 nm , nodem, xinc , xincp , xl , xlm , mp ,
gradyhillhouse 0:806c1dd6946c 1715 xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt,
gradyhillhouse 0:806c1dd6946c 1716 twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm,
gradyhillhouse 0:806c1dd6946c 1717 mu, vkmpersec, delmtemp;
gradyhillhouse 0:806c1dd6946c 1718 int ktr;
gradyhillhouse 0:806c1dd6946c 1719
gradyhillhouse 0:806c1dd6946c 1720 /* ------------------ set mathematical constants --------------- */
gradyhillhouse 0:806c1dd6946c 1721 // sgp4fix divisor for divide by zero check on inclination
gradyhillhouse 0:806c1dd6946c 1722 // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
gradyhillhouse 0:806c1dd6946c 1723 // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
gradyhillhouse 0:806c1dd6946c 1724 const double temp4 = 1.5e-12;
gradyhillhouse 0:806c1dd6946c 1725 twopi = 2.0 * pi;
gradyhillhouse 0:806c1dd6946c 1726 x2o3 = 2.0 / 3.0;
gradyhillhouse 0:806c1dd6946c 1727 // sgp4fix identify constants and allow alternate values
gradyhillhouse 0:806c1dd6946c 1728 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
gradyhillhouse 0:806c1dd6946c 1729 vkmpersec = radiusearthkm * xke/60.0;
gradyhillhouse 0:806c1dd6946c 1730
gradyhillhouse 0:806c1dd6946c 1731 /* --------------------- clear sgp4 error flag ----------------- */
gradyhillhouse 0:806c1dd6946c 1732 satrec.t = tsince;
gradyhillhouse 0:806c1dd6946c 1733 satrec.error = 0;
gradyhillhouse 0:806c1dd6946c 1734
gradyhillhouse 0:806c1dd6946c 1735 /* ------- update for secular gravity and atmospheric drag ----- */
gradyhillhouse 0:806c1dd6946c 1736 xmdf = satrec.mo + satrec.mdot * satrec.t;
gradyhillhouse 0:806c1dd6946c 1737 argpdf = satrec.argpo + satrec.argpdot * satrec.t;
gradyhillhouse 0:806c1dd6946c 1738 nodedf = satrec.nodeo + satrec.nodedot * satrec.t;
gradyhillhouse 0:806c1dd6946c 1739 argpm = argpdf;
gradyhillhouse 0:806c1dd6946c 1740 mm = xmdf;
gradyhillhouse 0:806c1dd6946c 1741 t2 = satrec.t * satrec.t;
gradyhillhouse 0:806c1dd6946c 1742 nodem = nodedf + satrec.nodecf * t2;
gradyhillhouse 0:806c1dd6946c 1743 tempa = 1.0 - satrec.cc1 * satrec.t;
gradyhillhouse 0:806c1dd6946c 1744 tempe = satrec.bstar * satrec.cc4 * satrec.t;
gradyhillhouse 0:806c1dd6946c 1745 templ = satrec.t2cof * t2;
gradyhillhouse 0:806c1dd6946c 1746
gradyhillhouse 0:806c1dd6946c 1747 if (satrec.isimp != 1)
gradyhillhouse 0:806c1dd6946c 1748 {
gradyhillhouse 0:806c1dd6946c 1749 delomg = satrec.omgcof * satrec.t;
gradyhillhouse 0:806c1dd6946c 1750 // sgp4fix use mutliply for speed instead of pow
gradyhillhouse 0:806c1dd6946c 1751 delmtemp = 1.0 + satrec.eta * cos(xmdf);
gradyhillhouse 0:806c1dd6946c 1752 delm = satrec.xmcof *
gradyhillhouse 0:806c1dd6946c 1753 (delmtemp * delmtemp * delmtemp -
gradyhillhouse 0:806c1dd6946c 1754 satrec.delmo);
gradyhillhouse 0:806c1dd6946c 1755 temp = delomg + delm;
gradyhillhouse 0:806c1dd6946c 1756 mm = xmdf + temp;
gradyhillhouse 0:806c1dd6946c 1757 argpm = argpdf - temp;
gradyhillhouse 0:806c1dd6946c 1758 t3 = t2 * satrec.t;
gradyhillhouse 0:806c1dd6946c 1759 t4 = t3 * satrec.t;
gradyhillhouse 0:806c1dd6946c 1760 tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 -
gradyhillhouse 0:806c1dd6946c 1761 satrec.d4 * t4;
gradyhillhouse 0:806c1dd6946c 1762 tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) -
gradyhillhouse 0:806c1dd6946c 1763 satrec.sinmao);
gradyhillhouse 0:806c1dd6946c 1764 templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof +
gradyhillhouse 0:806c1dd6946c 1765 satrec.t * satrec.t5cof);
gradyhillhouse 0:806c1dd6946c 1766 }
gradyhillhouse 0:806c1dd6946c 1767
gradyhillhouse 0:806c1dd6946c 1768 nm = satrec.no;
gradyhillhouse 0:806c1dd6946c 1769 em = satrec.ecco;
gradyhillhouse 0:806c1dd6946c 1770 inclm = satrec.inclo;
gradyhillhouse 0:806c1dd6946c 1771 if (satrec.method == 'd')
gradyhillhouse 0:806c1dd6946c 1772 {
gradyhillhouse 0:806c1dd6946c 1773 tc = satrec.t;
gradyhillhouse 0:806c1dd6946c 1774 dspace
gradyhillhouse 0:806c1dd6946c 1775 (
gradyhillhouse 0:806c1dd6946c 1776 satrec.irez,
gradyhillhouse 0:806c1dd6946c 1777 satrec.d2201, satrec.d2211, satrec.d3210,
gradyhillhouse 0:806c1dd6946c 1778 satrec.d3222, satrec.d4410, satrec.d4422,
gradyhillhouse 0:806c1dd6946c 1779 satrec.d5220, satrec.d5232, satrec.d5421,
gradyhillhouse 0:806c1dd6946c 1780 satrec.d5433, satrec.dedt, satrec.del1,
gradyhillhouse 0:806c1dd6946c 1781 satrec.del2, satrec.del3, satrec.didt,
gradyhillhouse 0:806c1dd6946c 1782 satrec.dmdt, satrec.dnodt, satrec.domdt,
gradyhillhouse 0:806c1dd6946c 1783 satrec.argpo, satrec.argpdot, satrec.t, tc,
gradyhillhouse 0:806c1dd6946c 1784 satrec.gsto, satrec.xfact, satrec.xlamo,
gradyhillhouse 0:806c1dd6946c 1785 satrec.no, satrec.atime,
gradyhillhouse 0:806c1dd6946c 1786 em, argpm, inclm, satrec.xli, mm, satrec.xni,
gradyhillhouse 0:806c1dd6946c 1787 nodem, dndt, nm
gradyhillhouse 0:806c1dd6946c 1788 );
gradyhillhouse 0:806c1dd6946c 1789 } // if method = d
gradyhillhouse 0:806c1dd6946c 1790
gradyhillhouse 0:806c1dd6946c 1791 if (nm <= 0.0)
gradyhillhouse 0:806c1dd6946c 1792 {
gradyhillhouse 0:806c1dd6946c 1793 // printf("# error nm %f\n", nm);
gradyhillhouse 0:806c1dd6946c 1794 satrec.error = 2;
gradyhillhouse 0:806c1dd6946c 1795 // sgp4fix add return
gradyhillhouse 0:806c1dd6946c 1796 return false;
gradyhillhouse 0:806c1dd6946c 1797 }
gradyhillhouse 0:806c1dd6946c 1798 am = pow((xke / nm),x2o3) * tempa * tempa;
gradyhillhouse 0:806c1dd6946c 1799 nm = xke / pow(am, 1.5);
gradyhillhouse 0:806c1dd6946c 1800 em = em - tempe;
gradyhillhouse 0:806c1dd6946c 1801
gradyhillhouse 0:806c1dd6946c 1802 // fix tolerance for error recognition
gradyhillhouse 0:806c1dd6946c 1803 // sgp4fix am is fixed from the previous nm check
gradyhillhouse 0:806c1dd6946c 1804 if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/ )
gradyhillhouse 0:806c1dd6946c 1805 {
gradyhillhouse 0:806c1dd6946c 1806 // printf("# error em %f\n", em);
gradyhillhouse 0:806c1dd6946c 1807 satrec.error = 1;
gradyhillhouse 0:806c1dd6946c 1808 // sgp4fix to return if there is an error in eccentricity
gradyhillhouse 0:806c1dd6946c 1809 return false;
gradyhillhouse 0:806c1dd6946c 1810 }
gradyhillhouse 0:806c1dd6946c 1811 // sgp4fix fix tolerance to avoid a divide by zero
gradyhillhouse 0:806c1dd6946c 1812 if (em < 1.0e-6)
gradyhillhouse 0:806c1dd6946c 1813 em = 1.0e-6;
gradyhillhouse 0:806c1dd6946c 1814 mm = mm + satrec.no * templ;
gradyhillhouse 0:806c1dd6946c 1815 xlm = mm + argpm + nodem;
gradyhillhouse 0:806c1dd6946c 1816 emsq = em * em;
gradyhillhouse 0:806c1dd6946c 1817 temp = 1.0 - emsq;
gradyhillhouse 0:806c1dd6946c 1818
gradyhillhouse 0:806c1dd6946c 1819 nodem = fmod(nodem, twopi);
gradyhillhouse 0:806c1dd6946c 1820 argpm = fmod(argpm, twopi);
gradyhillhouse 0:806c1dd6946c 1821 xlm = fmod(xlm, twopi);
gradyhillhouse 0:806c1dd6946c 1822 mm = fmod(xlm - argpm - nodem, twopi);
gradyhillhouse 0:806c1dd6946c 1823
gradyhillhouse 0:806c1dd6946c 1824 /* ----------------- compute extra mean quantities ------------- */
gradyhillhouse 0:806c1dd6946c 1825 sinim = sin(inclm);
gradyhillhouse 0:806c1dd6946c 1826 cosim = cos(inclm);
gradyhillhouse 0:806c1dd6946c 1827
gradyhillhouse 0:806c1dd6946c 1828 /* -------------------- add lunar-solar periodics -------------- */
gradyhillhouse 0:806c1dd6946c 1829 ep = em;
gradyhillhouse 0:806c1dd6946c 1830 xincp = inclm;
gradyhillhouse 0:806c1dd6946c 1831 argpp = argpm;
gradyhillhouse 0:806c1dd6946c 1832 nodep = nodem;
gradyhillhouse 0:806c1dd6946c 1833 mp = mm;
gradyhillhouse 0:806c1dd6946c 1834 sinip = sinim;
gradyhillhouse 0:806c1dd6946c 1835 cosip = cosim;
gradyhillhouse 0:806c1dd6946c 1836 if (satrec.method == 'd')
gradyhillhouse 0:806c1dd6946c 1837 {
gradyhillhouse 0:806c1dd6946c 1838 dpper
gradyhillhouse 0:806c1dd6946c 1839 (
gradyhillhouse 0:806c1dd6946c 1840 satrec.e3, satrec.ee2, satrec.peo,
gradyhillhouse 0:806c1dd6946c 1841 satrec.pgho, satrec.pho, satrec.pinco,
gradyhillhouse 0:806c1dd6946c 1842 satrec.plo, satrec.se2, satrec.se3,
gradyhillhouse 0:806c1dd6946c 1843 satrec.sgh2, satrec.sgh3, satrec.sgh4,
gradyhillhouse 0:806c1dd6946c 1844 satrec.sh2, satrec.sh3, satrec.si2,
gradyhillhouse 0:806c1dd6946c 1845 satrec.si3, satrec.sl2, satrec.sl3,
gradyhillhouse 0:806c1dd6946c 1846 satrec.sl4, satrec.t, satrec.xgh2,
gradyhillhouse 0:806c1dd6946c 1847 satrec.xgh3, satrec.xgh4, satrec.xh2,
gradyhillhouse 0:806c1dd6946c 1848 satrec.xh3, satrec.xi2, satrec.xi3,
gradyhillhouse 0:806c1dd6946c 1849 satrec.xl2, satrec.xl3, satrec.xl4,
gradyhillhouse 0:806c1dd6946c 1850 satrec.zmol, satrec.zmos, satrec.inclo,
gradyhillhouse 0:806c1dd6946c 1851 'n', ep, xincp, nodep, argpp, mp, satrec.operationmode
gradyhillhouse 0:806c1dd6946c 1852 );
gradyhillhouse 0:806c1dd6946c 1853 if (xincp < 0.0)
gradyhillhouse 0:806c1dd6946c 1854 {
gradyhillhouse 0:806c1dd6946c 1855 xincp = -xincp;
gradyhillhouse 0:806c1dd6946c 1856 nodep = nodep + pi;
gradyhillhouse 0:806c1dd6946c 1857 argpp = argpp - pi;
gradyhillhouse 0:806c1dd6946c 1858 }
gradyhillhouse 0:806c1dd6946c 1859 if ((ep < 0.0 ) || ( ep > 1.0))
gradyhillhouse 0:806c1dd6946c 1860 {
gradyhillhouse 0:806c1dd6946c 1861 // printf("# error ep %f\n", ep);
gradyhillhouse 0:806c1dd6946c 1862 satrec.error = 3;
gradyhillhouse 0:806c1dd6946c 1863 // sgp4fix add return
gradyhillhouse 0:806c1dd6946c 1864 return false;
gradyhillhouse 0:806c1dd6946c 1865 }
gradyhillhouse 0:806c1dd6946c 1866 } // if method = d
gradyhillhouse 0:806c1dd6946c 1867
gradyhillhouse 0:806c1dd6946c 1868 /* -------------------- long period periodics ------------------ */
gradyhillhouse 0:806c1dd6946c 1869 if (satrec.method == 'd')
gradyhillhouse 0:806c1dd6946c 1870 {
gradyhillhouse 0:806c1dd6946c 1871 sinip = sin(xincp);
gradyhillhouse 0:806c1dd6946c 1872 cosip = cos(xincp);
gradyhillhouse 0:806c1dd6946c 1873 satrec.aycof = -0.5*j3oj2*sinip;
gradyhillhouse 0:806c1dd6946c 1874 // sgp4fix for divide by zero for xincp = 180 deg
gradyhillhouse 0:806c1dd6946c 1875 if (fabs(cosip+1.0) > 1.5e-12)
gradyhillhouse 0:806c1dd6946c 1876 satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip);
gradyhillhouse 0:806c1dd6946c 1877 else
gradyhillhouse 0:806c1dd6946c 1878 satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4;
gradyhillhouse 0:806c1dd6946c 1879 }
gradyhillhouse 0:806c1dd6946c 1880 axnl = ep * cos(argpp);
gradyhillhouse 0:806c1dd6946c 1881 temp = 1.0 / (am * (1.0 - ep * ep));
gradyhillhouse 0:806c1dd6946c 1882 aynl = ep* sin(argpp) + temp * satrec.aycof;
gradyhillhouse 0:806c1dd6946c 1883 xl = mp + argpp + nodep + temp * satrec.xlcof * axnl;
gradyhillhouse 0:806c1dd6946c 1884
gradyhillhouse 0:806c1dd6946c 1885 /* --------------------- solve kepler's equation --------------- */
gradyhillhouse 0:806c1dd6946c 1886 u = fmod(xl - nodep, twopi);
gradyhillhouse 0:806c1dd6946c 1887 eo1 = u;
gradyhillhouse 0:806c1dd6946c 1888 tem5 = 9999.9;
gradyhillhouse 0:806c1dd6946c 1889 ktr = 1;
gradyhillhouse 0:806c1dd6946c 1890 // sgp4fix for kepler iteration
gradyhillhouse 0:806c1dd6946c 1891 // the following iteration needs better limits on corrections
gradyhillhouse 0:806c1dd6946c 1892 while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) )
gradyhillhouse 0:806c1dd6946c 1893 {
gradyhillhouse 0:806c1dd6946c 1894 sineo1 = sin(eo1);
gradyhillhouse 0:806c1dd6946c 1895 coseo1 = cos(eo1);
gradyhillhouse 0:806c1dd6946c 1896 tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl;
gradyhillhouse 0:806c1dd6946c 1897 tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
gradyhillhouse 0:806c1dd6946c 1898 if(fabs(tem5) >= 0.95)
gradyhillhouse 0:806c1dd6946c 1899 tem5 = tem5 > 0.0 ? 0.95 : -0.95;
gradyhillhouse 0:806c1dd6946c 1900 eo1 = eo1 + tem5;
gradyhillhouse 0:806c1dd6946c 1901 ktr = ktr + 1;
gradyhillhouse 0:806c1dd6946c 1902 }
gradyhillhouse 0:806c1dd6946c 1903
gradyhillhouse 0:806c1dd6946c 1904 /* ------------- short period preliminary quantities ----------- */
gradyhillhouse 0:806c1dd6946c 1905 ecose = axnl*coseo1 + aynl*sineo1;
gradyhillhouse 0:806c1dd6946c 1906 esine = axnl*sineo1 - aynl*coseo1;
gradyhillhouse 0:806c1dd6946c 1907 el2 = axnl*axnl + aynl*aynl;
gradyhillhouse 0:806c1dd6946c 1908 pl = am*(1.0-el2);
gradyhillhouse 0:806c1dd6946c 1909 if (pl < 0.0)
gradyhillhouse 0:806c1dd6946c 1910 {
gradyhillhouse 0:806c1dd6946c 1911 // printf("# error pl %f\n", pl);
gradyhillhouse 0:806c1dd6946c 1912 satrec.error = 4;
gradyhillhouse 0:806c1dd6946c 1913 // sgp4fix add return
gradyhillhouse 0:806c1dd6946c 1914 return false;
gradyhillhouse 0:806c1dd6946c 1915 }
gradyhillhouse 0:806c1dd6946c 1916 else
gradyhillhouse 0:806c1dd6946c 1917 {
gradyhillhouse 0:806c1dd6946c 1918 rl = am * (1.0 - ecose);
gradyhillhouse 0:806c1dd6946c 1919 rdotl = sqrt(am) * esine/rl;
gradyhillhouse 0:806c1dd6946c 1920 rvdotl = sqrt(pl) / rl;
gradyhillhouse 0:806c1dd6946c 1921 betal = sqrt(1.0 - el2);
gradyhillhouse 0:806c1dd6946c 1922 temp = esine / (1.0 + betal);
gradyhillhouse 0:806c1dd6946c 1923 sinu = am / rl * (sineo1 - aynl - axnl * temp);
gradyhillhouse 0:806c1dd6946c 1924 cosu = am / rl * (coseo1 - axnl + aynl * temp);
gradyhillhouse 0:806c1dd6946c 1925 su = atan2(sinu, cosu);
gradyhillhouse 0:806c1dd6946c 1926 sin2u = (cosu + cosu) * sinu;
gradyhillhouse 0:806c1dd6946c 1927 cos2u = 1.0 - 2.0 * sinu * sinu;
gradyhillhouse 0:806c1dd6946c 1928 temp = 1.0 / pl;
gradyhillhouse 0:806c1dd6946c 1929 temp1 = 0.5 * j2 * temp;
gradyhillhouse 0:806c1dd6946c 1930 temp2 = temp1 * temp;
gradyhillhouse 0:806c1dd6946c 1931
gradyhillhouse 0:806c1dd6946c 1932 /* -------------- update for short period periodics ------------ */
gradyhillhouse 0:806c1dd6946c 1933 if (satrec.method == 'd')
gradyhillhouse 0:806c1dd6946c 1934 {
gradyhillhouse 0:806c1dd6946c 1935 cosisq = cosip * cosip;
gradyhillhouse 0:806c1dd6946c 1936 satrec.con41 = 3.0*cosisq - 1.0;
gradyhillhouse 0:806c1dd6946c 1937 satrec.x1mth2 = 1.0 - cosisq;
gradyhillhouse 0:806c1dd6946c 1938 satrec.x7thm1 = 7.0*cosisq - 1.0;
gradyhillhouse 0:806c1dd6946c 1939 }
gradyhillhouse 0:806c1dd6946c 1940 mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) +
gradyhillhouse 0:806c1dd6946c 1941 0.5 * temp1 * satrec.x1mth2 * cos2u;
gradyhillhouse 0:806c1dd6946c 1942 su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u;
gradyhillhouse 0:806c1dd6946c 1943 xnode = nodep + 1.5 * temp2 * cosip * sin2u;
gradyhillhouse 0:806c1dd6946c 1944 xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
gradyhillhouse 0:806c1dd6946c 1945 mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke;
gradyhillhouse 0:806c1dd6946c 1946 rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u +
gradyhillhouse 0:806c1dd6946c 1947 1.5 * satrec.con41) / xke;
gradyhillhouse 0:806c1dd6946c 1948
gradyhillhouse 0:806c1dd6946c 1949 /* --------------------- orientation vectors ------------------- */
gradyhillhouse 0:806c1dd6946c 1950 sinsu = sin(su);
gradyhillhouse 0:806c1dd6946c 1951 cossu = cos(su);
gradyhillhouse 0:806c1dd6946c 1952 snod = sin(xnode);
gradyhillhouse 0:806c1dd6946c 1953 cnod = cos(xnode);
gradyhillhouse 0:806c1dd6946c 1954 sini = sin(xinc);
gradyhillhouse 0:806c1dd6946c 1955 cosi = cos(xinc);
gradyhillhouse 0:806c1dd6946c 1956 xmx = -snod * cosi;
gradyhillhouse 0:806c1dd6946c 1957 xmy = cnod * cosi;
gradyhillhouse 0:806c1dd6946c 1958 ux = xmx * sinsu + cnod * cossu;
gradyhillhouse 0:806c1dd6946c 1959 uy = xmy * sinsu + snod * cossu;
gradyhillhouse 0:806c1dd6946c 1960 uz = sini * sinsu;
gradyhillhouse 0:806c1dd6946c 1961 vx = xmx * cossu - cnod * sinsu;
gradyhillhouse 0:806c1dd6946c 1962 vy = xmy * cossu - snod * sinsu;
gradyhillhouse 0:806c1dd6946c 1963 vz = sini * cossu;
gradyhillhouse 0:806c1dd6946c 1964
gradyhillhouse 0:806c1dd6946c 1965 /* --------- position and velocity (in km and km/sec) ---------- */
gradyhillhouse 0:806c1dd6946c 1966 r[0] = (mrt * ux)* radiusearthkm;
gradyhillhouse 0:806c1dd6946c 1967 r[1] = (mrt * uy)* radiusearthkm;
gradyhillhouse 0:806c1dd6946c 1968 r[2] = (mrt * uz)* radiusearthkm;
gradyhillhouse 0:806c1dd6946c 1969 v[0] = (mvt * ux + rvdot * vx) * vkmpersec;
gradyhillhouse 0:806c1dd6946c 1970 v[1] = (mvt * uy + rvdot * vy) * vkmpersec;
gradyhillhouse 0:806c1dd6946c 1971 v[2] = (mvt * uz + rvdot * vz) * vkmpersec;
gradyhillhouse 0:806c1dd6946c 1972 } // if pl > 0
gradyhillhouse 0:806c1dd6946c 1973
gradyhillhouse 0:806c1dd6946c 1974 // sgp4fix for decaying satellites
gradyhillhouse 0:806c1dd6946c 1975 if (mrt < 1.0)
gradyhillhouse 0:806c1dd6946c 1976 {
gradyhillhouse 0:806c1dd6946c 1977 // printf("# decay condition %11.6f \n",mrt);
gradyhillhouse 0:806c1dd6946c 1978 satrec.error = 6;
gradyhillhouse 0:806c1dd6946c 1979 return false;
gradyhillhouse 0:806c1dd6946c 1980 }
gradyhillhouse 0:806c1dd6946c 1981
gradyhillhouse 0:806c1dd6946c 1982 //#include "debug7.cpp"
gradyhillhouse 0:806c1dd6946c 1983 return true;
gradyhillhouse 0:806c1dd6946c 1984 } // end sgp4
gradyhillhouse 0:806c1dd6946c 1985
gradyhillhouse 0:806c1dd6946c 1986
gradyhillhouse 0:806c1dd6946c 1987 /* -----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 1988 *
gradyhillhouse 0:806c1dd6946c 1989 * function gstime
gradyhillhouse 0:806c1dd6946c 1990 *
gradyhillhouse 0:806c1dd6946c 1991 * this function finds the greenwich sidereal time.
gradyhillhouse 0:806c1dd6946c 1992 *
gradyhillhouse 0:806c1dd6946c 1993 * author : david vallado 719-573-2600 1 mar 2001
gradyhillhouse 0:806c1dd6946c 1994 *
gradyhillhouse 0:806c1dd6946c 1995 * inputs description range / units
gradyhillhouse 0:806c1dd6946c 1996 * jdut1 - julian date in ut1 days from 4713 bc
gradyhillhouse 0:806c1dd6946c 1997 *
gradyhillhouse 0:806c1dd6946c 1998 * outputs :
gradyhillhouse 0:806c1dd6946c 1999 * gstime - greenwich sidereal time 0 to 2pi rad
gradyhillhouse 0:806c1dd6946c 2000 *
gradyhillhouse 0:806c1dd6946c 2001 * locals :
gradyhillhouse 0:806c1dd6946c 2002 * temp - temporary variable for doubles rad
gradyhillhouse 0:806c1dd6946c 2003 * tut1 - julian centuries from the
gradyhillhouse 0:806c1dd6946c 2004 * jan 1, 2000 12 h epoch (ut1)
gradyhillhouse 0:806c1dd6946c 2005 *
gradyhillhouse 0:806c1dd6946c 2006 * coupling :
gradyhillhouse 0:806c1dd6946c 2007 * none
gradyhillhouse 0:806c1dd6946c 2008 *
gradyhillhouse 0:806c1dd6946c 2009 * references :
gradyhillhouse 0:806c1dd6946c 2010 * vallado 2004, 191, eq 3-45
gradyhillhouse 0:806c1dd6946c 2011 * --------------------------------------------------------------------------- */
gradyhillhouse 0:806c1dd6946c 2012
gradyhillhouse 0:806c1dd6946c 2013 double gstime
gradyhillhouse 0:806c1dd6946c 2014 (
gradyhillhouse 0:806c1dd6946c 2015 double jdut1
gradyhillhouse 0:806c1dd6946c 2016 )
gradyhillhouse 0:806c1dd6946c 2017 {
gradyhillhouse 0:806c1dd6946c 2018 const double twopi = 2.0 * pi;
gradyhillhouse 0:806c1dd6946c 2019 const double deg2rad = pi / 180.0;
gradyhillhouse 0:806c1dd6946c 2020 double temp, tut1;
gradyhillhouse 0:806c1dd6946c 2021
gradyhillhouse 0:806c1dd6946c 2022 tut1 = (jdut1 - 2451545.0) / 36525.0;
gradyhillhouse 0:806c1dd6946c 2023 temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 +
gradyhillhouse 0:806c1dd6946c 2024 (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec
gradyhillhouse 0:806c1dd6946c 2025 temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad
gradyhillhouse 0:806c1dd6946c 2026
gradyhillhouse 0:806c1dd6946c 2027 // ------------------------ check quadrants ---------------------
gradyhillhouse 0:806c1dd6946c 2028 if (temp < 0.0)
gradyhillhouse 0:806c1dd6946c 2029 temp += twopi;
gradyhillhouse 0:806c1dd6946c 2030
gradyhillhouse 0:806c1dd6946c 2031 return temp;
gradyhillhouse 0:806c1dd6946c 2032 } // end gstime
gradyhillhouse 0:806c1dd6946c 2033
gradyhillhouse 0:806c1dd6946c 2034
gradyhillhouse 0:806c1dd6946c 2035 /* -----------------------------------------------------------------------------
gradyhillhouse 0:806c1dd6946c 2036 *
gradyhillhouse 0:806c1dd6946c 2037 * function getgravconst
gradyhillhouse 0:806c1dd6946c 2038 *
gradyhillhouse 0:806c1dd6946c 2039 * this function gets constants for the propagator. note that mu is identified to
gradyhillhouse 0:806c1dd6946c 2040 * facilitiate comparisons with newer models. the common useage is wgs72.
gradyhillhouse 0:806c1dd6946c 2041 *
gradyhillhouse 0:806c1dd6946c 2042 * author : david vallado 719-573-2600 21 jul 2006
gradyhillhouse 0:806c1dd6946c 2043 *
gradyhillhouse 0:806c1dd6946c 2044 * inputs :
gradyhillhouse 0:806c1dd6946c 2045 * whichconst - which set of constants to use wgs72old, wgs72, wgs84
gradyhillhouse 0:806c1dd6946c 2046 *
gradyhillhouse 0:806c1dd6946c 2047 * outputs :
gradyhillhouse 0:806c1dd6946c 2048 * tumin - minutes in one time unit
gradyhillhouse 0:806c1dd6946c 2049 * mu - earth gravitational parameter
gradyhillhouse 0:806c1dd6946c 2050 * radiusearthkm - radius of the earth in km
gradyhillhouse 0:806c1dd6946c 2051 * xke - reciprocal of tumin
gradyhillhouse 0:806c1dd6946c 2052 * j2, j3, j4 - un-normalized zonal harmonic values
gradyhillhouse 0:806c1dd6946c 2053 * j3oj2 - j3 divided by j2
gradyhillhouse 0:806c1dd6946c 2054 *
gradyhillhouse 0:806c1dd6946c 2055 * locals :
gradyhillhouse 0:806c1dd6946c 2056 *
gradyhillhouse 0:806c1dd6946c 2057 * coupling :
gradyhillhouse 0:806c1dd6946c 2058 * none
gradyhillhouse 0:806c1dd6946c 2059 *
gradyhillhouse 0:806c1dd6946c 2060 * references :
gradyhillhouse 0:806c1dd6946c 2061 * norad spacetrack report #3
gradyhillhouse 0:806c1dd6946c 2062 * vallado, crawford, hujsak, kelso 2006
gradyhillhouse 0:806c1dd6946c 2063 --------------------------------------------------------------------------- */
gradyhillhouse 0:806c1dd6946c 2064
gradyhillhouse 0:806c1dd6946c 2065 void getgravconst
gradyhillhouse 0:806c1dd6946c 2066 (
gradyhillhouse 0:806c1dd6946c 2067 gravconsttype whichconst,
gradyhillhouse 0:806c1dd6946c 2068 double& tumin,
gradyhillhouse 0:806c1dd6946c 2069 double& mu,
gradyhillhouse 0:806c1dd6946c 2070 double& radiusearthkm,
gradyhillhouse 0:806c1dd6946c 2071 double& xke,
gradyhillhouse 0:806c1dd6946c 2072 double& j2,
gradyhillhouse 0:806c1dd6946c 2073 double& j3,
gradyhillhouse 0:806c1dd6946c 2074 double& j4,
gradyhillhouse 0:806c1dd6946c 2075 double& j3oj2
gradyhillhouse 0:806c1dd6946c 2076 )
gradyhillhouse 0:806c1dd6946c 2077 {
gradyhillhouse 0:806c1dd6946c 2078
gradyhillhouse 0:806c1dd6946c 2079 switch (whichconst)
gradyhillhouse 0:806c1dd6946c 2080 {
gradyhillhouse 0:806c1dd6946c 2081 // -- wgs-72 low precision str#3 constants --
gradyhillhouse 0:806c1dd6946c 2082 case wgs72old:
gradyhillhouse 0:806c1dd6946c 2083 mu = 398600.79964; // in km3 / s2
gradyhillhouse 0:806c1dd6946c 2084 radiusearthkm = 6378.135; // km
gradyhillhouse 0:806c1dd6946c 2085 xke = 0.0743669161;
gradyhillhouse 0:806c1dd6946c 2086 tumin = 1.0 / xke;
gradyhillhouse 0:806c1dd6946c 2087 j2 = 0.001082616;
gradyhillhouse 0:806c1dd6946c 2088 j3 = -0.00000253881;
gradyhillhouse 0:806c1dd6946c 2089 j4 = -0.00000165597;
gradyhillhouse 0:806c1dd6946c 2090 j3oj2 = j3 / j2;
gradyhillhouse 0:806c1dd6946c 2091 break;
gradyhillhouse 0:806c1dd6946c 2092 // ------------ wgs-72 constants ------------
gradyhillhouse 0:806c1dd6946c 2093 case wgs72:
gradyhillhouse 0:806c1dd6946c 2094 mu = 398600.8; // in km3 / s2
gradyhillhouse 0:806c1dd6946c 2095 radiusearthkm = 6378.135; // km
gradyhillhouse 0:806c1dd6946c 2096 xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
gradyhillhouse 0:806c1dd6946c 2097 tumin = 1.0 / xke;
gradyhillhouse 0:806c1dd6946c 2098 j2 = 0.001082616;
gradyhillhouse 0:806c1dd6946c 2099 j3 = -0.00000253881;
gradyhillhouse 0:806c1dd6946c 2100 j4 = -0.00000165597;
gradyhillhouse 0:806c1dd6946c 2101 j3oj2 = j3 / j2;
gradyhillhouse 0:806c1dd6946c 2102 break;
gradyhillhouse 0:806c1dd6946c 2103 case wgs84:
gradyhillhouse 0:806c1dd6946c 2104 // ------------ wgs-84 constants ------------
gradyhillhouse 0:806c1dd6946c 2105 mu = 398600.5; // in km3 / s2
gradyhillhouse 0:806c1dd6946c 2106 radiusearthkm = 6378.137; // km
gradyhillhouse 0:806c1dd6946c 2107 xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
gradyhillhouse 0:806c1dd6946c 2108 tumin = 1.0 / xke;
gradyhillhouse 0:806c1dd6946c 2109 j2 = 0.00108262998905;
gradyhillhouse 0:806c1dd6946c 2110 j3 = -0.00000253215306;
gradyhillhouse 0:806c1dd6946c 2111 j4 = -0.00000161098761;
gradyhillhouse 0:806c1dd6946c 2112 j3oj2 = j3 / j2;
gradyhillhouse 0:806c1dd6946c 2113 break;
gradyhillhouse 0:806c1dd6946c 2114 default:
gradyhillhouse 0:806c1dd6946c 2115 // fprintf(stderr,"unknown gravity option (%d)\n",whichconst);
gradyhillhouse 0:806c1dd6946c 2116 break;
gradyhillhouse 0:806c1dd6946c 2117 }
gradyhillhouse 0:806c1dd6946c 2118
gradyhillhouse 0:806c1dd6946c 2119 } // end getgravconst
gradyhillhouse 0:806c1dd6946c 2120
gradyhillhouse 0:806c1dd6946c 2121
gradyhillhouse 0:806c1dd6946c 2122
gradyhillhouse 0:806c1dd6946c 2123