├── LICENSE ├── README.md ├── examples ├── Sgp4Predictor │ └── Sgp4Predictor.ino └── Sgp4Tracker │ └── Sgp4Tracker.ino ├── keywords.txt ├── library.properties └── src ├── Sgp4.h ├── brent.cpp ├── brent.h ├── sgp4coord.cpp ├── sgp4coord.h ├── sgp4ext.cpp ├── sgp4ext.h ├── sgp4io.cpp ├── sgp4io.h ├── sgp4pred.cpp ├── sgp4pred.h ├── sgp4unit.cpp ├── sgp4unit.h ├── visible.cpp └── visible.h /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Sgp4-Library 2 | Library for calculating satellite positions and predicting overpasses. 3 | Tested on an ESP8266 and ESP32. 4 | 5 | # Credits 6 | Original source code written by David Vallado: https://celestrak.com/software/vallado-sw.asp 7 | [Coordinate transformation](https://github.com/gradyh/ISS-Tracking-Pointer) ported by Grady Hillhouse. It is distributed under MIT license. 8 | -------------------------------------------------------------------------------- /examples/Sgp4Predictor/Sgp4Predictor.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | Sgp4 sat; 5 | unsigned long unixtime = 1458950400; 6 | int timezone = 12 ; //utc + 12 7 | 8 | int year; int mon; int day; int hr; int minute; double sec; 9 | 10 | 11 | 12 | void Predict(int many){ 13 | 14 | passinfo overpass; //structure to store overpass info 15 | sat.initpredpoint( unixtime , 0.0 ); //finds the startpoint 16 | 17 | bool error; 18 | unsigned long start = millis(); 19 | for (int i = 0; i 2 | #include 3 | 4 | Sgp4 sat; 5 | Ticker tkSecond; 6 | unsigned long unixtime = 1458950400; 7 | int timezone = 12 ; //utc + 12 8 | int framerate; 9 | 10 | int year; int mon; int day; int hr; int minute; double sec; 11 | 12 | void Second_Tick() 13 | { 14 | unixtime += 1; 15 | 16 | invjday(sat.satJd , timezone,true, year, mon, day, hr, minute, sec); 17 | Serial.println(String(day) + '/' + String(mon) + '/' + String(year) + ' ' + String(hr) + ':' + String(minute) + ':' + String(sec)); 18 | Serial.println("azimuth = " + String( sat.satAz) + " elevation = " + String(sat.satEl) + " distance = " + String(sat.satDist)); 19 | Serial.println("latitude = " + String( sat.satLat) + " longitude = " + String( sat.satLon) + " altitude = " + String( sat.satAlt)); 20 | 21 | switch(sat.satVis){ 22 | case -2: 23 | Serial.println("Visible : Under horizon"); 24 | break; 25 | case -1: 26 | Serial.println("Visible : Daylight"); 27 | break; 28 | default: 29 | Serial.println("Visible : " + String(sat.satVis)); //0:eclipsed - 1000:visible 30 | break; 31 | } 32 | 33 | Serial.println("Framerate: " + String(framerate) + " calc/sec"); 34 | Serial.println(); 35 | 36 | framerate=0; 37 | } 38 | 39 | void setup() { 40 | 41 | Serial.begin(115200); 42 | Serial.println(); 43 | 44 | sat.site(-0.5276847,166.9359231,34); //set site latitude[°], longitude[°] and altitude[m] 45 | 46 | char satname[] = "ISS (ZARYA)"; 47 | char tle_line1[] = "1 25544U 98067A 16065.25775256 -.00164574 00000-0 -25195-2 0 9990"; //Line one from the TLE data 48 | char tle_line2[] = "2 25544 51.6436 216.3171 0002750 185.0333 238.0864 15.54246933988812"; //Line two from the TLE data 49 | 50 | sat.init(satname,tle_line1,tle_line2); //initialize satellite parameters 51 | 52 | //Display TLE epoch time 53 | double jdC = sat.satrec.jdsatepoch; 54 | invjday(jdC , timezone, true, year, mon, day, hr, minute, sec); 55 | Serial.println("Epoch: " + String(day) + '/' + String(mon) + '/' + String(year) + ' ' + String(hr) + ':' + String(minute) + ':' + String(sec)); 56 | Serial.println(); 57 | 58 | tkSecond.attach(1,Second_Tick); 59 | } 60 | 61 | 62 | void loop() { 63 | 64 | sat.findsat(unixtime); 65 | framerate += 1; 66 | 67 | } 68 | 69 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | Sgp4 KEYWORD1 2 | 3 | init KEYWORD2 4 | site KEYWORD2 5 | setsunrise KEYWORD2 6 | findsat KEYWORD2 7 | nextpass KEYWORD2 8 | initpredpoint KEYWORD2 9 | visible KEYWORD2 10 | 11 | satLat KEYWORD2 12 | satLon KEYWORD2 13 | satAlt KEYWORD2 14 | satAz KEYWORD2 15 | satEl KEYWORD2 16 | satDist KEYWORD2 17 | satName KEYWORD2 18 | satVis KEYWORD2 19 | satJd KEYWORD2 20 | sunAz KEYWORD2 21 | sunEl KEYWORD2 22 | line1 KEYWORD2 23 | line2 KEYWORD2 24 | 25 | passinfo LITERAL2 26 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Sgp4 2 | version=1.0.3 3 | author=Hopperpop 4 | maintainer=Hopperpop 5 | sentence=Sgp4 model for the ESP8266 6 | paragraph= 7 | category=Uncategorized 8 | url=https://github.com/Hopperpop/Sgp4-Library 9 | architectures=* 10 | -------------------------------------------------------------------------------- /src/Sgp4.h: -------------------------------------------------------------------------------- 1 | #ifndef SGP4_H 2 | #define SGP4_H 3 | 4 | #include "sgp4pred.h" 5 | 6 | #endif -------------------------------------------------------------------------------- /src/brent.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "brent.h" 4 | #include "sgp4pred.h" 5 | 6 | #define ITMAX 100 //Here ITMAX is the maximum allowed number of iterations; 7 | #define R 0.61803399 8 | #define C 0.3819660 //CGOLD is the golden ratio; 9 | #define ZEPS 2.22e-15 // ZEPS is a small number that protects against trying to achieve fractional accuracy for a minimum that happens to be exactly zero. 10 | 11 | #define SHFT2(a,b,c) (a)=(b);(b)=(c); 12 | #define SHFT3(a,b,c,d) (a)=(b);(b)=(c);(c)=(d); 13 | 14 | double brentmin(double ax, double bx, double cx, double (Sgp4::*f)(double), double tol, double *xmin, Sgp4* obj) 15 | //Given a function f, and given a bracketing triplet of abscissas ax, bx, cx (such that bx is 16 | //between ax and cx, and f(bx) is less than both f(ax) and f(cx)), this routine isolates 17 | //the minimum to a fractional precision of about tol using Brent’s method. The abscissa of 18 | //the minimum is returned as xmin, and the minimum function value is returned as brent, the 19 | //returned function value. 20 | { 21 | 22 | int iter; 23 | double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm; 24 | double e=0.0; //This will be the distance moved on the step before last. 25 | 26 | a=(ax < cx ? ax : cx); //a and b must be in ascending order, 27 | b=(ax > cx ? ax : cx); //but input abscissas need not be. 28 | x=w=v=bx; //Initializations... 29 | fw=fv=fx=(obj->*f)(x); 30 | for (iter = 1; iter <= ITMAX; iter++) { //Main program loop. 31 | xm = 0.5*(a+b); 32 | tol2 = 2.0*(tol1=tol+ZEPS); //2.0*(tol1=tol*fabs(x)+ZEPS); 33 | if (fabs(x-xm) <= (tol2-0.5*(b-a))) {// Test for done here. 34 | *xmin=x; 35 | return fx; 36 | } 37 | if (fabs(e) > tol1) { //Construct a trial parabolic fit. 38 | r=(x-w)*(fx-fv); 39 | q=(x-v)*(fx-fw); 40 | p=(x-v)*q-(x-w)*r; 41 | q=2.0*(q-r); 42 | if (q > 0.0) p = -p; 43 | q=fabs(q); 44 | etemp=e; 45 | e=d; 46 | if (fabs(p) >= fabs(0.5*q*etemp) || p <= q*(a-x) || p >= q*(b-x)) 47 | d=C*(e=(x >= xm ? a-x : b-x)); 48 | //The above conditions determine the acceptability of the parabolic fit. Here we 49 | //take the golden section step into the larger of the two segments. 50 | else { 51 | d=p/q; //Take the parabolic step. 52 | u=x+d; 53 | if (u-a < tol2 || b-u < tol2) 54 | d=copysign(tol1,xm-x); 55 | } 56 | } else { 57 | d=C*(e=(x >= xm ? a-x : b-x)); 58 | } 59 | u=(fabs(d) >= tol1 ? x+d : x+copysign(tol1,d)); 60 | fu=(obj->*f)(u); 61 | //This is the one function evaluation per iteration. 62 | if (fu <= fx) { //Now decide what to do with our func 63 | if (u >= x) a=x; else b=x; //tion evaluation. 64 | SHFT3(v,w,x,u) //Housekeeping follows: 65 | SHFT3(fv,fw,fx,fu) 66 | } else { 67 | if (u < x) a=u; else b=u; 68 | if (fu <= fw || w == x) { 69 | v=w; 70 | w=u; 71 | fv=fw; 72 | fw=fu; 73 | } else if (fu <= fv || v == x || v == w) { 74 | v=u; 75 | fv=fu; 76 | } 77 | } //Done with housekeeping. Back for 78 | } //another iteration. 79 | //nrerror("Too many iterations in brent"); 80 | *xmin=-1.0; //Never get here. 81 | return fx; 82 | 83 | } 84 | 85 | 86 | double zbrent(double (Sgp4::*func)(double), double x1, double x2, double tol, Sgp4* obj) 87 | //Using Brent’s method, find the root of a function func known to lie between x1 and x2. The 88 | //root, returned as zbrent, will be refined until its accuracy is tol. 89 | { 90 | int iter; 91 | double a=x1,b=x2,c=x2,d,e,min1,min2; 92 | double fa=(obj->*func)(a),fb=(obj->*func)(b),fc,p,q,r,s,tol1,xm; 93 | 94 | if ((fa > 0.0 && fb > 0.0) || (fa < 0.0 && fb < 0.0)) 95 | return -1.0; 96 | fc=fb; 97 | for (iter=1;iter<=ITMAX;iter++) { 98 | if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0)) { 99 | c=a; //Rename a, b, c and adjust bounding interval 100 | fc=fa; //d. 101 | e=d=b-a; 102 | } 103 | if (fabs(fc) < fabs(fb)) { 104 | a=b; 105 | b=c; 106 | c=a; 107 | fa=fb; 108 | fb=fc; 109 | fc=fa; 110 | } 111 | tol1=2.0*ZEPS+0.5*tol; //Convergence check. 112 | xm=0.5*(c-b); 113 | if (fabs(xm) <= tol1 || fb == 0.0) return b; 114 | if (fabs(e) >= tol1 && fabs(fa) > fabs(fb)) { 115 | s=fb/fa; //Attempt inverse quadratic interpolation. 116 | if (a == c) { 117 | p=2.0*xm*s; 118 | q=1.0-s; 119 | } else { 120 | q=fa/fc; 121 | r=fb/fc; 122 | p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0)); 123 | q=(q-1.0)*(r-1.0)*(s-1.0); 124 | } 125 | if (p > 0.0) q = -q; //Check whether in bounds. 126 | p=fabs(p); 127 | min1=3.0*xm*q-fabs(tol1*q); 128 | min2=fabs(e*q); 129 | if (2.0*p < (min1 < min2 ? min1 : min2)) { 130 | e=d; //Accept interpolation. 131 | d=p/q; 132 | } else { 133 | d=xm; //Interpolation failed, use bisection. 134 | e=d; 135 | } 136 | } else { //Bounds decreasing too slowly, use bisection. 137 | d=xm; 138 | e=d; 139 | } 140 | a=b; //Move last best guess to a. 141 | fa=fb; 142 | if (fabs(d) > tol1) //Evaluate new trial root. 143 | b += d; 144 | else 145 | b += copysign(tol1,xm); 146 | fb=(obj->*func)(b); 147 | } 148 | //nrerror("Maximum number of iterations exceeded in zbrent"); 149 | return -1.0; //Never get here. 150 | } 151 | -------------------------------------------------------------------------------- /src/brent.h: -------------------------------------------------------------------------------- 1 | #ifndef _brent_ 2 | #define _brent_ 3 | 4 | #include "sgp4pred.h" 5 | 6 | //Given a function f, and given a bracketing triplet of abscissas ax, bx, cx (such that bx is 7 | //between ax and cx, and f(bx) is less than both f(ax) and f(cx)), this routine isolates 8 | //the minimum to a fractional precision of about tol using Brent’s method. The abscissa of 9 | //the minimum is returned as xmin, and the minimum function value is returned as brent, the 10 | //returned function value. 11 | double brentmin(double ax, double bx, double cx, double (Sgp4::*f)(double), double tol, double *xmin, Sgp4* obj); 12 | 13 | //Using Brent’s method, find the root of a function func known to lie between x1 and x2. The 14 | //root, returned as zbrent, will be refined until its accuracy is tol. 15 | double zbrent(double (Sgp4::*func)(double), double x1, double x2, double tol, Sgp4* obj); 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /src/sgp4coord.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | sgp4coord.cpp 3 | 4 | This file contains miscellaneous functions required for coordinate transformation. 5 | Functions were originally written for Matlab as companion code for "Fundamentals of Astrodynamics 6 | and Applications" by David Vallado (2007). (w) 719-573-2600, email dvallado@agi.com 7 | 8 | Ported to C++ by Grady Hillhouse with some modifications, July 2015. 9 | */ 10 | #include "sgp4coord.h" 11 | #include "sgp4ext.h" 12 | 13 | /* 14 | teme2ecef 15 | 16 | This function transforms a vector from a true equator mean equinox (TEME) 17 | frame to an earth-centered, earth-fixed (ECEF) frame. 18 | 19 | Author: David Vallado, 2007 20 | Ported to C++ by Grady Hillhouse with some modifications, July 2015. 21 | 22 | INPUTS DESCRIPTION RANGE/UNITS 23 | rteme Position vector (TEME) km 24 | vteme Velocity vector (TEME) km/s 25 | jdut1 Julian date days 26 | 27 | OUTPUTS DESCRIPTION RANGE/UNITS 28 | recef Position vector (ECEF) km 29 | vecef Velocity vector (ECEF) km/s 30 | */ 31 | 32 | //void teme2ecef(double rteme[3], double vteme[3], double jdut1, double recef[3], double vecef[3]) 33 | void teme2ecef(double rteme[3], double jdut1, double recef[3]) 34 | { 35 | double gmst; 36 | double st[3][3]; 37 | double rpef[3]; 38 | //double vpef[3]; 39 | double pm[3][3]; 40 | //double omegaearth[3]; 41 | 42 | //Get Greenwich mean sidereal time 43 | gmst = gstime(jdut1); 44 | 45 | //st is the pef - tod matrix 46 | st[0][0] = cos(gmst); 47 | st[0][1] = -sin(gmst); 48 | st[0][2] = 0.0; 49 | st[1][0] = sin(gmst); 50 | st[1][1] = cos(gmst); 51 | st[1][2] = 0.0; 52 | st[2][0] = 0.0; 53 | st[2][1] = 0.0; 54 | st[2][2] = 1.0; 55 | 56 | //Get pseudo earth fixed position vector by multiplying the inverse pef-tod matrix by rteme 57 | rpef[0] = st[0][0] * rteme[0] + st[1][0] * rteme[1] + st[2][0] * rteme[2]; 58 | rpef[1] = st[0][1] * rteme[0] + st[1][1] * rteme[1] + st[2][1] * rteme[2]; 59 | rpef[2] = st[0][2] * rteme[0] + st[1][2] * rteme[1] + st[2][2] * rteme[2]; 60 | 61 | //Get polar motion vector 62 | polarm(jdut1, pm); 63 | 64 | //ECEF postion vector is the inverse of the polar motion vector multiplied by rpef 65 | recef[0] = pm[0][0] * rpef[0] + pm[1][0] * rpef[1] + pm[2][0] * rpef[2]; 66 | recef[1] = pm[0][1] * rpef[0] + pm[1][1] * rpef[1] + pm[2][1] * rpef[2]; 67 | recef[2] = pm[0][2] * rpef[0] + pm[1][2] * rpef[1] + pm[2][2] * rpef[2]; 68 | 69 | //Earth's angular rotation vector (omega) 70 | //Note: I don't have a good source for LOD. Historically it has been on the order of 2 ms so I'm just using that as a constant. The effect is very small. 71 | //omegaearth[0] = 0.0; 72 | //omegaearth[1] = 0.0; 73 | //omegaearth[2] = 7.29211514670698e-05 * (1.0 - 0.0015563/86400.0); 74 | 75 | //Pseudo Earth Fixed velocity vector is st'*vteme - omegaearth X rpef 76 | //vpef[0] = st[0][0] * vteme[0] + st[1][0] * vteme[1] + st[2][0] * vteme[2] - (omegaearth[1]*rpef[2] - omegaearth[2]*rpef[1]); 77 | //vpef[1] = st[0][1] * vteme[0] + st[1][1] * vteme[1] + st[2][1] * vteme[2] - (omegaearth[2]*rpef[0] - omegaearth[0]*rpef[2]); 78 | //vpef[2] = st[0][2] * vteme[0] + st[1][2] * vteme[1] + st[2][2] * vteme[2] - (omegaearth[0]*rpef[1] - omegaearth[1]*rpef[0]); 79 | 80 | //ECEF velocty vector is the inverse of the polar motion vector multiplied by vpef 81 | //vecef[0] = pm[0][0] * vpef[0] + pm[1][0] * vpef[1] + pm[2][0] * vpef[2]; 82 | //vecef[1] = pm[0][1] * vpef[0] + pm[1][1] * vpef[1] + pm[2][1] * vpef[2]; 83 | //vecef[2] = pm[0][2] * vpef[0] + pm[1][2] * vpef[1] + pm[2][2] * vpef[2]; 84 | } 85 | 86 | /* 87 | polarm 88 | 89 | This function calulates the transformation matrix that accounts for polar 90 | motion. Polar motion coordinates are estimated using IERS Bulletin 91 | rather than directly input for simplicity. 92 | 93 | Author: David Vallado, 2007 94 | Ported to C++ by Grady Hillhouse with some modifications, July 2015. 95 | 96 | INPUTS DESCRIPTION RANGE/UNITS 97 | jdut1 Julian date days 98 | 99 | OUTPUTS DESCRIPTION 100 | pm Transformation matrix for ECEF - PEF 101 | */ 102 | 103 | void polarm(double jdut1, double pm[3][3]) 104 | { 105 | double MJD; //Julian Date - 2,400,000.5 days 106 | double A; 107 | double C; 108 | double xp; //Polar motion coefficient in radians 109 | double yp; //Polar motion coefficient in radians 110 | 111 | //Predict polar motion coefficients using IERS Bulletin - A (Vol. XXVIII No. 030) 112 | MJD = jdut1 - 2400000.5; 113 | A = 2 * pi * (MJD - 57226) / 365.25; 114 | C = 2 * pi * (MJD - 57226) / 435; 115 | 116 | xp = (0.1033 + 0.0494*cos(A) + 0.0482*sin(A) + 0.0297*cos(C) + 0.0307*sin(C)) * 4.84813681e-6; 117 | yp = (0.3498 + 0.0441*cos(A) - 0.0393*sin(A) + 0.0307*cos(C) - 0.0297*sin(C)) * 4.84813681e-6; 118 | 119 | pm[0][0] = cos(xp); 120 | pm[0][1] = 0.0; 121 | pm[0][2] = -sin(xp); 122 | pm[1][0] = sin(xp) * sin(yp); 123 | pm[1][1] = cos(yp); 124 | pm[1][2] = cos(xp) * sin(yp); 125 | pm[2][0] = sin(xp) * cos(yp); 126 | pm[2][1] = -sin(yp); 127 | pm[2][2] = cos(xp) * cos(yp); 128 | } 129 | 130 | /* 131 | ijk2ll 132 | 133 | This function calulates the latitude, longitude and altitude 134 | given the ECEF position matrix. 135 | 136 | Author: David Vallado, 2007 137 | Ported to C++ by Grady Hillhouse with some modifications, July 2015. 138 | 139 | INPUTS DESCRIPTION RANGE/UNITS 140 | r Position matrix (ECEF) km 141 | 142 | OUTPUTS DESCRIPTION 143 | latlongh Latitude, longitude, and altitude (rad, rad, and km) 144 | */ 145 | 146 | void ijk2ll(double r[3], double latlongh[3]) 147 | { 148 | double twopi = 2.0*pi; 149 | double small = 0.00000001; //small value for tolerances 150 | double re = 6378.137; //radius of earth in km 151 | double eesqrd = 0.006694385000; //eccentricity of earth sqrd 152 | double magr, temp, rtasc; 153 | 154 | magr = mag(r); 155 | temp = sqrt(r[0]*r[0] + r[1]*r[1]); 156 | 157 | if(fabs(temp) < small) 158 | { 159 | rtasc = sgn(r[2]) * pi * 0.5; 160 | } 161 | else 162 | { 163 | rtasc = atan2(r[1], r[0]); 164 | } 165 | 166 | latlongh[1] = rtasc; 167 | 168 | if (fabs(latlongh[1]) >= pi) 169 | { 170 | if (latlongh[1] < 0.0) 171 | { 172 | latlongh[1] += twopi; 173 | } 174 | else 175 | { 176 | latlongh[1] -= twopi; 177 | } 178 | } 179 | 180 | latlongh[0] = asin(r[2] / magr); 181 | 182 | //Iterate to find geodetic latitude 183 | int i = 1; 184 | double olddelta = latlongh[0] + 10.0; 185 | double sintemp, c = 0; 186 | 187 | while ( (fabs(olddelta - latlongh[0]) >= small) && (i < 10) ) 188 | { 189 | olddelta = latlongh[0]; 190 | sintemp = sin(latlongh[0]); 191 | c = re / sqrt(1.0 - eesqrd*sintemp*sintemp); 192 | latlongh[0] = atan( (r[2] + c*eesqrd*sintemp) / temp ); 193 | i++; 194 | } 195 | 196 | if (0.5*pi - fabs(latlongh[0]) > pi/180.0) 197 | { 198 | latlongh[2] = (temp/cos(latlongh[0])) - c; 199 | } 200 | else 201 | { 202 | latlongh[2] = r[2]/sin(latlongh[0]) - c*(1.0 - eesqrd); 203 | } 204 | } 205 | 206 | /* 207 | site 208 | 209 | This function finds the position and velocity vectors for a site. The 210 | outputs are in the ECEF coordinate system. Note that the velocity vector 211 | is zero because the coordinate system rotates with the earth. 212 | 213 | Author: David Vallado, 2007 214 | Ported to C++ by Grady Hillhouse with some modifications, July 2015. 215 | 216 | INPUTS DESCRIPTION RANGE/UNITS 217 | latgd Site geodetic latitude -pi/2 to pi/2 in radians 218 | lon Longitude -2pi to 2pi in radians 219 | alt Site altitude km 220 | 221 | OUTPUTS DESCRIPTION 222 | rs Site position vector km 223 | vs Site velocity vector km/s 224 | */ 225 | 226 | //void site(double latgd, double lon, double alt, double rs[3], double vs[3]) 227 | void site(double latgd, double lon, double alt, double rs[3]) 228 | { 229 | double sinlat, re, eesqrd, cearth, rdel, rk; 230 | re = 6378.137; //radius of earth in km 231 | eesqrd = 0.006694385000; //eccentricity of earth sqrd 232 | 233 | //Find rdel and rk components of site vector 234 | sinlat = sin(latgd); 235 | cearth = re / sqrt( 1.0 - (eesqrd*sinlat*sinlat) ); 236 | rdel = (cearth + alt) * cos(latgd); 237 | rk = ((1.0 - eesqrd) * cearth + alt ) * sinlat; 238 | 239 | //Find site position vector (ECEF) 240 | rs[0] = rdel * cos( lon ); 241 | rs[1] = rdel * sin( lon ); 242 | rs[2] = rk; 243 | 244 | //Velocity of site is zero because the coordinate system is rotating with the earth 245 | //vs[0] = 0.0; 246 | //vs[1] = 0.0; 247 | //vs[2] = 0.0; 248 | } 249 | 250 | 251 | /* 252 | rv2azel 253 | 254 | This function calculates the range, elevation, and azimuth (and their rates) 255 | from the TEME vectors output by the SGP4 function. 256 | 257 | Author: David Vallado, 2007 258 | Ported to C++ by Grady Hillhouse with some modifications, July 2015. 259 | 260 | INPUTS DESCRIPTION RANGE/UNITS 261 | ro Sat. position vector (TEME) km 262 | vo Sat. velocity vector (TEME) km/s 263 | latgd Site geodetic latitude -pi/2 to pi/2 in radians 264 | lon Site longitude -2pi to 2pi in radians 265 | alt Site altitude km 266 | jdut1 Julian date days 267 | 268 | OUTPUTS DESCRIPTION 269 | razel Range, azimuth, and elevation matrix 270 | razelrates Range rate, azimuth rate, and elevation rate matrix 271 | */ 272 | 273 | //void rv2azel(double ro[3], double vo[3], double latgd, double lon, double alt, double jdut1, double razel[3], double razelrates[3]) 274 | void rv2azel(double ro[3], double latgd, double lon, double alt, double jdut1, double razel[3]) 275 | { 276 | //Locals 277 | double halfpi = pi * 0.5; 278 | double small = 0.00000001; 279 | double temp; 280 | double rs[3]; 281 | //double vs[3]; 282 | double recef[3]; 283 | //double vecef[3]; 284 | double rhoecef[3]; 285 | //double drhoecef[3]; 286 | double tempvec[3]; 287 | double rhosez[3]; 288 | //double drhosez[3]; 289 | double magrhosez; 290 | double rho, az, el; 291 | //double drho, daz, del; 292 | 293 | //Get site vector in ECEF coordinate system 294 | //site(latgd, lon, alt, rs, vs); 295 | site(latgd, lon, alt, rs); 296 | 297 | //Convert TEME vectors to ECEF coordinate system 298 | //teme2ecef(ro, vo, jdut1, recef, vecef); 299 | teme2ecef(ro, jdut1, recef); 300 | 301 | //Find ECEF range vectors 302 | for (int i = 0; i < 3; i++) 303 | { 304 | rhoecef[i] = recef[i] - rs[i]; 305 | //drhoecef[i] = vecef[i]; 306 | } 307 | rho = mag(rhoecef); //Range in km 308 | 309 | //Convert to SEZ (topocentric horizon coordinate system) 310 | rot3(rhoecef, lon, tempvec); 311 | rot2(tempvec, (halfpi-latgd), rhosez); 312 | 313 | //rot3(drhoecef, lon, tempvec); 314 | //rot2(tempvec, (halfpi-latgd), drhosez); 315 | 316 | //Calculate azimuth, and elevation 317 | temp = sqrt(rhosez[0]*rhosez[0] + rhosez[1]*rhosez[1]); 318 | if (temp < small) 319 | { 320 | el = sgn(rhosez[2]) * halfpi; 321 | //az = atan2(drhosez[1], -drhosez[0]); 322 | az = NAN; 323 | } 324 | else 325 | { 326 | magrhosez = mag(rhosez); 327 | el = asin(rhosez[2]/magrhosez); 328 | az = atan2(rhosez[1], -rhosez[0]); 329 | } 330 | 331 | //Calculate rates for range, azimuth, and elevation 332 | /** 333 | drho = dot(rhosez,drhosez) / rho; 334 | 335 | if(fabs(temp*temp) > small) 336 | { 337 | daz = (drhosez[0]*rhosez[1] - drhosez[1]*rhosez[0]) / (temp * temp); 338 | } 339 | else 340 | { 341 | daz = 0.0; 342 | } 343 | 344 | if(fabs(temp) > small) 345 | { 346 | del = (drhosez[2] - drho*sin(el)) / temp; 347 | } 348 | else 349 | { 350 | del = 0.0; 351 | } 352 | ***/ 353 | //Move values to output vectors 354 | razel[0] = rho; //Range (km) 355 | razel[1] = az; //Azimuth (radians) 356 | razel[2] = el; //Elevation (radians) 357 | 358 | //razelrates[0] = drho; //Range rate (km/s) 359 | //razelrates[1] = daz; //Azimuth rate (rad/s) 360 | //razelrates[2] = del; //Elevation rate (rad/s) 361 | } 362 | 363 | void rot3(double invec[3], double xval, double outvec[3]) 364 | { 365 | double temp = invec[1]; 366 | double c = cos(xval); 367 | double s = sin(xval); 368 | 369 | outvec[1] = c*invec[1] - s*invec[0]; 370 | outvec[0] = c*invec[0] + s*temp; 371 | outvec[2] = invec[2]; 372 | } 373 | 374 | void rot2(double invec[3], double xval, double outvec[3]) 375 | { 376 | double temp = invec[2]; 377 | double c = cos(xval); 378 | double s = sin(xval); 379 | 380 | outvec[2] = c*invec[2] + s*invec[0]; 381 | outvec[0] = c*invec[0] - s*temp; 382 | outvec[1] = invec[1]; 383 | } 384 | 385 | /* 386 | getJulianFromUnix 387 | 388 | returns the Julian Date from Unix Time 389 | */ 390 | 391 | double getJulianFromUnix(double unixSecs) 392 | { 393 | return ( unixSecs / 86400.0 ) + 2440587.5; 394 | } 395 | 396 | 397 | unsigned long getUnixFromJulian(double julian) 398 | { 399 | return (unsigned long)((julian - 2440587.5)*86400.0 + 0.5); 400 | } 401 | -------------------------------------------------------------------------------- /src/sgp4coord.h: -------------------------------------------------------------------------------- 1 | #ifndef _sgp4coord_ 2 | #define _sgp4coord_ 3 | 4 | /* 5 | sgp4coord.h 6 | 7 | This file contains miscellaneous functions required for coordinate transformation. 8 | Functions were originally written for Matlab as companion code for "Fundamentals of Astrodynamics 9 | and Applications" by David Vallado (2007). (w) 719-573-2600, email dvallado@agi.com 10 | 11 | Ported to C++ by Grady Hillhouse with some modifications, July 2015. 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | //void teme2ecef(double rteme[3], double vteme[3], double jdut1, double recef[3], double vecef[3]); 18 | void teme2ecef(double rteme[3], double jdut1, double recef[3]); 19 | 20 | void polarm(double jdut1, double pm[3][3]); 21 | 22 | void ijk2ll(double r[3], double latlongh[3]); 23 | 24 | //void site(double latgd, double lon, double alt, double rs[3], double vs[3]); 25 | void site(double latgd, double lon, double alt, double rs[3]); 26 | 27 | //void rv2azel(double ro[3], double vo[3], double latgd, double lon, double alt, double jdut1, double razel[3], double razelrates[3]); 28 | void rv2azel(double ro[3], double latgd, double lon, double alt, double jdut1, double razel[3]); 29 | 30 | void rot3(double invec[3], double xval, double outvec[3]); 31 | 32 | void rot2(double invec[3], double xval, double outvec[3]); 33 | 34 | double getJulianFromUnix(double unixSecs); 35 | 36 | unsigned long getUnixFromJulian(double julian); 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/sgp4ext.cpp: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------- 2 | * 3 | * sgp4ext.cpp 4 | * 5 | * this file contains extra routines needed for the main test program for sgp4. 6 | * these routines are derived from the astro libraries. 7 | * 8 | * companion code for 9 | * fundamentals of astrodynamics and applications 10 | * 2007 11 | * by david vallado 12 | * 13 | * (w) 719-573-2600, email dvallado@agi.com 14 | * 15 | * current : 16 | * 7 may 08 david vallado 17 | * fix sgn 18 | * changes : 19 | * 2 apr 07 david vallado 20 | * fix jday floor and str lengths 21 | * updates for constants 22 | * 14 aug 06 david vallado 23 | * original baseline 24 | * ---------------------------------------------------------------- */ 25 | 26 | #include "sgp4ext.h" 27 | 28 | 29 | double sgn 30 | ( 31 | double x 32 | ) 33 | { 34 | if (x < 0.0) 35 | { 36 | return -1.0; 37 | } 38 | else 39 | { 40 | return 1.0; 41 | } 42 | 43 | } // end sgn 44 | 45 | /* ----------------------------------------------------------------------------- 46 | * 47 | * function mag 48 | * 49 | * this procedure finds the magnitude of a vector. the tolerance is set to 50 | * 0.000001, thus the 1.0e-12 for the squared test of underflows. 51 | * 52 | * author : david vallado 719-573-2600 1 mar 2001 53 | * 54 | * inputs description range / units 55 | * vec - vector 56 | * 57 | * outputs : 58 | * vec - answer stored in fourth component 59 | * 60 | * locals : 61 | * none. 62 | * 63 | * coupling : 64 | * none. 65 | * --------------------------------------------------------------------------- */ 66 | 67 | double mag 68 | ( 69 | double x[3] 70 | ) 71 | { 72 | return sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]); 73 | } // end mag 74 | 75 | /* ----------------------------------------------------------------------------- 76 | * 77 | * procedure cross 78 | * 79 | * this procedure crosses two vectors. 80 | * 81 | * author : david vallado 719-573-2600 1 mar 2001 82 | * 83 | * inputs description range / units 84 | * vec1 - vector number 1 85 | * vec2 - vector number 2 86 | * 87 | * outputs : 88 | * outvec - vector result of a x b 89 | * 90 | * locals : 91 | * none. 92 | * 93 | * coupling : 94 | * mag magnitude of a vector 95 | ---------------------------------------------------------------------------- */ 96 | 97 | void cross 98 | ( 99 | double vec1[3], double vec2[3], double outvec[3] 100 | ) 101 | { 102 | outvec[0]= vec1[1]*vec2[2] - vec1[2]*vec2[1]; 103 | outvec[1]= vec1[2]*vec2[0] - vec1[0]*vec2[2]; 104 | outvec[2]= vec1[0]*vec2[1] - vec1[1]*vec2[0]; 105 | } // end cross 106 | 107 | 108 | /* ----------------------------------------------------------------------------- 109 | * 110 | * function dot 111 | * 112 | * this function finds the dot product of two vectors. 113 | * 114 | * author : david vallado 719-573-2600 1 mar 2001 115 | * 116 | * inputs description range / units 117 | * vec1 - vector number 1 118 | * vec2 - vector number 2 119 | * 120 | * outputs : 121 | * dot - result 122 | * 123 | * locals : 124 | * none. 125 | * 126 | * coupling : 127 | * none. 128 | * 129 | * --------------------------------------------------------------------------- */ 130 | 131 | double dot 132 | ( 133 | double x[3], double y[3] 134 | ) 135 | { 136 | return (x[0]*y[0] + x[1]*y[1] + x[2]*y[2]); 137 | } // end dot 138 | 139 | /* ----------------------------------------------------------------------------- 140 | * 141 | * procedure angle 142 | * 143 | * this procedure calculates the angle between two vectors. the output is 144 | * set to 999999.1 to indicate an undefined value. be sure to check for 145 | * this at the output phase. 146 | * 147 | * author : david vallado 719-573-2600 1 mar 2001 148 | * 149 | * inputs description range / units 150 | * vec1 - vector number 1 151 | * vec2 - vector number 2 152 | * 153 | * outputs : 154 | * theta - angle between the two vectors -pi to pi 155 | * 156 | * locals : 157 | * temp - temporary real variable 158 | * 159 | * coupling : 160 | * dot dot product of two vectors 161 | * --------------------------------------------------------------------------- */ 162 | 163 | double angle 164 | ( 165 | double vec1[3], 166 | double vec2[3] 167 | ) 168 | { 169 | double small, undefined, magv1, magv2, temp; 170 | small = 0.00000001; 171 | undefined = 999999.1; 172 | 173 | magv1 = mag(vec1); 174 | magv2 = mag(vec2); 175 | 176 | if (magv1*magv2 > small*small) 177 | { 178 | temp= dot(vec1,vec2) / (magv1*magv2); 179 | if (fabs( temp ) > 1.0) 180 | temp= sgn(temp) * 1.0; 181 | return acos( temp ); 182 | } 183 | else 184 | return undefined; 185 | } // end angle 186 | 187 | 188 | /* ----------------------------------------------------------------------------- 189 | * 190 | * function asinh 191 | * 192 | * this function evaluates the inverse hyperbolic sine function. 193 | * 194 | * author : david vallado 719-573-2600 1 mar 2001 195 | * 196 | * inputs description range / units 197 | * xval - angle value any real 198 | * 199 | * outputs : 200 | * arcsinh - result any real 201 | * 202 | * locals : 203 | * none. 204 | * 205 | * coupling : 206 | * none. 207 | * 208 | * --------------------------------------------------------------------------- */ 209 | 210 | double asinh 211 | ( 212 | double xval 213 | ) 214 | { 215 | return log( xval + sqrt( xval*xval + 1.0 ) ); 216 | } // end asinh 217 | 218 | 219 | /* ----------------------------------------------------------------------------- 220 | * 221 | * function newtonnu 222 | * 223 | * this function solves keplers equation when the true anomaly is known. 224 | * the mean and eccentric, parabolic, or hyperbolic anomaly is also found. 225 | * the parabolic limit at 168� is arbitrary. the hyperbolic anomaly is also 226 | * limited. the hyperbolic sine is used because it's not double valued. 227 | * 228 | * author : david vallado 719-573-2600 27 may 2002 229 | * 230 | * revisions 231 | * vallado - fix small 24 sep 2002 232 | * 233 | * inputs description range / units 234 | * ecc - eccentricity 0.0 to 235 | * nu - true anomaly -2pi to 2pi rad 236 | * 237 | * outputs : 238 | * e0 - eccentric anomaly 0.0 to 2pi rad 153.02 � 239 | * m - mean anomaly 0.0 to 2pi rad 151.7425 � 240 | * 241 | * locals : 242 | * e1 - eccentric anomaly, next value rad 243 | * sine - sine of e 244 | * cose - cosine of e 245 | * ktr - index 246 | * 247 | * coupling : 248 | * asinh - arc hyperbolic sine 249 | * 250 | * references : 251 | * vallado 2007, 85, alg 5 252 | * --------------------------------------------------------------------------- */ 253 | 254 | void newtonnu 255 | ( 256 | double ecc, double nu, 257 | double& e0, double& m 258 | ) 259 | { 260 | double small, sine, cose; 261 | 262 | // --------------------- implementation --------------------- 263 | e0= 999999.9; 264 | m = 999999.9; 265 | small = 0.00000001; 266 | 267 | // --------------------------- circular ------------------------ 268 | if ( fabs( ecc ) < small ) 269 | { 270 | m = nu; 271 | e0= nu; 272 | } 273 | else 274 | // ---------------------- elliptical ----------------------- 275 | if ( ecc < 1.0-small ) 276 | { 277 | sine= ( sqrt( 1.0 -ecc*ecc ) * sin(nu) ) / ( 1.0 +ecc*cos(nu) ); 278 | cose= ( ecc + cos(nu) ) / ( 1.0 + ecc*cos(nu) ); 279 | e0 = atan2( sine,cose ); 280 | m = e0 - ecc*sin(e0); 281 | } 282 | else 283 | // -------------------- hyperbolic -------------------- 284 | if ( ecc > 1.0 + small ) 285 | { 286 | if ((ecc > 1.0 ) && (fabs(nu)+0.00001 < pi-acos(1.0 /ecc))) 287 | { 288 | sine= ( sqrt( ecc*ecc-1.0 ) * sin(nu) ) / ( 1.0 + ecc*cos(nu) ); 289 | e0 = asinh( sine ); 290 | m = ecc*sinh(e0) - e0; 291 | } 292 | } 293 | else 294 | // ----------------- parabolic --------------------- 295 | if ( fabs(nu) < 168.0*pi/180.0 ) 296 | { 297 | e0= tan( nu*0.5 ); 298 | m = e0 + (e0*e0*e0)/3.0; 299 | } 300 | 301 | if ( ecc < 1.0 ) 302 | { 303 | m = floatmod( m,2.0 *pi ); 304 | if ( m < 0.0 ) 305 | m = m + 2.0 *pi; 306 | e0 = floatmod( e0,2.0 *pi ); 307 | } 308 | } // end newtonnu 309 | 310 | 311 | /* ----------------------------------------------------------------------------- 312 | * 313 | * function rv2coe 314 | * 315 | * this function finds the classical orbital elements given the geocentric 316 | * equatorial position and velocity vectors. 317 | * 318 | * author : david vallado 719-573-2600 21 jun 2002 319 | * 320 | * revisions 321 | * vallado - fix special cases 5 sep 2002 322 | * vallado - delete extra check in inclination code 16 oct 2002 323 | * vallado - add constant file use 29 jun 2003 324 | * vallado - add mu 2 apr 2007 325 | * 326 | * inputs description range / units 327 | * r - ijk position vector km 328 | * v - ijk velocity vector km / s 329 | * mu - gravitational parameter km3 / s2 330 | * 331 | * outputs : 332 | * p - semilatus rectum km 333 | * a - semimajor axis km 334 | * ecc - eccentricity 335 | * incl - inclination 0.0 to pi rad 336 | * omega - longitude of ascending node 0.0 to 2pi rad 337 | * argp - argument of perigee 0.0 to 2pi rad 338 | * nu - true anomaly 0.0 to 2pi rad 339 | * m - mean anomaly 0.0 to 2pi rad 340 | * arglat - argument of latitude (ci) 0.0 to 2pi rad 341 | * truelon - true longitude (ce) 0.0 to 2pi rad 342 | * lonper - longitude of periapsis (ee) 0.0 to 2pi rad 343 | * 344 | * locals : 345 | * hbar - angular momentum h vector km2 / s 346 | * ebar - eccentricity e vector 347 | * nbar - line of nodes n vector 348 | * c1 - v**2 - u/r 349 | * rdotv - r dot v 350 | * hk - hk unit vector 351 | * sme - specfic mechanical energy km2 / s2 352 | * i - index 353 | * e - eccentric, parabolic, 354 | * hyperbolic anomaly rad 355 | * temp - temporary variable 356 | * typeorbit - type of orbit ee, ei, ce, ci 357 | * 358 | * coupling : 359 | * mag - magnitude of a vector 360 | * cross - cross product of two vectors 361 | * angle - find the angle between two vectors 362 | * newtonnu - find the mean anomaly 363 | * 364 | * references : 365 | * vallado 2007, 126, alg 9, ex 2-5 366 | * --------------------------------------------------------------------------- */ 367 | 368 | void rv2coe 369 | ( 370 | double r[3], double v[3], double mu, 371 | double& p, double& a, double& ecc, double& incl, double& omega, double& argp, 372 | double& nu, double& m, double& arglat, double& truelon, double& lonper 373 | ) 374 | { 375 | double undefined, small, hbar[3], nbar[3], magr, magv, magn, ebar[3], sme, 376 | rdotv, infinite, temp, c1, hk, twopi, magh, halfpi, e; 377 | 378 | int i; 379 | char typeorbit[3]; 380 | 381 | twopi = 2.0 * pi; 382 | halfpi = 0.5 * pi; 383 | small = 0.00000001; 384 | undefined = 999999.1; 385 | infinite = 999999.9; 386 | 387 | // ------------------------- implementation ----------------- 388 | magr = mag( r ); 389 | magv = mag( v ); 390 | 391 | // ------------------ find h n and e vectors ---------------- 392 | cross( r,v, hbar ); 393 | magh = mag( hbar ); 394 | if ( magh > small ) 395 | { 396 | nbar[0]= -hbar[1]; 397 | nbar[1]= hbar[0]; 398 | nbar[2]= 0.0; 399 | magn = mag( nbar ); 400 | c1 = magv*magv - mu /magr; 401 | rdotv = dot( r,v ); 402 | for (i= 0; i <= 2; i++) 403 | ebar[i]= (c1*r[i] - rdotv*v[i])/mu; 404 | ecc = mag( ebar ); 405 | 406 | // ------------ find a e and semi-latus rectum ---------- 407 | sme= ( magv*magv*0.5 ) - ( mu /magr ); 408 | if ( fabs( sme ) > small ) 409 | a= -mu / (2.0 *sme); 410 | else 411 | a= infinite; 412 | p = magh*magh/mu; 413 | 414 | // ----------------- find inclination ------------------- 415 | hk= hbar[2]/magh; 416 | incl= acos( hk ); 417 | 418 | // -------- determine type of orbit for later use -------- 419 | // ------ elliptical, parabolic, hyperbolic inclined ------- 420 | strcpy(typeorbit,"ei"); 421 | if ( ecc < small ) 422 | { 423 | // ---------------- circular equatorial --------------- 424 | if ((incl small ) 439 | { 440 | temp= nbar[0] / magn; 441 | if ( fabs(temp) > 1.0 ) 442 | temp= sgn(temp); 443 | omega= acos( temp ); 444 | if ( nbar[1] < 0.0 ) 445 | omega= twopi - omega; 446 | } 447 | else 448 | omega= undefined; 449 | 450 | // ---------------- find argument of perigee --------------- 451 | if ( strcmp(typeorbit,"ei") == 0 ) 452 | { 453 | argp = angle( nbar,ebar); 454 | if ( ebar[2] < 0.0 ) 455 | argp= twopi - argp; 456 | } 457 | else 458 | argp= undefined; 459 | 460 | // ------------ find true anomaly at epoch ------------- 461 | if ( typeorbit[0] == 'e' ) 462 | { 463 | nu = angle( ebar,r); 464 | if ( rdotv < 0.0 ) 465 | nu= twopi - nu; 466 | } 467 | else 468 | nu= undefined; 469 | 470 | // ---- find argument of latitude - circular inclined ----- 471 | if ( strcmp(typeorbit,"ci") == 0 ) 472 | { 473 | arglat = angle( nbar,r ); 474 | if ( r[2] < 0.0 ) 475 | arglat= twopi - arglat; 476 | m = arglat; 477 | } 478 | else 479 | arglat= undefined; 480 | 481 | // -- find longitude of perigee - elliptical equatorial ---- 482 | if (( ecc>small ) && (strcmp(typeorbit,"ee") == 0)) 483 | { 484 | temp= ebar[0]/ecc; 485 | if ( fabs(temp) > 1.0 ) 486 | temp= sgn(temp); 487 | lonper= acos( temp ); 488 | if ( ebar[1] < 0.0 ) 489 | lonper= twopi - lonper; 490 | if ( incl > halfpi ) 491 | lonper= twopi - lonper; 492 | } 493 | else 494 | lonper= undefined; 495 | 496 | // -------- find true longitude - circular equatorial ------ 497 | if (( magr>small ) && ( strcmp(typeorbit,"ce") == 0 )) 498 | { 499 | temp= r[0]/magr; 500 | if ( fabs(temp) > 1.0 ) 501 | temp= sgn(temp); 502 | truelon= acos( temp ); 503 | if ( r[1] < 0.0 ) 504 | truelon= twopi - truelon; 505 | if ( incl > halfpi ) 506 | truelon= twopi - truelon; 507 | m = truelon; 508 | } 509 | else 510 | truelon= undefined; 511 | 512 | // ------------ find mean anomaly for all orbits ----------- 513 | if ( typeorbit[0] == 'e' ) 514 | newtonnu(ecc,nu, e, m); 515 | } 516 | else 517 | { 518 | p = undefined; 519 | a = undefined; 520 | ecc = undefined; 521 | incl = undefined; 522 | omega= undefined; 523 | argp = undefined; 524 | nu = undefined; 525 | m = undefined; 526 | arglat = undefined; 527 | truelon= undefined; 528 | lonper = undefined; 529 | } 530 | } // end rv2coe 531 | 532 | /* ----------------------------------------------------------------------------- 533 | * 534 | * procedure jday 535 | * 536 | * this procedure finds the julian date given the year, month, day, and time. 537 | * the julian date is defined by each elapsed day since noon, jan 1, 4713 bc. 538 | * 539 | * algorithm : calculate the answer in one step for efficiency 540 | * 541 | * author : david vallado 719-573-2600 1 mar 2001 542 | * changed by : bram gurdebeke 543 | * 544 | * inputs description range / units 545 | * year - year 1900 .. 2100 546 | * mon - month 1 .. 12 547 | * day - day 1 .. 28,29,30,31 548 | * hr - time hour 0 .. 23 549 | * min - time min 0 .. 59 550 | * sec - time sec 0.0 .. 59.999 551 | * timezone - hours from utc -11 .. 14 552 | * daylighsaving true/false 553 | * 554 | * outputs : 555 | * jd - julian date days from 4713 bc 556 | * 557 | * locals : 558 | * none. 559 | * 560 | * coupling : 561 | * none. 562 | * 563 | * references : 564 | * vallado 2007, 189, alg 14, ex 3-14 565 | * 566 | * --------------------------------------------------------------------------- */ 567 | 568 | void jday 569 | ( 570 | int year, int mon, int day, int hr, int minute, double sec, 571 | int timezone, bool daylightsaving, double& jd 572 | ) 573 | { 574 | jd = 367.0 * year - 575 | floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) + 576 | floor( 275 * mon / 9.0 ) + 577 | day + 1721013.5 + 578 | ((sec / 60.0 + minute) / 60.0 + hr - timezone - (daylightsaving && summertime(year,mon,day,hr,timezone))) / 24.0; // ut in days 579 | // - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5; 580 | } // end jday 581 | 582 | 583 | /* ----------------------------------------------------------------------------- 584 | * 585 | * procedure days2mdhms 586 | * 587 | * this procedure converts the day of the year, days, to the equivalent month 588 | * day, hour, minute and second. 589 | * 590 | * algorithm : set up array for the number of days per month 591 | * find leap year - use 1900 because 2000 is a leap year 592 | * loop through a temp value while the value is < the days 593 | * perform int conversions to the correct day and month 594 | * convert remainder into h m s using type conversions 595 | * 596 | * author : david vallado 719-573-2600 1 mar 2001 597 | * 598 | * inputs description range / units 599 | * year - year 1900 .. 2100 600 | * days - julian day of the year 0.0 .. 366.0 601 | * 602 | * outputs : 603 | * mon - month 1 .. 12 604 | * day - day 1 .. 28,29,30,31 605 | * hr - hour 0 .. 23 606 | * min - minute 0 .. 59 607 | * sec - second 0.0 .. 59.999 608 | * 609 | * locals : 610 | * dayofyr - day of year 611 | * temp - temporary extended values 612 | * inttemp - temporary int value 613 | * i - index 614 | * lmonth[12] - int array containing the number of days per month 615 | * 616 | * coupling : 617 | * none. 618 | * --------------------------------------------------------------------------- */ 619 | 620 | void days2mdhms 621 | ( 622 | int year, double days, 623 | int& mon, int& day, int& hr, int& minute, double& sec 624 | ) 625 | { 626 | int i, inttemp, dayofyr; 627 | double temp; 628 | int lmonth[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; 629 | 630 | dayofyr = (int)floor(days); 631 | /* ----------------- find month and day of month ---------------- */ 632 | if ( (year % 4) == 0 ) 633 | lmonth[1] = 29; 634 | 635 | i = 1; 636 | inttemp = 0; 637 | while ((dayofyr > inttemp + lmonth[i-1]) && (i < 12)) 638 | { 639 | inttemp = inttemp + lmonth[i-1]; 640 | i++; 641 | } 642 | mon = i; 643 | day = dayofyr - inttemp; 644 | 645 | /* ----------------- find hours minutes and seconds ------------- */ 646 | temp = (days - dayofyr) * 24.0; 647 | hr = (int)floor(temp); 648 | temp = (temp - hr) * 60.0; 649 | minute = (int)floor(temp); 650 | sec = (temp - minute) * 60.0; 651 | } // end days2mdhms 652 | 653 | /* ----------------------------------------------------------------------------- 654 | * 655 | * procedure invjday 656 | * 657 | * this procedure finds the year, month, day, hour, minute and second 658 | * given the julian date. tu can be ut1, tdt, tdb, etc. 659 | * 660 | * algorithm : set up starting values 661 | * find leap year - use 1900 because 2000 is a leap year 662 | * find the elapsed days through the year in a loop 663 | * call routine to find each individual value 664 | * 665 | * author : david vallado 719-573-2600 1 mar 2001 666 | * 667 | * inputs description range / units 668 | * jd - julian date days from 4713 bc 669 | * 670 | * outputs : 671 | * year - year 1900 .. 2100 672 | * mon - month 1 .. 12 673 | * day - day 1 .. 28,29,30,31 674 | * hr - hour 0 .. 23 675 | * min - minute 0 .. 59 676 | * sec - second 0.0 .. 59.999 677 | * 678 | * locals : 679 | * days - day of year plus fractional 680 | * portion of a day days 681 | * tu - julian centuries from 0 h 682 | * jan 0, 1900 683 | * temp - temporary double values 684 | * leapyrs - number of leap years from 1900 685 | * 686 | * coupling : 687 | * days2mdhms - finds month, day, hour, minute and second given days and year 688 | * 689 | * references : 690 | * vallado 2007, 208, alg 22, ex 3-13 691 | * --------------------------------------------------------------------------- */ 692 | 693 | void invjday 694 | ( 695 | double jd, int timezone, bool daylightsaving, 696 | int& year, int& mon, int& day, 697 | int& hr, int& minute, double& sec 698 | ) 699 | { 700 | int leapyrs; 701 | double days, tu, temp; 702 | 703 | jd += (double)timezone / 24.0; 704 | 705 | /* --------------- find year and days of the year --------------- */ 706 | temp = jd - 2415019.5; 707 | tu = temp / 365.25; 708 | year = 1900 + (int)floor(tu); 709 | leapyrs = (int)floor((year - 1901) * 0.25); 710 | 711 | // optional nudge by 8.64x10-7 sec to get even outputs 712 | days = temp - ((year - 1900) * 365.0 + leapyrs) + 0.00000000001; 713 | 714 | /* ------------ check for case of beginning of a year ----------- */ 715 | if (days < 1.0) 716 | { 717 | year = year - 1; 718 | leapyrs = (int)floor((year - 1901) * 0.25); 719 | days = temp - ((year - 1900) * 365.0 + leapyrs); 720 | } 721 | 722 | /* ----------------- find remaing data ------------------------- */ 723 | days2mdhms(year, days, mon, day, hr, minute, sec); 724 | if (daylightsaving && summertime(year, mon, day, hr, timezone)) { 725 | days2mdhms(year, days + 1.0/24.0, mon, day, hr, minute, sec); 726 | } 727 | sec = sec - 0.00000086400; 728 | } // end invjday 729 | 730 | float floatmod(float a, float b) 731 | { 732 | return (a - b * floor(a / b)); 733 | } 734 | 735 | double floatmod(double a, double b) 736 | { 737 | return (a - b * floor(a / b)); 738 | } 739 | 740 | bool summertime(int year, int month, int day, int hour, int tzHours) 741 | // input parameters: "normal time" for year, month, day, hour and tzHours (0=UTC, 1=MEZ) 742 | { 743 | if (month<3 || month>10) return false; // keine Sommerzeit in Jan, Feb, Nov, Dez 744 | if (month>3 && month<10) return true; // Sommerzeit in Apr, Mai, Jun, Jul, Aug, Sep 745 | if (((month == 3) && ((hour + 24 * day) >= (1 + tzHours + 24 * (31 - (5 * year / 4 + 4) % 7)))) 746 | || ((month == 10) && ((hour + 24 * day) < (1 + tzHours + 24 * (31 - (5 * year / 4 + 1) % 7))))) 747 | return true; 748 | else 749 | return false; 750 | } 751 | -------------------------------------------------------------------------------- /src/sgp4ext.h: -------------------------------------------------------------------------------- 1 | #ifndef _sgp4ext_ 2 | #define _sgp4ext_ 3 | /* ---------------------------------------------------------------- 4 | * 5 | * sgp4ext.h 6 | * 7 | * this file contains extra routines needed for the main test program for sgp4. 8 | * these routines are derived from the astro libraries. 9 | * 10 | * companion code for 11 | * fundamentals of astrodynamics and applications 12 | * 2007 13 | * by david vallado 14 | * 15 | * (w) 719-573-2600, email dvallado@agi.com 16 | * 17 | * current : 18 | * 20 apr 07 david vallado 19 | * misc documentation updates 20 | * changes : 21 | * 14 aug 06 david vallado 22 | * original baseline 23 | * ---------------------------------------------------------------- */ 24 | 25 | #include 26 | #include 27 | 28 | #include "sgp4unit.h" 29 | 30 | 31 | // ------------------------- function declarations ------------------------- 32 | 33 | double sgn 34 | ( 35 | double x 36 | ); 37 | 38 | double mag 39 | ( 40 | double x[3] 41 | ); 42 | 43 | void cross 44 | ( 45 | double vec1[3], double vec2[3], double outvec[3] 46 | ); 47 | 48 | double dot 49 | ( 50 | double x[3], double y[3] 51 | ); 52 | 53 | double angle 54 | ( 55 | double vec1[3], 56 | double vec2[3] 57 | ); 58 | 59 | void newtonnu 60 | ( 61 | double ecc, double nu, 62 | double& e0, double& m 63 | ); 64 | 65 | double asinh 66 | ( 67 | double xval 68 | ); 69 | 70 | void rv2coe 71 | ( 72 | double r[3], double v[3], double mu, 73 | double& p, double& a, double& ecc, double& incl, double& omega, double& argp, 74 | double& nu, double& m, double& arglat, double& truelon, double& lonper 75 | ); 76 | 77 | void jday 78 | ( 79 | int year, int mon, int day, int hr, int minute, double sec, 80 | int timezone, bool daylightsaving, double& jd 81 | ); 82 | 83 | void days2mdhms 84 | ( 85 | int year, double days, 86 | int& mon, int& day, int& hr, int& minute, double& sec 87 | ); 88 | 89 | void invjday 90 | ( 91 | double jd, int timezone, bool daylightsaving, 92 | int& year, int& mon, int& day, 93 | int& hr, int& minute, double& sec 94 | ); 95 | 96 | float floatmod(float a, float b); 97 | 98 | double floatmod(double a, double b); 99 | 100 | bool summertime(int year, int month, int day, int hour, int tzHours); 101 | 102 | #endif 103 | 104 | -------------------------------------------------------------------------------- /src/sgp4io.cpp: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------- 2 | * 3 | * sgp4io.cpp 4 | * 5 | * this file contains a function to read two line element sets. while 6 | * not formerly part of the sgp4 mathematical theory, it is 7 | * required for practical implemenation. 8 | * 9 | * companion code for 10 | * fundamentals of astrodynamics and applications 11 | * 2007 12 | * by david vallado 13 | * 14 | * (w) 719-573-2600, email dvallado@agi.com 15 | * 16 | * current : 17 | * 27 Aug 10 david vallado 18 | * fix input format and delete unused variables in twoline2rv 19 | * changes : 20 | * 3 sep 08 david vallado 21 | * add operationmode for afspc (a) or improved (i) 22 | * 9 may 07 david vallado 23 | * fix year correction to 57 24 | * 27 mar 07 david vallado 25 | * misc fixes to manual inputs 26 | * 14 aug 06 david vallado 27 | * original baseline 28 | * ---------------------------------------------------------------- */ 29 | 30 | #include "sgp4io.h" 31 | #include "stdlib.h" 32 | 33 | /* ----------------------------------------------------------------------------- 34 | * 35 | * function twoline2rv 36 | * 37 | * this function converts the two line element set character string data to 38 | * variables and initializes the sgp4 variables. several intermediate varaibles 39 | * and quantities are determined. note that the result is a structure so multiple 40 | * satellites can be processed simultaneously without having to reinitialize. the 41 | * verification mode is an important option that permits quick checks of any 42 | * changes to the underlying technical theory. this option works using a 43 | * modified tle file in which the start, stop, and delta time values are 44 | * included at the end of the second line of data. this only works with the 45 | * verification mode. the catalog mode simply propagates from -1440 to 1440 min 46 | * from epoch and is useful when performing entire catalog runs. 47 | * 48 | * author : david vallado 719-573-2600 1 mar 2001 49 | * 50 | * inputs : 51 | * longstr1 - first line of the tle 52 | * longstr2 - second line of the tle 53 | * typerun - type of run verification 'v', catalog 'c', 54 | * manual 'm' 55 | * typeinput - type of manual input mfe 'm', epoch 'e', dayofyr 'd' 56 | * opsmode - mode of operation afspc or improved 'a', 'i' 57 | * whichconst - which set of constants to use 72, 84 58 | * 59 | * outputs : 60 | * satrec - structure containing all the sgp4 satellite information 61 | * 62 | * coupling : 63 | * getgravconst- 64 | * days2mdhms - conversion of days to month, day, hour, minute, second 65 | * jday - convert day month year hour minute second into julian date 66 | * sgp4init - initialize the sgp4 variables 67 | * 68 | * references : 69 | * norad spacetrack report #3 70 | * vallado, crawford, hujsak, kelso 2006 71 | --------------------------------------------------------------------------- */ 72 | 73 | void twoline2rv 74 | ( 75 | char longstr1[130], char longstr2[130], 76 | char opsmode, gravconsttype whichconst, 77 | elsetrec& satrec 78 | ) 79 | { 80 | char tempstr[13]; 81 | const double deg2rad = pi / 180.0; // 0.0174532925199433 82 | const double xpdotp = 1440.0 / (2.0 *pi); // 229.1831180523293 83 | 84 | double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2; 85 | double startsec, stopsec, startdayofyr, stopdayofyr, jdstart, jdstop; 86 | int startyear, stopyear, startmon, stopmon, startday, stopday, 87 | starthr, stophr, startmin, stopmin; 88 | int cardnumb, numb, j; 89 | long revnum = 0, elnum = 0; 90 | char classification, intldesg[11]; 91 | int year = 0; 92 | int mon, day, hr, minute, nexp, ibexp; 93 | 94 | getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 95 | 96 | satrec.error = 0; 97 | 98 | // set the implied decimal points since doing a formated read 99 | // fixes for bad input data values (missing, ...) 100 | for (j = 10; j <= 15; j++) 101 | if (longstr1[j] == ' ') 102 | longstr1[j] = '_'; 103 | 104 | if (longstr1[44] != ' ') 105 | longstr1[43] = longstr1[44]; 106 | longstr1[44] = '.'; 107 | if (longstr1[7] == ' ') 108 | longstr1[7] = 'U'; 109 | if (longstr1[9] == ' ') 110 | longstr1[9] = '.'; 111 | for (j = 45; j <= 49; j++) 112 | if (longstr1[j] == ' ') 113 | longstr1[j] = '0'; 114 | if (longstr1[51] == ' ') 115 | longstr1[51] = '0'; 116 | if (longstr1[53] != ' ') 117 | longstr1[52] = longstr1[53]; 118 | longstr1[53] = '.'; 119 | longstr2[25] = '.'; 120 | for (j = 26; j <= 32; j++) 121 | if (longstr2[j] == ' ') 122 | longstr2[j] = '0'; 123 | if (longstr1[62] == ' ') 124 | longstr1[62] = '0'; 125 | if (longstr1[68] == ' ') 126 | longstr1[68] = '0'; 127 | 128 | /** 129 | sscanf(longstr1,"%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ", 130 | &cardnumb,&satrec.satnum,&classification, intldesg, &satrec.epochyr, 131 | &satrec.epochdays,&satrec.ndot, &satrec.nddot, &nexp, &satrec.bstar, 132 | &ibexp, &numb, &elnum ); 133 | **/ 134 | 135 | //memcpy( tempstr, &longstr1[0] , 2); tempstr[1] = '\0'; satrec.cardnumb = atoi(tempstr); 136 | memcpy( tempstr, &longstr1[2] , 5); tempstr[5] = '\0'; satrec.satnum = atol(tempstr); 137 | //classification = longstr1[7]; 138 | //memcpy( intldesg, &longstr1[8] , 10); intldesg[10] = '\0'; 139 | memcpy( tempstr, &longstr1[18] , 2); tempstr[2] = '\0'; satrec.epochyr = atoi(tempstr); 140 | memcpy( tempstr, &longstr1[20] , 12); tempstr[12] = '\0'; satrec.epochdays = atof(tempstr); 141 | memcpy( tempstr, &longstr1[32] , 11); tempstr[11] = '\0'; satrec.ndot = atof(tempstr); 142 | memcpy( tempstr, &longstr1[43] , 7); tempstr[7] = '\0'; satrec.nddot = atof(tempstr); 143 | memcpy( tempstr, &longstr1[50] , 2); tempstr[2] = '\0'; nexp = atoi(tempstr); 144 | memcpy( tempstr, &longstr1[52] , 7); tempstr[7] = '\0'; satrec.bstar = atof(tempstr); 145 | memcpy( tempstr, &longstr1[59] , 2); tempstr[2] = '\0'; ibexp = atoi(tempstr); 146 | //memcpy( tempstr, &longstr1[61] , 2); tempstr[2] = '\0'; numb = atoi(tempstr); 147 | //memcpy( tempstr, &longstr1[63] , 6); tempstr[6] = '\0'; elnum = atol(tempstr); 148 | 149 | memcpy( tempstr, &longstr2[2] , 5); tempstr[5] = '\0'; satrec.satnum = atol(tempstr); 150 | memcpy( tempstr, &longstr2[7] , 9); tempstr[9] = '\0'; satrec.inclo = atof(tempstr); 151 | memcpy( tempstr, &longstr2[16] , 9); tempstr[9] = '\0'; satrec.nodeo = atof(tempstr); 152 | memcpy( tempstr, &longstr2[25] , 8); tempstr[8] = '\0'; satrec.ecco = atof(tempstr); 153 | memcpy( tempstr, &longstr2[33] , 9); tempstr[9] = '\0'; satrec.argpo = atof(tempstr); 154 | memcpy( tempstr, &longstr2[42] , 9); tempstr[9] = '\0'; satrec.mo = atof(tempstr); 155 | memcpy( tempstr, &longstr2[51] , 11); tempstr[10] = '\0'; satrec.no = atof(tempstr); 156 | //memcpy( tempstr, &longstr2[63] , 6); tempstr[6] = '\0'; revnum = atol(tempstr); 157 | 158 | 159 | 160 | // ---- find no, ndot, nddot ---- 161 | satrec.no = satrec.no / xpdotp; //* rad/min 162 | satrec.nddot= satrec.nddot * pow(10.0, nexp); 163 | satrec.bstar= satrec.bstar * pow(10.0, ibexp); 164 | 165 | // ---- convert to sgp4 units ---- 166 | satrec.a = pow( satrec.no*tumin , (-2.0/3.0) ); 167 | satrec.ndot = satrec.ndot / (xpdotp*1440.0); //* ? * minperday 168 | satrec.nddot= satrec.nddot / (xpdotp*1440.0*1440); 169 | 170 | // ---- find standard orbital elements ---- 171 | satrec.inclo = satrec.inclo * deg2rad; 172 | satrec.nodeo = satrec.nodeo * deg2rad; 173 | satrec.argpo = satrec.argpo * deg2rad; 174 | satrec.mo = satrec.mo * deg2rad; 175 | 176 | satrec.alta = satrec.a*(1.0 + satrec.ecco) - 1.0; 177 | satrec.altp = satrec.a*(1.0 - satrec.ecco) - 1.0; 178 | 179 | // ---------------------------------------------------------------- 180 | // find sgp4epoch time of element set 181 | // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch) 182 | // and minutes from the epoch (time) 183 | // ---------------------------------------------------------------- 184 | 185 | // ---------------- temp fix for years from 1957-2056 ------------------- 186 | // --------- correct fix will occur when year is 4-digit in tle --------- 187 | if (satrec.epochyr < 57) 188 | year= satrec.epochyr + 2000; 189 | else 190 | year= satrec.epochyr + 1900; 191 | 192 | days2mdhms ( year,satrec.epochdays, mon,day,hr,minute,sec ); 193 | jday( year,mon,day,hr,minute,sec, 0,false,satrec.jdsatepoch ); 194 | 195 | /*** 196 | // ---- input start stop times manually 197 | if ((typerun != 'v') && (typerun != 'c')) 198 | { 199 | // ------------- enter start/stop ymd hms values -------------------- 200 | if (typeinput == 'e') 201 | { 202 | printf("input start prop year mon day hr min sec \n"); 203 | // make sure there is no space at the end of the format specifiers in scanf! 204 | scanf( "%i %i %i %i %i %lf",&startyear, &startmon, &startday, &starthr, &startmin, &startsec); 205 | fflush(stdin); 206 | jday( startyear,startmon,startday,starthr,startmin,startsec, jdstart ); 207 | 208 | printf("input stop prop year mon day hr min sec \n"); 209 | scanf( "%i %i %i %i %i %lf",&stopyear, &stopmon, &stopday, &stophr, &stopmin, &stopsec); 210 | fflush(stdin); 211 | jday( stopyear,stopmon,stopday,stophr,stopmin,stopsec, jdstop ); 212 | 213 | startmfe = (jdstart - satrec.jdsatepoch) * 1440.0; 214 | stopmfe = (jdstop - satrec.jdsatepoch) * 1440.0; 215 | 216 | printf("input time step in minutes \n"); 217 | scanf( "%lf",&deltamin ); 218 | } 219 | // -------- enter start/stop year and days of year values ----------- 220 | if (typeinput == 'd') 221 | { 222 | printf("input start year dayofyr \n"); 223 | scanf( "%i %lf",&startyear, &startdayofyr ); 224 | printf("input stop year dayofyr \n"); 225 | scanf( "%i %lf",&stopyear, &stopdayofyr ); 226 | 227 | days2mdhms ( startyear,startdayofyr, mon,day,hr,minute,sec ); 228 | jday( startyear,mon,day,hr,minute,sec, jdstart ); 229 | days2mdhms ( stopyear,stopdayofyr, mon,day,hr,minute,sec ); 230 | jday( stopyear,mon,day,hr,minute,sec, jdstop ); 231 | 232 | startmfe = (jdstart - satrec.jdsatepoch) * 1440.0; 233 | stopmfe = (jdstop - satrec.jdsatepoch) * 1440.0; 234 | 235 | printf("input time step in minutes \n"); 236 | scanf( "%lf",&deltamin ); 237 | } 238 | // ------------------ enter start/stop mfe values ------------------- 239 | if (typeinput == 'm') 240 | { 241 | printf("input start min from epoch \n"); 242 | scanf( "%lf",&startmfe ); 243 | printf("input stop min from epoch \n"); 244 | scanf( "%lf",&stopmfe ); 245 | printf("input time step in minutes \n"); 246 | scanf( "%lf",&deltamin ); 247 | } 248 | } 249 | ***/ 250 | // ---------------- initialize the orbit at sgp4epoch ------------------- 251 | sgp4init( whichconst, opsmode, satrec.satnum, satrec.jdsatepoch-2433281.5, satrec.bstar, 252 | satrec.ecco, satrec.argpo, satrec.inclo, satrec.mo, satrec.no, 253 | satrec.nodeo, satrec); 254 | } // end twoline2rv 255 | 256 | /* ----------------------------------------------------------------------------- 257 | * 258 | * function twolineChecksum 259 | * 260 | * this function checks if the checksum on the end of the line is correct. 261 | * 262 | * author : Hopperpop 6 mar 2016 263 | * 264 | * inputs : 265 | * longstr - line of the tle 266 | * 267 | * outputs : 268 | * bool - true if test was succesfull 269 | * 270 | * references : 271 | * https://en.wikipedia.org/wiki/Two-line_element_set 272 | --------------------------------------------------------------------------- */ 273 | bool twolineChecksum 274 | ( 275 | const char longstr[80] 276 | ) 277 | { 278 | 279 | unsigned int cks = 0; 280 | const char* checksumpointer = longstr + 68; 281 | 282 | for (const char* p = longstr; p < checksumpointer; p++){ 283 | 284 | switch(*p){ 285 | case 0: 286 | return false; 287 | break; 288 | case '-': 289 | cks++; 290 | break; 291 | case '1': 292 | case '2': 293 | case '3': 294 | case '4': 295 | case '5': 296 | case '6': 297 | case '7': 298 | case '8': 299 | case '9': 300 | cks += *p - '0'; 301 | break; 302 | } 303 | 304 | } 305 | cks %= 10; //modulo 10 306 | if (cks + '0' == *checksumpointer) 307 | return true; 308 | 309 | return false; 310 | } 311 | -------------------------------------------------------------------------------- /src/sgp4io.h: -------------------------------------------------------------------------------- 1 | #ifndef _sgp4io_ 2 | #define _sgp4io_ 3 | /* ---------------------------------------------------------------- 4 | * 5 | * sgp4io.h; 6 | * 7 | * this file contains a function to read two line element sets. while 8 | * not formerly part of the sgp4 mathematical theory, it is 9 | * required for practical implemenation. 10 | * 11 | * companion code for 12 | * fundamentals of astrodynamics and applications 13 | * 2007 14 | * by david vallado 15 | * 16 | * (w) 719-573-2600, email dvallado@agi.com 17 | * 18 | * current : 19 | * 3 sep 07 david vallado 20 | * add operationmode for afspc (a) or improved (i) 21 | * changes : 22 | * 20 apr 07 david vallado 23 | * misc updates for manual operation 24 | * 14 aug 06 david vallado 25 | * original baseline 26 | * ---------------------------------------------------------------- */ 27 | 28 | #include 29 | #include 30 | 31 | #include "sgp4ext.h" // for several misc routines 32 | #include "sgp4unit.h" // for sgp4init and getgravconst 33 | 34 | // ------------------------- function declarations ------------------------- 35 | 36 | void twoline2rv 37 | ( 38 | char longstr1[130], char longstr2[130], 39 | char opsmode, gravconsttype whichconst, 40 | elsetrec& satrec 41 | ); 42 | 43 | bool twolineChecksum 44 | ( 45 | const char longstr[] 46 | ); 47 | #endif 48 | -------------------------------------------------------------------------------- /src/sgp4pred.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file contains miscellaneous functions required for predicting satellite overpasses and the main class for easy acces to all the functions. 3 | Most functions are based on the columns written by Dr. T.S. Kelso on the celestrak website (https://celestrak.com/columns/) 4 | 5 | Written by Hopperpop 6 | */ 7 | 8 | #include 9 | #include "sgp4ext.h" 10 | #include "sgp4unit.h" 11 | #include "sgp4io.h" 12 | #include "sgp4coord.h" 13 | #include "brent.h" 14 | #include "sgp4pred.h" 15 | #include "visible.h" 16 | #include 17 | 18 | #define MAX_itter 30 19 | #define tol 0.000005 //tol = +-0,432 sec 20 | 21 | 22 | ///////////Classs/////////// 23 | 24 | Sgp4::Sgp4(){ 25 | opsmode = 'i'; //improved mode 26 | whichconst = wgs84; //newest constants 27 | sunoffset = -0.10471975511966; //sun aboven -6° => not dark enough 28 | offset = 0.0; 29 | } 30 | 31 | ///Init functions///// 32 | bool Sgp4::init(const char naam[24], char longstr1[130], char longstr2[130]){ 33 | 34 | if (strcmp(longstr1, line1) == 0) { 35 | return false; 36 | } 37 | 38 | strlcpy(satName, naam, sizeof(satName)); 39 | strlcpy(line1, longstr1, sizeof(line1)); 40 | strlcpy(line2, longstr2, sizeof(line2)); 41 | 42 | twoline2rv(longstr1, longstr2, opsmode, whichconst, satrec ); 43 | 44 | revpday = 1440.0 / (2.0 * pi) * satrec.no; 45 | return true; 46 | } 47 | 48 | 49 | //set site coordinates 50 | void Sgp4::site(double lat, double lon, double alt){ 51 | siteLat = lat; 52 | siteLon = lon; 53 | siteAlt = alt / 1000; //meters to kilometers 54 | siteLatRad = siteLat * pi / 180.0; 55 | siteLonRad = siteLon * pi / 180.0; 56 | } 57 | 58 | ///set sunoffset 59 | void Sgp4::setsunrise(double degrees){ 60 | sunoffset = degrees * pi / 180.0; 61 | } 62 | 63 | ////Location functions///// 64 | void Sgp4::findsat(double jdI){ 65 | 66 | double latlongh[3]; 67 | double recef[3]; 68 | double vecef[3]; 69 | double razelrates[3]; 70 | 71 | jdC = jdI; 72 | 73 | double tsince = (jdC - satrec.jdsatepoch) * 24.0 * 60.0; 74 | sgp4(whichconst, satrec, tsince, ro, vo); 75 | rv2azel(ro, siteLatRad, siteLonRad, siteAlt, jdC, razel); 76 | teme2ecef(ro, jdC, recef); 77 | ijk2ll(recef, latlongh); 78 | 79 | satLat = latlongh[0]*180/pi; //Latidude sattelite (degrees) 80 | satLon = latlongh[1]*180/pi; //longitude sattelite (degrees) 81 | satAlt = latlongh[2]; //Altitude sattelite (degrees) 82 | satAz = floatmod( razel[1]*180/pi+360.0, 360.0); //Azemith sattelite (degrees) 83 | satEl = razel[2]*180/pi; //elevation sattelite (degrees) 84 | satDist = razel[0]; //Distance to sattelite (km) 85 | satJd = jdI; //time (julian day) 86 | 87 | satVis = visible(); 88 | if (satEl < 0.0) { 89 | satVis = -2; //under horizon 90 | } 91 | 92 | } 93 | 94 | void Sgp4::findsat(unsigned long unix){ 95 | findsat(getJulianFromUnix(unix)); 96 | } 97 | 98 | 99 | //////Predict functions///////// 100 | 101 | //returns the elevation for a given julian date 102 | double Sgp4::sgp4wrap( double jdCe){ 103 | 104 | double tsince = (jdCe - satrec.jdsatepoch) * 24.0 * 60.0; 105 | 106 | sgp4(whichconst, satrec, tsince, ro, vo); 107 | rv2azel(ro, siteLatRad, siteLonRad, siteAlt, jdCe, razel); 108 | return -razel[2]+offset; 109 | 110 | } 111 | 112 | 113 | // returns next overpass maximum, starting from a maximum called startpoint 114 | bool Sgp4::nextpass(passinfo* passdata, int itterations) { 115 | return (Sgp4::nextpass( passdata, itterations, false, 0.0)); 116 | } 117 | 118 | // returns next overpass. Set direc to false for forward search, true for backwards search 119 | bool Sgp4::nextpass(passinfo* passdata, int itterations, bool direc) { 120 | return (Sgp4::nextpass( passdata, itterations, direc, 0.0)); 121 | } 122 | 123 | // returns next overpass maximum, starting from a maximum called startpoint 124 | // direc = false for forward search, true for backwards search 125 | // minimumElevation is the minimum elevation above the horizon in degrees. Passes which are lower than this are rejected 126 | // returns false if all itterations are below the minimumElevation 127 | bool Sgp4::nextpass( passinfo* passdata, int itterations, bool direc, double minimumElevation){ 128 | 129 | double range,jump; 130 | int i; 131 | double max_elevation = -1.0; 132 | int16_t vissum,vis; 133 | bool isdaylight; 134 | double startphi, stopphi,phi; 135 | 136 | range = 0.25/revpday; 137 | 138 | if (direc) { ///set search direction 139 | jump = -1.0 / revpday; 140 | } 141 | else { 142 | jump = 1.0 / revpday; 143 | } 144 | 145 | for (i = 0; i < itterations && max_elevation <= (minimumElevation * pi / 180); i++){ //search for elevation above minimumElevation 146 | jdCp+= jump; 147 | max_elevation = - brentmin(jdCp - range , jdCp, jdCp + range, &Sgp4::sgp4wrap , tol, &jdCp, this); 148 | #ifdef ESP8266 149 | yield(); 150 | #endif 151 | } 152 | jdC = jdCp; 153 | if (i>=itterations) return 0; 154 | 155 | ///max elevation 156 | 157 | (*passdata).maxelevation = (max_elevation+offset)*180/pi; 158 | (*passdata).jdmax = jdC; 159 | (*passdata).azmax = floatmod(razel[1] * 180 / pi + 360.0, 360.0); 160 | vis = visible(isdaylight,phi); 161 | 162 | if (isdaylight) {(*passdata).vismax = daylight;} 163 | else if (vis < 1000){(*passdata).vismax = eclipsed;} 164 | else {(*passdata).vismax = lighted;} 165 | 166 | //start point 167 | 168 | range = 0.5/revpday; 169 | jdC = zbrent(&Sgp4::sgp4wrap, jdCp, jdCp - range, tol, this); 170 | if (jdC < 0.0) return 0; 171 | (*passdata).jdstart = jdC; 172 | (*passdata).azstart = floatmod(razel[1] * 180 / pi + 360.0, 360.0); 173 | vis = visible(isdaylight,startphi); 174 | 175 | if (isdaylight) {(*passdata).visstart = daylight;} 176 | else if (vis < 1000){(*passdata).visstart = eclipsed;} 177 | else {(*passdata).visstart = lighted;} 178 | vissum = vis; 179 | 180 | //stop point 181 | 182 | jdC = zbrent(&Sgp4::sgp4wrap, jdCp, jdCp + range, tol, this); 183 | if (jdC < 0.0) return 0; 184 | (*passdata).jdstop = jdC; 185 | (*passdata).azstop = floatmod(razel[1] * 180 / pi + 360.0, 360.0); 186 | vis = visible(isdaylight,stopphi); 187 | 188 | if (isdaylight) {(*passdata).visstop = daylight;} 189 | else if (vis < 1000){(*passdata).visstop = eclipsed;} 190 | else {(*passdata).visstop = lighted;} 191 | vissum += vis; 192 | 193 | //global visibility 194 | 195 | if ((*passdata).visstop == daylight && (*passdata).visstart == daylight) {(*passdata).sight = daylight;} 196 | else if (vissum < 1000){(*passdata).sight = eclipsed;} 197 | else {(*passdata).sight = lighted;} 198 | 199 | //transit 200 | 201 | if (sgn(startphi) == sgn(stopphi)) { 202 | (*passdata).transit = none; 203 | (*passdata).jdtransit = NAN; 204 | (*passdata).aztransit = NAN; 205 | (*passdata).transitelevation = NAN; 206 | (*passdata).vistransit = daylight; 207 | } 208 | else { 209 | if (sgn(startphi)>sgn(stopphi)) { 210 | (*passdata).transit = enter; 211 | } 212 | else { 213 | (*passdata).transit = leave; 214 | } 215 | 216 | jdC = zbrent(&Sgp4::visiblewrap, (*passdata).jdstart, (*passdata).jdstop, tol, this); 217 | if (jdC < 0.0) return 0; 218 | (*passdata).jdtransit = jdC; 219 | (*passdata).aztransit = floatmod(razel[1] * 180 / pi + 360.0, 360.0); 220 | (*passdata).transitelevation = razel[2] * 180 / pi; 221 | vis = visible(isdaylight,phi); 222 | 223 | if (isdaylight) {(*passdata).vistransit = daylight;} 224 | else {(*passdata).vistransit = eclipsed;} 225 | //else {(*passdata).visstop = lighted;} 226 | } 227 | 228 | 229 | (*passdata).minelevation = offset*180/pi; 230 | 231 | return 1; 232 | } 233 | 234 | 235 | // finds a startpoint for the prediction algorithm 236 | bool Sgp4::initpredpoint( double startpoint, double startelevation){ 237 | 238 | 239 | double a,b,c; 240 | double fa,fb,fc; 241 | double jdI; 242 | int i; 243 | 244 | offset = startelevation *pi/180.0; 245 | 246 | c = startpoint; 247 | fc = sgp4wrap( c ); 248 | b = startpoint - 0.166/revpday; 249 | fb = sgp4wrap( b ); 250 | a = startpoint - 0.322/revpday; 251 | fa = sgp4wrap( a ); 252 | 253 | for(i = 0; i < MAX_itter && ( fb > fa || fb > fc ); i++){ 254 | fc=fb; 255 | fb=fa; 256 | c=b; 257 | b=a; 258 | a = startpoint - 0.166*(i+3)/revpday; 259 | fa = sgp4wrap( a ); 260 | } 261 | if ( i >= MAX_itter-1){ 262 | return 0; 263 | } 264 | 265 | brentmin(a , b, c, &Sgp4::sgp4wrap , tol, &jdI, this); 266 | jdCp = jdI; 267 | return 1; 268 | } 269 | 270 | bool Sgp4::initpredpoint( unsigned long unix, double startelevation){ 271 | return initpredpoint( getJulianFromUnix(unix), startelevation); 272 | } 273 | 274 | double Sgp4::getpredpoint() { 275 | return jdCp; 276 | } 277 | 278 | void Sgp4::setpredpoint(double bob) { 279 | jdCp = bob; 280 | } 281 | -------------------------------------------------------------------------------- /src/sgp4pred.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file contains miscellaneous functions required for predicting satellite overpasses and the main class for easy acces to all the functions. 3 | Most functions are based on the columns written by Dr. T.S. Kelso on the celestrak website (https://celestrak.com/columns/) 4 | 5 | Written by Hopperpop 6 | */ 7 | 8 | #ifndef _sgp4pred_ 9 | #define _sgp4pred_ 10 | 11 | #include "sgp4ext.h" 12 | #include "sgp4unit.h" 13 | #include "sgp4io.h" 14 | #include "sgp4coord.h" 15 | #include 16 | 17 | enum visibletype 18 | { 19 | daylight, 20 | eclipsed, 21 | lighted 22 | }; 23 | 24 | enum shadowtransit 25 | { 26 | none, 27 | enter, 28 | leave 29 | }; 30 | 31 | struct passinfo 32 | { 33 | double jdstart; 34 | double jdstop; 35 | double jdmax; 36 | double jdtransit; 37 | 38 | double maxelevation; 39 | double minelevation; //elevation at start and end 40 | double transitelevation; 41 | 42 | double azstart; 43 | double azmax; 44 | double azstop; 45 | double aztransit; 46 | 47 | visibletype visstart; 48 | visibletype visstop; 49 | visibletype vismax; 50 | visibletype vistransit; 51 | 52 | visibletype sight; 53 | shadowtransit transit; 54 | 55 | }; 56 | 57 | 58 | 59 | class Sgp4 { 60 | char opsmode; 61 | gravconsttype whichconst; 62 | double ro[3]; 63 | double vo[3]; 64 | double razel[3]; 65 | double offset; //Min elevation for overpass prediction in radials 66 | double sunoffset; //Min elevation sun for daylight in radials 67 | double jdC; //Current used julian date 68 | double jdCp; //Current used julian date for prediction 69 | 70 | double sgp4wrap( double jdCe); //returns the elevation for a given julian date 71 | double visiblewrap(double jdCe); //returns angle between sun surface and earth surface 72 | 73 | public: 74 | char satName[25]; ///satellite name 75 | char line1[80]; //tle line 1 76 | char line2[80]; //tle line 2 77 | 78 | double revpday; ///revolutions per day 79 | elsetrec satrec; 80 | double siteLat, siteLon, siteAlt, siteLatRad, siteLonRad; 81 | double satLat, satLon, satAlt, satAz, satEl, satDist,satJd; 82 | double sunAz, sunEl; 83 | int16_t satVis; 84 | 85 | Sgp4(); 86 | bool init(const char naam[], char longstr1[130], char longstr2[130]); //initialize parameters from 2 line elements 87 | void site(double lat, double lon, double alt); //initialize site latitude[degrees],longitude[degrees],altitude[meters] 88 | void setsunrise(double degrees); //change the elevation that the sun needs to make it daylight 89 | 90 | void findsat(double jdI); //find satellite position from julian date 91 | void findsat(unsigned long); //find satellite position from unix time 92 | 93 | bool nextpass( passinfo* passdata, int itterations); // calculate next overpass data, returns true if succesfull 94 | bool nextpass(passinfo* passdata, int itterations, bool direc); //direc = false for forward search, true for backwards search 95 | bool nextpass(passinfo* passdata, int itterations, bool direc, double minimumElevation); //minimumElevation = minimum elevation above the horizon (in degrees) 96 | bool initpredpoint( double juliandate , double startelevation); //initialize prediction algorithm, starting from a juliandate and predict passes aboven startelevation 97 | bool initpredpoint( unsigned long unix, double startelevation); // from unix time 98 | 99 | int16_t visible(); //check if satellite is visible 100 | int16_t visible(bool& notdark, double& deltaphi); 101 | 102 | double getpredpoint(); 103 | void setpredpoint(double jdCe); 104 | } ; 105 | 106 | 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /src/sgp4unit.cpp: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------- 2 | * 3 | * sgp4unit.cpp 4 | * 5 | * this file contains the sgp4 procedures for analytical propagation 6 | * of a satellite. the code was originally released in the 1980 and 1986 7 | * spacetrack papers. a detailed discussion of the theory and history 8 | * may be found in the 2006 aiaa paper by vallado, crawford, hujsak, 9 | * and kelso. 10 | * 11 | * companion code for 12 | * fundamentals of astrodynamics and applications 13 | * 2007 14 | * by david vallado 15 | * 16 | * (w) 719-573-2600, email dvallado@agi.com 17 | * 18 | * current : 19 | * 30 Aug 10 david vallado 20 | * delete unused variables in initl 21 | * replace pow inetger 2, 3 with multiplies for speed 22 | * changes : 23 | * 3 Nov 08 david vallado 24 | * put returns in for error codes 25 | * 29 sep 08 david vallado 26 | * fix atime for faster operation in dspace 27 | * add operationmode for afspc (a) or improved (i) 28 | * performance mode 29 | * 16 jun 08 david vallado 30 | * update small eccentricity check 31 | * 16 nov 07 david vallado 32 | * misc fixes for better compliance 33 | * 20 apr 07 david vallado 34 | * misc fixes for constants 35 | * 11 aug 06 david vallado 36 | * chg lyddane choice back to strn3, constants, misc doc 37 | * 15 dec 05 david vallado 38 | * misc fixes 39 | * 26 jul 05 david vallado 40 | * fixes for paper 41 | * note that each fix is preceded by a 42 | * comment with "sgp4fix" and an explanation of 43 | * what was changed 44 | * 10 aug 04 david vallado 45 | * 2nd printing baseline working 46 | * 14 may 01 david vallado 47 | * 2nd edition baseline 48 | * 80 norad 49 | * original baseline 50 | * ---------------------------------------------------------------- */ 51 | 52 | #include "sgp4unit.h" 53 | #include "sgp4ext.h" 54 | 55 | const char help = 'n'; 56 | FILE *dbgfile; 57 | 58 | 59 | /* ----------- local functions - only ever used internally by sgp4 ---------- */ 60 | static void dpper 61 | ( 62 | double e3, double ee2, double peo, double pgho, double pho, 63 | double pinco, double plo, double se2, double se3, double sgh2, 64 | double sgh3, double sgh4, double sh2, double sh3, double si2, 65 | double si3, double sl2, double sl3, double sl4, double t, 66 | double xgh2, double xgh3, double xgh4, double xh2, double xh3, 67 | double xi2, double xi3, double xl2, double xl3, double xl4, 68 | double zmol, double zmos, double inclo, 69 | char init, 70 | double& ep, double& inclp, double& nodep, double& argpp, double& mp, 71 | char opsmode 72 | ); 73 | 74 | static void dscom 75 | ( 76 | double epoch, double ep, double argpp, double tc, double inclp, 77 | double nodep, double np, 78 | double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, 79 | double& cosomm,double& day, double& e3, double& ee2, double& em, 80 | double& emsq, double& gam, double& peo, double& pgho, double& pho, 81 | double& pinco, double& plo, double& rtemsq, double& se2, double& se3, 82 | double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, 83 | double& si2, double& si3, double& sl2, double& sl3, double& sl4, 84 | double& s1, double& s2, double& s3, double& s4, double& s5, 85 | double& s6, double& s7, double& ss1, double& ss2, double& ss3, 86 | double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, 87 | double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, 88 | double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, 89 | double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, 90 | double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, 91 | double& xl4, double& nm, double& z1, double& z2, double& z3, 92 | double& z11, double& z12, double& z13, double& z21, double& z22, 93 | double& z23, double& z31, double& z32, double& z33, double& zmol, 94 | double& zmos 95 | ); 96 | 97 | static void dsinit 98 | ( 99 | gravconsttype whichconst, 100 | double cosim, double emsq, double argpo, double s1, double s2, 101 | double s3, double s4, double s5, double sinim, double ss1, 102 | double ss2, double ss3, double ss4, double ss5, double sz1, 103 | double sz3, double sz11, double sz13, double sz21, double sz23, 104 | double sz31, double sz33, double t, double tc, double gsto, 105 | double mo, double mdot, double no, double nodeo, double nodedot, 106 | double xpidot, double z1, double z3, double z11, double z13, 107 | double z21, double z23, double z31, double z33, double ecco, 108 | double eccsq, double& em, double& argpm, double& inclm, double& mm, 109 | double& nm, double& nodem, 110 | int& irez, 111 | double& atime, double& d2201, double& d2211, double& d3210, double& d3222, 112 | double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, 113 | double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, 114 | double& dnodt, double& domdt, double& del1, double& del2, double& del3, 115 | double& xfact, double& xlamo, double& xli, double& xni 116 | ); 117 | 118 | static void dspace 119 | ( 120 | int irez, 121 | double d2201, double d2211, double d3210, double d3222, double d4410, 122 | double d4422, double d5220, double d5232, double d5421, double d5433, 123 | double dedt, double del1, double del2, double del3, double didt, 124 | double dmdt, double dnodt, double domdt, double argpo, double argpdot, 125 | double t, double tc, double gsto, double xfact, double xlamo, 126 | double no, 127 | double& atime, double& em, double& argpm, double& inclm, double& xli, 128 | double& mm, double& xni, double& nodem, double& dndt, double& nm 129 | ); 130 | 131 | static void initl 132 | ( 133 | int satn, gravconsttype whichconst, 134 | double ecco, double epoch, double inclo, double& no, 135 | char& method, 136 | double& ainv, double& ao, double& con41, double& con42, double& cosio, 137 | double& cosio2,double& eccsq, double& omeosq, double& posq, 138 | double& rp, double& rteosq,double& sinio , double& gsto, char opsmode 139 | ); 140 | 141 | /* ----------------------------------------------------------------------------- 142 | * 143 | * procedure dpper 144 | * 145 | * this procedure provides deep space long period periodic contributions 146 | * to the mean elements. by design, these periodics are zero at epoch. 147 | * this used to be dscom which included initialization, but it's really a 148 | * recurring function. 149 | * 150 | * author : david vallado 719-573-2600 28 jun 2005 151 | * 152 | * inputs : 153 | * e3 - 154 | * ee2 - 155 | * peo - 156 | * pgho - 157 | * pho - 158 | * pinco - 159 | * plo - 160 | * se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - 161 | * t - 162 | * xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 163 | * zmol - 164 | * zmos - 165 | * ep - eccentricity 0.0 - 1.0 166 | * inclo - inclination - needed for lyddane modification 167 | * nodep - right ascension of ascending node 168 | * argpp - argument of perigee 169 | * mp - mean anomaly 170 | * 171 | * outputs : 172 | * ep - eccentricity 0.0 - 1.0 173 | * inclp - inclination 174 | * nodep - right ascension of ascending node 175 | * argpp - argument of perigee 176 | * mp - mean anomaly 177 | * 178 | * locals : 179 | * alfdp - 180 | * betdp - 181 | * cosip , sinip , cosop , sinop , 182 | * dalf - 183 | * dbet - 184 | * dls - 185 | * f2, f3 - 186 | * pe - 187 | * pgh - 188 | * ph - 189 | * pinc - 190 | * pl - 191 | * sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , 192 | * sll , sls 193 | * xls - 194 | * xnoh - 195 | * zf - 196 | * zm - 197 | * 198 | * coupling : 199 | * none. 200 | * 201 | * references : 202 | * hoots, roehrich, norad spacetrack report #3 1980 203 | * hoots, norad spacetrack report #6 1986 204 | * hoots, schumacher and glover 2004 205 | * vallado, crawford, hujsak, kelso 2006 206 | ----------------------------------------------------------------------------*/ 207 | 208 | static void dpper 209 | ( 210 | double e3, double ee2, double peo, double pgho, double pho, 211 | double pinco, double plo, double se2, double se3, double sgh2, 212 | double sgh3, double sgh4, double sh2, double sh3, double si2, 213 | double si3, double sl2, double sl3, double sl4, double t, 214 | double xgh2, double xgh3, double xgh4, double xh2, double xh3, 215 | double xi2, double xi3, double xl2, double xl3, double xl4, 216 | double zmol, double zmos, double inclo, 217 | char init, 218 | double& ep, double& inclp, double& nodep, double& argpp, double& mp, 219 | char opsmode 220 | ) 221 | { 222 | /* --------------------- local variables ------------------------ */ 223 | const double twopi = 2.0 * pi; 224 | double alfdp, betdp, cosip, cosop, dalf, dbet, dls, 225 | f2, f3, pe, pgh, ph, pinc, pl , 226 | sel, ses, sghl, sghs, shll, shs, sil, 227 | sinip, sinop, sinzf, sis, sll, sls, xls, 228 | xnoh, zf, zm, zel, zes, znl, zns; 229 | 230 | /* ---------------------- constants ----------------------------- */ 231 | zns = 1.19459e-5; 232 | zes = 0.01675; 233 | znl = 1.5835218e-4; 234 | zel = 0.05490; 235 | 236 | /* --------------- calculate time varying periodics ----------- */ 237 | zm = zmos + zns * t; 238 | // be sure that the initial call has time set to zero 239 | if (init == 'y') 240 | zm = zmos; 241 | zf = zm + 2.0 * zes * sin(zm); 242 | sinzf = sin(zf); 243 | f2 = 0.5 * sinzf * sinzf - 0.25; 244 | f3 = -0.5 * sinzf * cos(zf); 245 | ses = se2* f2 + se3 * f3; 246 | sis = si2 * f2 + si3 * f3; 247 | sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; 248 | sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; 249 | shs = sh2 * f2 + sh3 * f3; 250 | zm = zmol + znl * t; 251 | if (init == 'y') 252 | zm = zmol; 253 | zf = zm + 2.0 * zel * sin(zm); 254 | sinzf = sin(zf); 255 | f2 = 0.5 * sinzf * sinzf - 0.25; 256 | f3 = -0.5 * sinzf * cos(zf); 257 | sel = ee2 * f2 + e3 * f3; 258 | sil = xi2 * f2 + xi3 * f3; 259 | sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; 260 | sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; 261 | shll = xh2 * f2 + xh3 * f3; 262 | pe = ses + sel; 263 | pinc = sis + sil; 264 | pl = sls + sll; 265 | pgh = sghs + sghl; 266 | ph = shs + shll; 267 | 268 | if (init == 'n') 269 | { 270 | pe = pe - peo; 271 | pinc = pinc - pinco; 272 | pl = pl - plo; 273 | pgh = pgh - pgho; 274 | ph = ph - pho; 275 | inclp = inclp + pinc; 276 | ep = ep + pe; 277 | sinip = sin(inclp); 278 | cosip = cos(inclp); 279 | 280 | /* ----------------- apply periodics directly ------------ */ 281 | // sgp4fix for lyddane choice 282 | // strn3 used original inclination - this is technically feasible 283 | // gsfc used perturbed inclination - also technically feasible 284 | // probably best to readjust the 0.2 limit value and limit discontinuity 285 | // 0.2 rad = 11.45916 deg 286 | // use next line for original strn3 approach and original inclination 287 | // if (inclo >= 0.2) 288 | // use next line for gsfc version and perturbed inclination 289 | if (inclp >= 0.2) 290 | { 291 | ph = ph / sinip; 292 | pgh = pgh - cosip * ph; 293 | argpp = argpp + pgh; 294 | nodep = nodep + ph; 295 | mp = mp + pl; 296 | } 297 | else 298 | { 299 | /* ---- apply periodics with lyddane modification ---- */ 300 | sinop = sin(nodep); 301 | cosop = cos(nodep); 302 | alfdp = sinip * sinop; 303 | betdp = sinip * cosop; 304 | dalf = ph * cosop + pinc * cosip * sinop; 305 | dbet = -ph * sinop + pinc * cosip * cosop; 306 | alfdp = alfdp + dalf; 307 | betdp = betdp + dbet; 308 | nodep = floatmod(nodep, twopi); 309 | // sgp4fix for afspc written intrinsic functions 310 | // nodep used without a trigonometric function ahead 311 | if ((nodep < 0.0) && (opsmode == 'a')) 312 | nodep = nodep + twopi; 313 | xls = mp + argpp + cosip * nodep; 314 | dls = pl + pgh - pinc * nodep * sinip; 315 | xls = xls + dls; 316 | xnoh = nodep; 317 | nodep = atan2(alfdp, betdp); 318 | // sgp4fix for afspc written intrinsic functions 319 | // nodep used without a trigonometric function ahead 320 | if ((nodep < 0.0) && (opsmode == 'a')) 321 | nodep = nodep + twopi; 322 | if ((fabs(xnoh - nodep)) > pi) 323 | { 324 | if (nodep < xnoh) 325 | nodep = nodep + twopi; 326 | else 327 | nodep = nodep - twopi; 328 | } 329 | mp = mp + pl; 330 | argpp = xls - mp - cosip * nodep; 331 | } 332 | } // if init == 'n' 333 | 334 | //#include "debug1.cpp" 335 | } // end dpper 336 | 337 | /*----------------------------------------------------------------------------- 338 | * 339 | * procedure dscom 340 | * 341 | * this procedure provides deep space common items used by both the secular 342 | * and periodics subroutines. input is provided as shown. this routine 343 | * used to be called dpper, but the functions inside weren't well organized. 344 | * 345 | * author : david vallado 719-573-2600 28 jun 2005 346 | * 347 | * inputs : 348 | * epoch - 349 | * ep - eccentricity 350 | * argpp - argument of perigee 351 | * tc - 352 | * inclp - inclination 353 | * nodep - right ascension of ascending node 354 | * np - mean motion 355 | * 356 | * outputs : 357 | * sinim , cosim , sinomm , cosomm , snodm , cnodm 358 | * day - 359 | * e3 - 360 | * ee2 - 361 | * em - eccentricity 362 | * emsq - eccentricity squared 363 | * gam - 364 | * peo - 365 | * pgho - 366 | * pho - 367 | * pinco - 368 | * plo - 369 | * rtemsq - 370 | * se2, se3 - 371 | * sgh2, sgh3, sgh4 - 372 | * sh2, sh3, si2, si3, sl2, sl3, sl4 - 373 | * s1, s2, s3, s4, s5, s6, s7 - 374 | * ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - 375 | * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - 376 | * xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 377 | * nm - mean motion 378 | * z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - 379 | * zmol - 380 | * zmos - 381 | * 382 | * locals : 383 | * a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - 384 | * betasq - 385 | * cc - 386 | * ctem, stem - 387 | * x1, x2, x3, x4, x5, x6, x7, x8 - 388 | * xnodce - 389 | * xnoi - 390 | * zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , 391 | * zcosi , zsini , zcosil , zsinil , 392 | * zx - 393 | * zy - 394 | * 395 | * coupling : 396 | * none. 397 | * 398 | * references : 399 | * hoots, roehrich, norad spacetrack report #3 1980 400 | * hoots, norad spacetrack report #6 1986 401 | * hoots, schumacher and glover 2004 402 | * vallado, crawford, hujsak, kelso 2006 403 | ----------------------------------------------------------------------------*/ 404 | 405 | static void dscom 406 | ( 407 | double epoch, double ep, double argpp, double tc, double inclp, 408 | double nodep, double np, 409 | double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, 410 | double& cosomm,double& day, double& e3, double& ee2, double& em, 411 | double& emsq, double& gam, double& peo, double& pgho, double& pho, 412 | double& pinco, double& plo, double& rtemsq, double& se2, double& se3, 413 | double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, 414 | double& si2, double& si3, double& sl2, double& sl3, double& sl4, 415 | double& s1, double& s2, double& s3, double& s4, double& s5, 416 | double& s6, double& s7, double& ss1, double& ss2, double& ss3, 417 | double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, 418 | double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, 419 | double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, 420 | double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, 421 | double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, 422 | double& xl4, double& nm, double& z1, double& z2, double& z3, 423 | double& z11, double& z12, double& z13, double& z21, double& z22, 424 | double& z23, double& z31, double& z32, double& z33, double& zmol, 425 | double& zmos 426 | ) 427 | { 428 | /* -------------------------- constants ------------------------- */ 429 | const double zes = 0.01675; 430 | const double zel = 0.05490; 431 | const double c1ss = 2.9864797e-6; 432 | const double c1l = 4.7968065e-7; 433 | const double zsinis = 0.39785416; 434 | const double zcosis = 0.91744867; 435 | const double zcosgs = 0.1945905; 436 | const double zsings = -0.98088458; 437 | const double twopi = 2.0 * pi; 438 | 439 | /* --------------------- local variables ------------------------ */ 440 | int lsflg; 441 | double a1 , a2 , a3 , a4 , a5 , a6 , a7 , 442 | a8 , a9 , a10 , betasq, cc , ctem , stem , 443 | x1 , x2 , x3 , x4 , x5 , x6 , x7 , 444 | x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, 445 | zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , 446 | zsinil, zx , zy; 447 | 448 | nm = np; 449 | em = ep; 450 | snodm = sin(nodep); 451 | cnodm = cos(nodep); 452 | sinomm = sin(argpp); 453 | cosomm = cos(argpp); 454 | sinim = sin(inclp); 455 | cosim = cos(inclp); 456 | emsq = em * em; 457 | betasq = 1.0 - emsq; 458 | rtemsq = sqrt(betasq); 459 | 460 | /* ----------------- initialize lunar solar terms --------------- */ 461 | peo = 0.0; 462 | pinco = 0.0; 463 | plo = 0.0; 464 | pgho = 0.0; 465 | pho = 0.0; 466 | day = epoch + 18261.5 + tc / 1440.0; 467 | xnodce = floatmod(4.5236020 - 9.2422029e-4 * day, twopi); 468 | stem = sin(xnodce); 469 | ctem = cos(xnodce); 470 | zcosil = 0.91375164 - 0.03568096 * ctem; 471 | zsinil = sqrt(1.0 - zcosil * zcosil); 472 | zsinhl = 0.089683511 * stem / zsinil; 473 | zcoshl = sqrt(1.0 - zsinhl * zsinhl); 474 | gam = 5.8351514 + 0.0019443680 * day; 475 | zx = 0.39785416 * stem / zsinil; 476 | zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; 477 | zx = atan2(zx, zy); 478 | zx = gam + zx - xnodce; 479 | zcosgl = cos(zx); 480 | zsingl = sin(zx); 481 | 482 | /* ------------------------- do solar terms --------------------- */ 483 | zcosg = zcosgs; 484 | zsing = zsings; 485 | zcosi = zcosis; 486 | zsini = zsinis; 487 | zcosh = cnodm; 488 | zsinh = snodm; 489 | cc = c1ss; 490 | xnoi = 1.0 / nm; 491 | 492 | for (lsflg = 1; lsflg <= 2; lsflg++) 493 | { 494 | a1 = zcosg * zcosh + zsing * zcosi * zsinh; 495 | a3 = -zsing * zcosh + zcosg * zcosi * zsinh; 496 | a7 = -zcosg * zsinh + zsing * zcosi * zcosh; 497 | a8 = zsing * zsini; 498 | a9 = zsing * zsinh + zcosg * zcosi * zcosh; 499 | a10 = zcosg * zsini; 500 | a2 = cosim * a7 + sinim * a8; 501 | a4 = cosim * a9 + sinim * a10; 502 | a5 = -sinim * a7 + cosim * a8; 503 | a6 = -sinim * a9 + cosim * a10; 504 | 505 | x1 = a1 * cosomm + a2 * sinomm; 506 | x2 = a3 * cosomm + a4 * sinomm; 507 | x3 = -a1 * sinomm + a2 * cosomm; 508 | x4 = -a3 * sinomm + a4 * cosomm; 509 | x5 = a5 * sinomm; 510 | x6 = a6 * sinomm; 511 | x7 = a5 * cosomm; 512 | x8 = a6 * cosomm; 513 | 514 | z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; 515 | z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; 516 | z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; 517 | z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; 518 | z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; 519 | z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; 520 | z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); 521 | z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * 522 | (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); 523 | z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); 524 | z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); 525 | z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * 526 | (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); 527 | z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); 528 | z1 = z1 + z1 + betasq * z31; 529 | z2 = z2 + z2 + betasq * z32; 530 | z3 = z3 + z3 + betasq * z33; 531 | s3 = cc * xnoi; 532 | s2 = -0.5 * s3 / rtemsq; 533 | s4 = s3 * rtemsq; 534 | s1 = -15.0 * em * s4; 535 | s5 = x1 * x3 + x2 * x4; 536 | s6 = x2 * x3 + x1 * x4; 537 | s7 = x2 * x4 - x1 * x3; 538 | 539 | /* ----------------------- do lunar terms ------------------- */ 540 | if (lsflg == 1) 541 | { 542 | ss1 = s1; 543 | ss2 = s2; 544 | ss3 = s3; 545 | ss4 = s4; 546 | ss5 = s5; 547 | ss6 = s6; 548 | ss7 = s7; 549 | sz1 = z1; 550 | sz2 = z2; 551 | sz3 = z3; 552 | sz11 = z11; 553 | sz12 = z12; 554 | sz13 = z13; 555 | sz21 = z21; 556 | sz22 = z22; 557 | sz23 = z23; 558 | sz31 = z31; 559 | sz32 = z32; 560 | sz33 = z33; 561 | zcosg = zcosgl; 562 | zsing = zsingl; 563 | zcosi = zcosil; 564 | zsini = zsinil; 565 | zcosh = zcoshl * cnodm + zsinhl * snodm; 566 | zsinh = snodm * zcoshl - cnodm * zsinhl; 567 | cc = c1l; 568 | } 569 | } 570 | 571 | zmol = floatmod(4.7199672 + 0.22997150 * day - gam, twopi); 572 | zmos = floatmod(6.2565837 + 0.017201977 * day, twopi); 573 | 574 | /* ------------------------ do solar terms ---------------------- */ 575 | se2 = 2.0 * ss1 * ss6; 576 | se3 = 2.0 * ss1 * ss7; 577 | si2 = 2.0 * ss2 * sz12; 578 | si3 = 2.0 * ss2 * (sz13 - sz11); 579 | sl2 = -2.0 * ss3 * sz2; 580 | sl3 = -2.0 * ss3 * (sz3 - sz1); 581 | sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; 582 | sgh2 = 2.0 * ss4 * sz32; 583 | sgh3 = 2.0 * ss4 * (sz33 - sz31); 584 | sgh4 = -18.0 * ss4 * zes; 585 | sh2 = -2.0 * ss2 * sz22; 586 | sh3 = -2.0 * ss2 * (sz23 - sz21); 587 | 588 | /* ------------------------ do lunar terms ---------------------- */ 589 | ee2 = 2.0 * s1 * s6; 590 | e3 = 2.0 * s1 * s7; 591 | xi2 = 2.0 * s2 * z12; 592 | xi3 = 2.0 * s2 * (z13 - z11); 593 | xl2 = -2.0 * s3 * z2; 594 | xl3 = -2.0 * s3 * (z3 - z1); 595 | xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; 596 | xgh2 = 2.0 * s4 * z32; 597 | xgh3 = 2.0 * s4 * (z33 - z31); 598 | xgh4 = -18.0 * s4 * zel; 599 | xh2 = -2.0 * s2 * z22; 600 | xh3 = -2.0 * s2 * (z23 - z21); 601 | 602 | //#include "debug2.cpp" 603 | } // end dscom 604 | 605 | /*----------------------------------------------------------------------------- 606 | * 607 | * procedure dsinit 608 | * 609 | * this procedure provides deep space contributions to mean motion dot due 610 | * to geopotential resonance with half day and one day orbits. 611 | * 612 | * author : david vallado 719-573-2600 28 jun 2005 613 | * 614 | * inputs : 615 | * cosim, sinim- 616 | * emsq - eccentricity squared 617 | * argpo - argument of perigee 618 | * s1, s2, s3, s4, s5 - 619 | * ss1, ss2, ss3, ss4, ss5 - 620 | * sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - 621 | * t - time 622 | * tc - 623 | * gsto - greenwich sidereal time rad 624 | * mo - mean anomaly 625 | * mdot - mean anomaly dot (rate) 626 | * no - mean motion 627 | * nodeo - right ascension of ascending node 628 | * nodedot - right ascension of ascending node dot (rate) 629 | * xpidot - 630 | * z1, z3, z11, z13, z21, z23, z31, z33 - 631 | * eccm - eccentricity 632 | * argpm - argument of perigee 633 | * inclm - inclination 634 | * mm - mean anomaly 635 | * xn - mean motion 636 | * nodem - right ascension of ascending node 637 | * 638 | * outputs : 639 | * em - eccentricity 640 | * argpm - argument of perigee 641 | * inclm - inclination 642 | * mm - mean anomaly 643 | * nm - mean motion 644 | * nodem - right ascension of ascending node 645 | * irez - flag for resonance 0-none, 1-one day, 2-half day 646 | * atime - 647 | * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 648 | * dedt - 649 | * didt - 650 | * dmdt - 651 | * dndt - 652 | * dnodt - 653 | * domdt - 654 | * del1, del2, del3 - 655 | * ses , sghl , sghs , sgs , shl , shs , sis , sls 656 | * theta - 657 | * xfact - 658 | * xlamo - 659 | * xli - 660 | * xni 661 | * 662 | * locals : 663 | * ainv2 - 664 | * aonv - 665 | * cosisq - 666 | * eoc - 667 | * f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - 668 | * g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - 669 | * sini2 - 670 | * temp - 671 | * temp1 - 672 | * theta - 673 | * xno2 - 674 | * 675 | * coupling : 676 | * getgravconst 677 | * 678 | * references : 679 | * hoots, roehrich, norad spacetrack report #3 1980 680 | * hoots, norad spacetrack report #6 1986 681 | * hoots, schumacher and glover 2004 682 | * vallado, crawford, hujsak, kelso 2006 683 | ----------------------------------------------------------------------------*/ 684 | 685 | static void dsinit 686 | ( 687 | gravconsttype whichconst, 688 | double cosim, double emsq, double argpo, double s1, double s2, 689 | double s3, double s4, double s5, double sinim, double ss1, 690 | double ss2, double ss3, double ss4, double ss5, double sz1, 691 | double sz3, double sz11, double sz13, double sz21, double sz23, 692 | double sz31, double sz33, double t, double tc, double gsto, 693 | double mo, double mdot, double no, double nodeo, double nodedot, 694 | double xpidot, double z1, double z3, double z11, double z13, 695 | double z21, double z23, double z31, double z33, double ecco, 696 | double eccsq, double& em, double& argpm, double& inclm, double& mm, 697 | double& nm, double& nodem, 698 | int& irez, 699 | double& atime, double& d2201, double& d2211, double& d3210, double& d3222, 700 | double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, 701 | double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, 702 | double& dnodt, double& domdt, double& del1, double& del2, double& del3, 703 | double& xfact, double& xlamo, double& xli, double& xni 704 | ) 705 | { 706 | /* --------------------- local variables ------------------------ */ 707 | const double twopi = 2.0 * pi; 708 | 709 | double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , 710 | f321 , f322 , f330 , f441 , f442 , f522 , f523 , 711 | f542 , f543 , g200 , g201 , g211 , g300 , g310 , 712 | g322 , g410 , g422 , g520 , g521 , g532 , g533 , 713 | ses , sgs , sghl , sghs , shs , shll , sis , 714 | sini2 , sls , temp , temp1 , theta , xno2 , q22 , 715 | q31 , q33 , root22, root44, root54, rptim , root32, 716 | root52, x2o3 , xke , znl , emo , zns , emsqo, 717 | tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; 718 | 719 | q22 = 1.7891679e-6; 720 | q31 = 2.1460748e-6; 721 | q33 = 2.2123015e-7; 722 | root22 = 1.7891679e-6; 723 | root44 = 7.3636953e-9; 724 | root54 = 2.1765803e-9; 725 | rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 726 | root32 = 3.7393792e-7; 727 | root52 = 1.1428639e-7; 728 | x2o3 = 2.0 / 3.0; 729 | znl = 1.5835218e-4; 730 | zns = 1.19459e-5; 731 | 732 | // sgp4fix identify constants and allow alternate values 733 | getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 734 | 735 | /* -------------------- deep space initialization ------------ */ 736 | irez = 0; 737 | if ((nm < 0.0052359877) && (nm > 0.0034906585)) 738 | irez = 1; 739 | if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) 740 | irez = 2; 741 | 742 | /* ------------------------ do solar terms ------------------- */ 743 | ses = ss1 * zns * ss5; 744 | sis = ss2 * zns * (sz11 + sz13); 745 | sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); 746 | sghs = ss4 * zns * (sz31 + sz33 - 6.0); 747 | shs = -zns * ss2 * (sz21 + sz23); 748 | // sgp4fix for 180 deg incl 749 | if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) 750 | shs = 0.0; 751 | if (sinim != 0.0) 752 | shs = shs / sinim; 753 | sgs = sghs - cosim * shs; 754 | 755 | /* ------------------------- do lunar terms ------------------ */ 756 | dedt = ses + s1 * znl * s5; 757 | didt = sis + s2 * znl * (z11 + z13); 758 | dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); 759 | sghl = s4 * znl * (z31 + z33 - 6.0); 760 | shll = -znl * s2 * (z21 + z23); 761 | // sgp4fix for 180 deg incl 762 | if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) 763 | shll = 0.0; 764 | domdt = sgs + sghl; 765 | dnodt = shs; 766 | if (sinim != 0.0) 767 | { 768 | domdt = domdt - cosim / sinim * shll; 769 | dnodt = dnodt + shll / sinim; 770 | } 771 | 772 | /* ----------- calculate deep space resonance effects -------- */ 773 | dndt = 0.0; 774 | theta = floatmod(gsto + tc * rptim, twopi); 775 | em = em + dedt * t; 776 | inclm = inclm + didt * t; 777 | argpm = argpm + domdt * t; 778 | nodem = nodem + dnodt * t; 779 | mm = mm + dmdt * t; 780 | // sgp4fix for negative inclinations 781 | // the following if statement should be commented out 782 | //if (inclm < 0.0) 783 | // { 784 | // inclm = -inclm; 785 | // argpm = argpm - pi; 786 | // nodem = nodem + pi; 787 | // } 788 | 789 | /* -------------- initialize the resonance terms ------------- */ 790 | if (irez != 0) 791 | { 792 | aonv = pow(nm / xke, x2o3); 793 | 794 | /* ---------- geopotential resonance for 12 hour orbits ------ */ 795 | if (irez == 2) 796 | { 797 | cosisq = cosim * cosim; 798 | emo = em; 799 | em = ecco; 800 | emsqo = emsq; 801 | emsq = eccsq; 802 | eoc = em * emsq; 803 | g201 = -0.306 - (em - 0.64) * 0.440; 804 | 805 | if (em <= 0.65) 806 | { 807 | g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; 808 | g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; 809 | g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; 810 | g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; 811 | g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; 812 | g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; 813 | } 814 | else 815 | { 816 | g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; 817 | g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; 818 | g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; 819 | g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; 820 | g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; 821 | if (em > 0.715) 822 | g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; 823 | else 824 | g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; 825 | } 826 | if (em < 0.7) 827 | { 828 | g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; 829 | g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; 830 | g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; 831 | } 832 | else 833 | { 834 | g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; 835 | g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; 836 | g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; 837 | } 838 | 839 | sini2= sinim * sinim; 840 | f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); 841 | f221 = 1.5 * sini2; 842 | f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); 843 | f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); 844 | f441 = 35.0 * sini2 * f220; 845 | f442 = 39.3750 * sini2 * sini2; 846 | f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + 847 | 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); 848 | f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + 849 | 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); 850 | f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * 851 | (-12.0 + 8.0 * cosim + 10.0 * cosisq)); 852 | f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * 853 | (12.0 + 8.0 * cosim - 10.0 * cosisq)); 854 | xno2 = nm * nm; 855 | ainv2 = aonv * aonv; 856 | temp1 = 3.0 * xno2 * ainv2; 857 | temp = temp1 * root22; 858 | d2201 = temp * f220 * g201; 859 | d2211 = temp * f221 * g211; 860 | temp1 = temp1 * aonv; 861 | temp = temp1 * root32; 862 | d3210 = temp * f321 * g310; 863 | d3222 = temp * f322 * g322; 864 | temp1 = temp1 * aonv; 865 | temp = 2.0 * temp1 * root44; 866 | d4410 = temp * f441 * g410; 867 | d4422 = temp * f442 * g422; 868 | temp1 = temp1 * aonv; 869 | temp = temp1 * root52; 870 | d5220 = temp * f522 * g520; 871 | d5232 = temp * f523 * g532; 872 | temp = 2.0 * temp1 * root54; 873 | d5421 = temp * f542 * g521; 874 | d5433 = temp * f543 * g533; 875 | xlamo = floatmod(mo + nodeo + nodeo-theta - theta, twopi); 876 | xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; 877 | em = emo; 878 | emsq = emsqo; 879 | } 880 | 881 | /* ---------------- synchronous resonance terms -------------- */ 882 | if (irez == 1) 883 | { 884 | g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); 885 | g310 = 1.0 + 2.0 * emsq; 886 | g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); 887 | f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); 888 | f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); 889 | f330 = 1.0 + cosim; 890 | f330 = 1.875 * f330 * f330 * f330; 891 | del1 = 3.0 * nm * nm * aonv * aonv; 892 | del2 = 2.0 * del1 * f220 * g200 * q22; 893 | del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; 894 | del1 = del1 * f311 * g310 * q31 * aonv; 895 | xlamo = floatmod(mo + nodeo + argpo - theta, twopi); 896 | xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; 897 | } 898 | 899 | /* ------------ for sgp4, initialize the integrator ---------- */ 900 | xli = xlamo; 901 | xni = no; 902 | atime = 0.0; 903 | nm = no + dndt; 904 | } 905 | 906 | //#include "debug3.cpp" 907 | } // end dsinit 908 | 909 | /*----------------------------------------------------------------------------- 910 | * 911 | * procedure dspace 912 | * 913 | * this procedure provides deep space contributions to mean elements for 914 | * perturbing third body. these effects have been averaged over one 915 | * revolution of the sun and moon. for earth resonance effects, the 916 | * effects have been averaged over no revolutions of the satellite. 917 | * (mean motion) 918 | * 919 | * author : david vallado 719-573-2600 28 jun 2005 920 | * 921 | * inputs : 922 | * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 923 | * dedt - 924 | * del1, del2, del3 - 925 | * didt - 926 | * dmdt - 927 | * dnodt - 928 | * domdt - 929 | * irez - flag for resonance 0-none, 1-one day, 2-half day 930 | * argpo - argument of perigee 931 | * argpdot - argument of perigee dot (rate) 932 | * t - time 933 | * tc - 934 | * gsto - gst 935 | * xfact - 936 | * xlamo - 937 | * no - mean motion 938 | * atime - 939 | * em - eccentricity 940 | * ft - 941 | * argpm - argument of perigee 942 | * inclm - inclination 943 | * xli - 944 | * mm - mean anomaly 945 | * xni - mean motion 946 | * nodem - right ascension of ascending node 947 | * 948 | * outputs : 949 | * atime - 950 | * em - eccentricity 951 | * argpm - argument of perigee 952 | * inclm - inclination 953 | * xli - 954 | * mm - mean anomaly 955 | * xni - 956 | * nodem - right ascension of ascending node 957 | * dndt - 958 | * nm - mean motion 959 | * 960 | * locals : 961 | * delt - 962 | * ft - 963 | * theta - 964 | * x2li - 965 | * x2omi - 966 | * xl - 967 | * xldot - 968 | * xnddt - 969 | * xndt - 970 | * xomi - 971 | * 972 | * coupling : 973 | * none - 974 | * 975 | * references : 976 | * hoots, roehrich, norad spacetrack report #3 1980 977 | * hoots, norad spacetrack report #6 1986 978 | * hoots, schumacher and glover 2004 979 | * vallado, crawford, hujsak, kelso 2006 980 | ----------------------------------------------------------------------------*/ 981 | 982 | static void dspace 983 | ( 984 | int irez, 985 | double d2201, double d2211, double d3210, double d3222, double d4410, 986 | double d4422, double d5220, double d5232, double d5421, double d5433, 987 | double dedt, double del1, double del2, double del3, double didt, 988 | double dmdt, double dnodt, double domdt, double argpo, double argpdot, 989 | double t, double tc, double gsto, double xfact, double xlamo, 990 | double no, 991 | double& atime, double& em, double& argpm, double& inclm, double& xli, 992 | double& mm, double& xni, double& nodem, double& dndt, double& nm 993 | ) 994 | { 995 | const double twopi = 2.0 * pi; 996 | int iretn , iret; 997 | double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, 998 | g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; 999 | 1000 | fasx2 = 0.13130908; 1001 | fasx4 = 2.8843198; 1002 | fasx6 = 0.37448087; 1003 | g22 = 5.7686396; 1004 | g32 = 0.95240898; 1005 | g44 = 1.8014998; 1006 | g52 = 1.0508330; 1007 | g54 = 4.4108898; 1008 | rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 1009 | stepp = 720.0; 1010 | stepn = -720.0; 1011 | step2 = 259200.0; 1012 | 1013 | /* ----------- calculate deep space resonance effects ----------- */ 1014 | dndt = 0.0; 1015 | theta = floatmod(gsto + tc * rptim, twopi); 1016 | em = em + dedt * t; 1017 | 1018 | inclm = inclm + didt * t; 1019 | argpm = argpm + domdt * t; 1020 | nodem = nodem + dnodt * t; 1021 | mm = mm + dmdt * t; 1022 | 1023 | // sgp4fix for negative inclinations 1024 | // the following if statement should be commented out 1025 | // if (inclm < 0.0) 1026 | // { 1027 | // inclm = -inclm; 1028 | // argpm = argpm - pi; 1029 | // nodem = nodem + pi; 1030 | // } 1031 | 1032 | /* - update resonances : numerical (euler-maclaurin) integration - */ 1033 | /* ------------------------- epoch restart ---------------------- */ 1034 | // sgp4fix for propagator problems 1035 | // the following integration works for negative time steps and periods 1036 | // the specific changes are unknown because the original code was so convoluted 1037 | 1038 | // sgp4fix take out atime = 0.0 and fix for faster operation 1039 | ft = 0.0; 1040 | if (irez != 0) 1041 | { 1042 | // sgp4fix streamline check 1043 | if ((atime == 0.0) || (t * atime <= 0.0) || (fabs(t) < fabs(atime)) ) 1044 | { 1045 | atime = 0.0; 1046 | xni = no; 1047 | xli = xlamo; 1048 | } 1049 | // sgp4fix move check outside loop 1050 | if (t > 0.0) 1051 | delt = stepp; 1052 | else 1053 | delt = stepn; 1054 | 1055 | iretn = 381; // added for do loop 1056 | iret = 0; // added for loop 1057 | while (iretn == 381) 1058 | { 1059 | /* ------------------- dot terms calculated ------------- */ 1060 | /* ----------- near - synchronous resonance terms ------- */ 1061 | if (irez != 2) 1062 | { 1063 | xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + 1064 | del3 * sin(3.0 * (xli - fasx6)); 1065 | xldot = xni + xfact; 1066 | xnddt = del1 * cos(xli - fasx2) + 1067 | 2.0 * del2 * cos(2.0 * (xli - fasx4)) + 1068 | 3.0 * del3 * cos(3.0 * (xli - fasx6)); 1069 | xnddt = xnddt * xldot; 1070 | } 1071 | else 1072 | { 1073 | /* --------- near - half-day resonance terms -------- */ 1074 | xomi = argpo + argpdot * atime; 1075 | x2omi = xomi + xomi; 1076 | x2li = xli + xli; 1077 | xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + 1078 | d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ 1079 | d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + 1080 | d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ 1081 | d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); 1082 | xldot = xni + xfact; 1083 | xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + 1084 | d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + 1085 | d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + 1086 | 2.0 * (d4410 * cos(x2omi + x2li - g44) + 1087 | d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + 1088 | d5433 * cos(-xomi + x2li - g54)); 1089 | xnddt = xnddt * xldot; 1090 | } 1091 | 1092 | /* ----------------------- integrator ------------------- */ 1093 | // sgp4fix move end checks to end of routine 1094 | if (fabs(t - atime) >= stepp) 1095 | { 1096 | iret = 0; 1097 | iretn = 381; 1098 | } 1099 | else // exit here 1100 | { 1101 | ft = t - atime; 1102 | iretn = 0; 1103 | } 1104 | 1105 | if (iretn == 381) 1106 | { 1107 | xli = xli + xldot * delt + xndt * step2; 1108 | xni = xni + xndt * delt + xnddt * step2; 1109 | atime = atime + delt; 1110 | } 1111 | } // while iretn = 381 1112 | 1113 | nm = xni + xndt * ft + xnddt * ft * ft * 0.5; 1114 | xl = xli + xldot * ft + xndt * ft * ft * 0.5; 1115 | if (irez != 1) 1116 | { 1117 | mm = xl - 2.0 * nodem + 2.0 * theta; 1118 | dndt = nm - no; 1119 | } 1120 | else 1121 | { 1122 | mm = xl - nodem - argpm + theta; 1123 | dndt = nm - no; 1124 | } 1125 | nm = no + dndt; 1126 | } 1127 | 1128 | //#include "debug4.cpp" 1129 | } // end dsspace 1130 | 1131 | /*----------------------------------------------------------------------------- 1132 | * 1133 | * procedure initl 1134 | * 1135 | * this procedure initializes the spg4 propagator. all the initialization is 1136 | * consolidated here instead of having multiple loops inside other routines. 1137 | * 1138 | * author : david vallado 719-573-2600 28 jun 2005 1139 | * 1140 | * inputs : 1141 | * ecco - eccentricity 0.0 - 1.0 1142 | * epoch - epoch time in days from jan 0, 1950. 0 hr 1143 | * inclo - inclination of satellite 1144 | * no - mean motion of satellite 1145 | * satn - satellite number 1146 | * 1147 | * outputs : 1148 | * ainv - 1.0 / a 1149 | * ao - semi major axis 1150 | * con41 - 1151 | * con42 - 1.0 - 5.0 cos(i) 1152 | * cosio - cosine of inclination 1153 | * cosio2 - cosio squared 1154 | * eccsq - eccentricity squared 1155 | * method - flag for deep space 'd', 'n' 1156 | * omeosq - 1.0 - ecco * ecco 1157 | * posq - semi-parameter squared 1158 | * rp - radius of perigee 1159 | * rteosq - square root of (1.0 - ecco*ecco) 1160 | * sinio - sine of inclination 1161 | * gsto - gst at time of observation rad 1162 | * no - mean motion of satellite 1163 | * 1164 | * locals : 1165 | * ak - 1166 | * d1 - 1167 | * del - 1168 | * adel - 1169 | * po - 1170 | * 1171 | * coupling : 1172 | * getgravconst 1173 | * gstime - find greenwich sidereal time from the julian date 1174 | * 1175 | * references : 1176 | * hoots, roehrich, norad spacetrack report #3 1980 1177 | * hoots, norad spacetrack report #6 1986 1178 | * hoots, schumacher and glover 2004 1179 | * vallado, crawford, hujsak, kelso 2006 1180 | ----------------------------------------------------------------------------*/ 1181 | 1182 | static void initl 1183 | ( 1184 | int satn, gravconsttype whichconst, 1185 | double ecco, double epoch, double inclo, double& no, 1186 | char& method, 1187 | double& ainv, double& ao, double& con41, double& con42, double& cosio, 1188 | double& cosio2,double& eccsq, double& omeosq, double& posq, 1189 | double& rp, double& rteosq,double& sinio , double& gsto, 1190 | char opsmode 1191 | ) 1192 | { 1193 | /* --------------------- local variables ------------------------ */ 1194 | double ak, d1, del, adel, po, x2o3, j2, xke, 1195 | tumin, mu, radiusearthkm, j3, j4, j3oj2; 1196 | 1197 | // sgp4fix use old way of finding gst 1198 | double ds70; 1199 | double ts70, tfrac, c1, thgr70, fk5r, c1p2p; 1200 | const double twopi = 2.0 * pi; 1201 | 1202 | /* ----------------------- earth constants ---------------------- */ 1203 | // sgp4fix identify constants and allow alternate values 1204 | getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1205 | x2o3 = 2.0 / 3.0; 1206 | 1207 | /* ------------- calculate auxillary epoch quantities ---------- */ 1208 | eccsq = ecco * ecco; 1209 | omeosq = 1.0 - eccsq; 1210 | rteosq = sqrt(omeosq); 1211 | cosio = cos(inclo); 1212 | cosio2 = cosio * cosio; 1213 | 1214 | /* ------------------ un-kozai the mean motion ----------------- */ 1215 | ak = pow(xke / no, x2o3); 1216 | d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); 1217 | del = d1 / (ak * ak); 1218 | adel = ak * (1.0 - del * del - del * 1219 | (1.0 / 3.0 + 134.0 * del * del / 81.0)); 1220 | del = d1/(adel * adel); 1221 | no = no / (1.0 + del); 1222 | 1223 | ao = pow(xke / no, x2o3); 1224 | sinio = sin(inclo); 1225 | po = ao * omeosq; 1226 | con42 = 1.0 - 5.0 * cosio2; 1227 | con41 = -con42-cosio2-cosio2; 1228 | ainv = 1.0 / ao; 1229 | posq = po * po; 1230 | rp = ao * (1.0 - ecco); 1231 | method = 'n'; 1232 | 1233 | // sgp4fix modern approach to finding sidereal time 1234 | if (opsmode == 'a') 1235 | { 1236 | // sgp4fix use old way of finding gst 1237 | // count integer number of days from 0 jan 1970 1238 | ts70 = epoch - 7305.0; 1239 | ds70 = floor(ts70 + 1.0e-8); 1240 | tfrac = ts70 - ds70; 1241 | // find greenwich location at epoch 1242 | c1 = 1.72027916940703639e-2; 1243 | thgr70= 1.7321343856509374; 1244 | fk5r = 5.07551419432269442e-15; 1245 | c1p2p = c1 + twopi; 1246 | gsto = floatmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); 1247 | if ( gsto < 0.0 ) 1248 | gsto = gsto + twopi; 1249 | } 1250 | else 1251 | gsto = gstime(epoch + 2433281.5); 1252 | 1253 | 1254 | //#include "debug5.cpp" 1255 | } // end initl 1256 | 1257 | /*----------------------------------------------------------------------------- 1258 | * 1259 | * procedure sgp4init 1260 | * 1261 | * this procedure initializes variables for sgp4. 1262 | * 1263 | * author : david vallado 719-573-2600 28 jun 2005 1264 | * 1265 | * inputs : 1266 | * opsmode - mode of operation afspc or improved 'a', 'i' 1267 | * whichconst - which set of constants to use 72, 84 1268 | * satn - satellite number 1269 | * bstar - sgp4 type drag coefficient kg/m2er 1270 | * ecco - eccentricity 1271 | * epoch - epoch time in days from jan 0, 1950. 0 hr 1272 | * argpo - argument of perigee (output if ds) 1273 | * inclo - inclination 1274 | * mo - mean anomaly (output if ds) 1275 | * no - mean motion 1276 | * nodeo - right ascension of ascending node 1277 | * 1278 | * outputs : 1279 | * satrec - common values for subsequent calls 1280 | * return code - non-zero on error. 1281 | * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er 1282 | * 2 - mean motion less than 0.0 1283 | * 3 - pert elements, ecc < 0.0 or ecc > 1.0 1284 | * 4 - semi-latus rectum < 0.0 1285 | * 5 - epoch elements are sub-orbital 1286 | * 6 - satellite has decayed 1287 | * 1288 | * locals : 1289 | * cnodm , snodm , cosim , sinim , cosomm , sinomm 1290 | * cc1sq , cc2 , cc3 1291 | * coef , coef1 1292 | * cosio4 - 1293 | * day - 1294 | * dndt - 1295 | * em - eccentricity 1296 | * emsq - eccentricity squared 1297 | * eeta - 1298 | * etasq - 1299 | * gam - 1300 | * argpm - argument of perigee 1301 | * nodem - 1302 | * inclm - inclination 1303 | * mm - mean anomaly 1304 | * nm - mean motion 1305 | * perige - perigee 1306 | * pinvsq - 1307 | * psisq - 1308 | * qzms24 - 1309 | * rtemsq - 1310 | * s1, s2, s3, s4, s5, s6, s7 - 1311 | * sfour - 1312 | * ss1, ss2, ss3, ss4, ss5, ss6, ss7 - 1313 | * sz1, sz2, sz3 1314 | * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - 1315 | * tc - 1316 | * temp - 1317 | * temp1, temp2, temp3 - 1318 | * tsi - 1319 | * xpidot - 1320 | * xhdot1 - 1321 | * z1, z2, z3 - 1322 | * z11, z12, z13, z21, z22, z23, z31, z32, z33 - 1323 | * 1324 | * coupling : 1325 | * getgravconst- 1326 | * initl - 1327 | * dscom - 1328 | * dpper - 1329 | * dsinit - 1330 | * sgp4 - 1331 | * 1332 | * references : 1333 | * hoots, roehrich, norad spacetrack report #3 1980 1334 | * hoots, norad spacetrack report #6 1986 1335 | * hoots, schumacher and glover 2004 1336 | * vallado, crawford, hujsak, kelso 2006 1337 | ----------------------------------------------------------------------------*/ 1338 | 1339 | bool sgp4init 1340 | ( 1341 | gravconsttype whichconst, char opsmode, const int satn, const double epoch, 1342 | const double xbstar, const double xecco, const double xargpo, 1343 | const double xinclo, const double xmo, const double xno, 1344 | const double xnodeo, elsetrec& satrec 1345 | ) 1346 | { 1347 | /* --------------------- local variables ------------------------ */ 1348 | double ao, ainv, con42, cosio, sinio, cosio2, eccsq, 1349 | omeosq, posq, rp, rteosq, 1350 | cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , 1351 | cc2 , cc3 , coef , coef1 , cosio4, day , dndt , 1352 | em , emsq , eeta , etasq , gam , argpm , nodem , 1353 | inclm , mm , nm , perige, pinvsq, psisq , qzms24, 1354 | rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , 1355 | s7 , sfour , ss1 , ss2 , ss3 , ss4 , ss5 , 1356 | ss6 , ss7 , sz1 , sz2 , sz3 , sz11 , sz12 , 1357 | sz13 , sz21 , sz22 , sz23 , sz31 , sz32 , sz33 , 1358 | tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, 1359 | xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , 1360 | z21 , z22 , z23 , z31 , z32 , z33, 1361 | qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], 1362 | tumin, mu, radiusearthkm, xke, j3, delmotemp, qzms2ttemp, qzms24temp; 1363 | 1364 | /* ------------------------ initialization --------------------- */ 1365 | // sgp4fix divisor for divide by zero check on inclination 1366 | // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to 1367 | // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 1368 | const double temp4 = 1.5e-12; 1369 | 1370 | /* ----------- set all near earth variables to zero ------------ */ 1371 | satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; 1372 | satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; 1373 | satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; 1374 | satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; 1375 | satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; 1376 | satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; 1377 | satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; 1378 | satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; 1379 | satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; 1380 | 1381 | /* ----------- set all deep space variables to zero ------------ */ 1382 | satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; 1383 | satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; 1384 | satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; 1385 | satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; 1386 | satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; 1387 | satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; 1388 | satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; 1389 | satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; 1390 | satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; 1391 | satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; 1392 | satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; 1393 | satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; 1394 | satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; 1395 | satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; 1396 | satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; 1397 | satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; 1398 | satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; 1399 | satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; 1400 | satrec.xli = 0.0; satrec.xni = 0.0; 1401 | 1402 | // sgp4fix - note the following variables are also passed directly via satrec. 1403 | // it is possible to streamline the sgp4init call by deleting the "x" 1404 | // variables, but the user would need to set the satrec.* values first. we 1405 | // include the additional assignments in case twoline2rv is not used. 1406 | satrec.bstar = xbstar; 1407 | satrec.ecco = xecco; 1408 | satrec.argpo = xargpo; 1409 | satrec.inclo = xinclo; 1410 | satrec.mo = xmo; 1411 | satrec.no = xno; 1412 | satrec.nodeo = xnodeo; 1413 | 1414 | // sgp4fix add opsmode 1415 | satrec.operationmode = opsmode; 1416 | 1417 | /* ------------------------ earth constants ----------------------- */ 1418 | // sgp4fix identify constants and allow alternate values 1419 | getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1420 | ss = 78.0 / radiusearthkm + 1.0; 1421 | // sgp4fix use multiply for speed instead of pow 1422 | qzms2ttemp = (120.0 - 78.0) / radiusearthkm; 1423 | qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp; 1424 | x2o3 = 2.0 / 3.0; 1425 | 1426 | satrec.init = 'y'; 1427 | satrec.t = 0.0; 1428 | 1429 | initl 1430 | ( 1431 | satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, 1432 | ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, 1433 | posq, rp, rteosq, sinio, satrec.gsto, satrec.operationmode 1434 | ); 1435 | satrec.error = 0; 1436 | 1437 | // sgp4fix remove this check as it is unnecessary 1438 | // the mrt check in sgp4 handles decaying satellite cases even if the starting 1439 | // condition is below the surface of te earth 1440 | // if (rp < 1.0) 1441 | // { 1442 | // printf("# *** satn%d epoch elts sub-orbital ***\n", satn); 1443 | // satrec.error = 5; 1444 | // } 1445 | 1446 | if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) 1447 | { 1448 | satrec.isimp = 0; 1449 | if (rp < (220.0 / radiusearthkm + 1.0)) 1450 | satrec.isimp = 1; 1451 | sfour = ss; 1452 | qzms24 = qzms2t; 1453 | perige = (rp - 1.0) * radiusearthkm; 1454 | 1455 | /* - for perigees below 156 km, s and qoms2t are altered - */ 1456 | if (perige < 156.0) 1457 | { 1458 | sfour = perige - 78.0; 1459 | if (perige < 98.0) 1460 | sfour = 20.0; 1461 | // sgp4fix use multiply for speed instead of pow 1462 | qzms24temp = (120.0 - sfour) / radiusearthkm; 1463 | qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp; 1464 | sfour = sfour / radiusearthkm + 1.0; 1465 | } 1466 | pinvsq = 1.0 / posq; 1467 | 1468 | tsi = 1.0 / (ao - sfour); 1469 | satrec.eta = ao * satrec.ecco * tsi; 1470 | etasq = satrec.eta * satrec.eta; 1471 | eeta = satrec.ecco * satrec.eta; 1472 | psisq = fabs(1.0 - etasq); 1473 | coef = qzms24 * pow(tsi, 4.0); 1474 | coef1 = coef / pow(psisq, 3.5); 1475 | cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * 1476 | (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * 1477 | (8.0 + 3.0 * etasq * (8.0 + etasq))); 1478 | satrec.cc1 = satrec.bstar * cc2; 1479 | cc3 = 0.0; 1480 | if (satrec.ecco > 1.0e-4) 1481 | cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; 1482 | satrec.x1mth2 = 1.0 - cosio2; 1483 | satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * 1484 | (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * 1485 | (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * 1486 | (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * 1487 | (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * 1488 | (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); 1489 | satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * 1490 | (etasq + eeta) + eeta * etasq); 1491 | cosio4 = cosio2 * cosio2; 1492 | temp1 = 1.5 * j2 * pinvsq * satrec.no; 1493 | temp2 = 0.5 * temp1 * j2 * pinvsq; 1494 | temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; 1495 | satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * 1496 | temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); 1497 | satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * 1498 | (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + 1499 | temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); 1500 | xhdot1 = -temp1 * cosio; 1501 | satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + 1502 | 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; 1503 | xpidot = satrec.argpdot+ satrec.nodedot; 1504 | satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); 1505 | satrec.xmcof = 0.0; 1506 | if (satrec.ecco > 1.0e-4) 1507 | satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; 1508 | satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; 1509 | satrec.t2cof = 1.5 * satrec.cc1; 1510 | // sgp4fix for divide by zero with xinco = 180 deg 1511 | if (fabs(cosio+1.0) > 1.5e-12) 1512 | satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); 1513 | else 1514 | satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; 1515 | satrec.aycof = -0.5 * j3oj2 * sinio; 1516 | // sgp4fix use multiply for speed instead of pow 1517 | delmotemp = 1.0 + satrec.eta * cos(satrec.mo); 1518 | satrec.delmo = delmotemp * delmotemp * delmotemp; 1519 | satrec.sinmao = sin(satrec.mo); 1520 | satrec.x7thm1 = 7.0 * cosio2 - 1.0; 1521 | 1522 | /* --------------- deep space initialization ------------- */ 1523 | if ((2*pi / satrec.no) >= 225.0) 1524 | { 1525 | satrec.method = 'd'; 1526 | satrec.isimp = 1; 1527 | tc = 0.0; 1528 | inclm = satrec.inclo; 1529 | 1530 | dscom 1531 | ( 1532 | epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, 1533 | satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, 1534 | day, satrec.e3, satrec.ee2, em, emsq, gam, 1535 | satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, 1536 | satrec.plo, rtemsq, satrec.se2, satrec.se3, 1537 | satrec.sgh2, satrec.sgh3, satrec.sgh4, 1538 | satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, 1539 | satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, 1540 | s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, 1541 | sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, 1542 | satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, 1543 | satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, 1544 | satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, 1545 | z12, z13, z21, z22, z23, z31, z32, z33, 1546 | satrec.zmol, satrec.zmos 1547 | ); 1548 | dpper 1549 | ( 1550 | satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, 1551 | satrec.pho, satrec.pinco, satrec.plo, satrec.se2, 1552 | satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, 1553 | satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, 1554 | satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, 1555 | satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, 1556 | satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, 1557 | satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, 1558 | satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo, 1559 | satrec.operationmode 1560 | ); 1561 | 1562 | argpm = 0.0; 1563 | nodem = 0.0; 1564 | mm = 0.0; 1565 | 1566 | dsinit 1567 | ( 1568 | whichconst, 1569 | cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, 1570 | ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, 1571 | satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, 1572 | satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, 1573 | satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, 1574 | satrec.irez, satrec.atime, 1575 | satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , 1576 | satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, 1577 | satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, 1578 | satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , 1579 | satrec.del1, satrec.del2, satrec.del3, satrec.xfact, 1580 | satrec.xlamo, satrec.xli, satrec.xni 1581 | ); 1582 | } 1583 | 1584 | /* ----------- set variables if not deep space ----------- */ 1585 | if (satrec.isimp != 1) 1586 | { 1587 | cc1sq = satrec.cc1 * satrec.cc1; 1588 | satrec.d2 = 4.0 * ao * tsi * cc1sq; 1589 | temp = satrec.d2 * tsi * satrec.cc1 / 3.0; 1590 | satrec.d3 = (17.0 * ao + sfour) * temp; 1591 | satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * 1592 | satrec.cc1; 1593 | satrec.t3cof = satrec.d2 + 2.0 * cc1sq; 1594 | satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * 1595 | (12.0 * satrec.d2 + 10.0 * cc1sq)); 1596 | satrec.t5cof = 0.2 * (3.0 * satrec.d4 + 1597 | 12.0 * satrec.cc1 * satrec.d3 + 1598 | 6.0 * satrec.d2 * satrec.d2 + 1599 | 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); 1600 | } 1601 | } // if omeosq = 0 ... 1602 | 1603 | /* finally propogate to zero epoch to initialize all others. */ 1604 | // sgp4fix take out check to let satellites process until they are actually below earth surface 1605 | // if(satrec.error == 0) 1606 | sgp4(whichconst, satrec, 0.0, r, v); 1607 | 1608 | satrec.init = 'n'; 1609 | 1610 | //#include "debug6.cpp" 1611 | //sgp4fix return boolean. satrec.error contains any error codes 1612 | return true; 1613 | } // end sgp4init 1614 | 1615 | /*----------------------------------------------------------------------------- 1616 | * 1617 | * procedure sgp4 1618 | * 1619 | * this procedure is the sgp4 prediction model from space command. this is an 1620 | * updated and combined version of sgp4 and sdp4, which were originally 1621 | * published separately in spacetrack report #3. this version follows the 1622 | * methodology from the aiaa paper (2006) describing the history and 1623 | * development of the code. 1624 | * 1625 | * author : david vallado 719-573-2600 28 jun 2005 1626 | * 1627 | * inputs : 1628 | * satrec - initialised structure from sgp4init() call. 1629 | * tsince - time eince epoch (minutes) 1630 | * 1631 | * outputs : 1632 | * r - position vector km 1633 | * v - velocity km/sec 1634 | * return code - non-zero on error. 1635 | * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er 1636 | * 2 - mean motion less than 0.0 1637 | * 3 - pert elements, ecc < 0.0 or ecc > 1.0 1638 | * 4 - semi-latus rectum < 0.0 1639 | * 5 - epoch elements are sub-orbital 1640 | * 6 - satellite has decayed 1641 | * 1642 | * locals : 1643 | * am - 1644 | * axnl, aynl - 1645 | * betal - 1646 | * cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , 1647 | * sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , 1648 | * cosisq , cossu , sinsu , cosu , sinu 1649 | * delm - 1650 | * delomg - 1651 | * dndt - 1652 | * eccm - 1653 | * emsq - 1654 | * ecose - 1655 | * el2 - 1656 | * eo1 - 1657 | * eccp - 1658 | * esine - 1659 | * argpm - 1660 | * argpp - 1661 | * omgadf - 1662 | * pl - 1663 | * r - 1664 | * rtemsq - 1665 | * rdotl - 1666 | * rl - 1667 | * rvdot - 1668 | * rvdotl - 1669 | * su - 1670 | * t2 , t3 , t4 , tc 1671 | * tem5, temp , temp1 , temp2 , tempa , tempe , templ 1672 | * u , ux , uy , uz , vx , vy , vz 1673 | * inclm - inclination 1674 | * mm - mean anomaly 1675 | * nm - mean motion 1676 | * nodem - right asc of ascending node 1677 | * xinc - 1678 | * xincp - 1679 | * xl - 1680 | * xlm - 1681 | * mp - 1682 | * xmdf - 1683 | * xmx - 1684 | * xmy - 1685 | * nodedf - 1686 | * xnode - 1687 | * nodep - 1688 | * np - 1689 | * 1690 | * coupling : 1691 | * getgravconst- 1692 | * dpper 1693 | * dpspace 1694 | * 1695 | * references : 1696 | * hoots, roehrich, norad spacetrack report #3 1980 1697 | * hoots, norad spacetrack report #6 1986 1698 | * hoots, schumacher and glover 2004 1699 | * vallado, crawford, hujsak, kelso 2006 1700 | ----------------------------------------------------------------------------*/ 1701 | 1702 | bool sgp4 1703 | ( 1704 | gravconsttype whichconst, elsetrec& satrec, double tsince, 1705 | double r[3], double v[3] 1706 | ) 1707 | { 1708 | double am , axnl , aynl , betal , cosim , cnod , 1709 | cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, 1710 | delm , delomg, em , emsq , ecose , el2 , eo1 , 1711 | ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, 1712 | mvt , rdotl , rl , rvdot , rvdotl, sinim , 1713 | sin2u, sineo1, sini , sinip , sinsu , sinu , 1714 | snod , su , t2 , t3 , t4 , tem5 , temp, 1715 | temp1, temp2 , tempa, tempe , templ , u , ux , 1716 | uy , uz , vx , vy , vz , inclm , mm , 1717 | nm , nodem, xinc , xincp , xl , xlm , mp , 1718 | xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, 1719 | twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, 1720 | mu, vkmpersec, delmtemp; 1721 | int ktr; 1722 | 1723 | /* ------------------ set mathematical constants --------------- */ 1724 | // sgp4fix divisor for divide by zero check on inclination 1725 | // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to 1726 | // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 1727 | const double temp4 = 1.5e-12; 1728 | twopi = 2.0 * pi; 1729 | x2o3 = 2.0 / 3.0; 1730 | // sgp4fix identify constants and allow alternate values 1731 | getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1732 | vkmpersec = radiusearthkm * xke/60.0; 1733 | 1734 | /* --------------------- clear sgp4 error flag ----------------- */ 1735 | satrec.t = tsince; 1736 | satrec.error = 0; 1737 | 1738 | /* ------- update for secular gravity and atmospheric drag ----- */ 1739 | xmdf = satrec.mo + satrec.mdot * satrec.t; 1740 | argpdf = satrec.argpo + satrec.argpdot * satrec.t; 1741 | nodedf = satrec.nodeo + satrec.nodedot * satrec.t; 1742 | argpm = argpdf; 1743 | mm = xmdf; 1744 | t2 = satrec.t * satrec.t; 1745 | nodem = nodedf + satrec.nodecf * t2; 1746 | tempa = 1.0 - satrec.cc1 * satrec.t; 1747 | tempe = satrec.bstar * satrec.cc4 * satrec.t; 1748 | templ = satrec.t2cof * t2; 1749 | 1750 | if (satrec.isimp != 1) 1751 | { 1752 | delomg = satrec.omgcof * satrec.t; 1753 | // sgp4fix use mutliply for speed instead of pow 1754 | delmtemp = 1.0 + satrec.eta * cos(xmdf); 1755 | delm = satrec.xmcof * 1756 | (delmtemp * delmtemp * delmtemp - 1757 | satrec.delmo); 1758 | temp = delomg + delm; 1759 | mm = xmdf + temp; 1760 | argpm = argpdf - temp; 1761 | t3 = t2 * satrec.t; 1762 | t4 = t3 * satrec.t; 1763 | tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - 1764 | satrec.d4 * t4; 1765 | tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - 1766 | satrec.sinmao); 1767 | templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + 1768 | satrec.t * satrec.t5cof); 1769 | } 1770 | 1771 | nm = satrec.no; 1772 | em = satrec.ecco; 1773 | inclm = satrec.inclo; 1774 | if (satrec.method == 'd') 1775 | { 1776 | tc = satrec.t; 1777 | dspace 1778 | ( 1779 | satrec.irez, 1780 | satrec.d2201, satrec.d2211, satrec.d3210, 1781 | satrec.d3222, satrec.d4410, satrec.d4422, 1782 | satrec.d5220, satrec.d5232, satrec.d5421, 1783 | satrec.d5433, satrec.dedt, satrec.del1, 1784 | satrec.del2, satrec.del3, satrec.didt, 1785 | satrec.dmdt, satrec.dnodt, satrec.domdt, 1786 | satrec.argpo, satrec.argpdot, satrec.t, tc, 1787 | satrec.gsto, satrec.xfact, satrec.xlamo, 1788 | satrec.no, satrec.atime, 1789 | em, argpm, inclm, satrec.xli, mm, satrec.xni, 1790 | nodem, dndt, nm 1791 | ); 1792 | } // if method = d 1793 | 1794 | if (nm <= 0.0) 1795 | { 1796 | // printf("# error nm %f\n", nm); 1797 | satrec.error = 2; 1798 | // sgp4fix add return 1799 | return false; 1800 | } 1801 | am = pow((xke / nm),x2o3) * tempa * tempa; 1802 | nm = xke / pow(am, 1.5); 1803 | em = em - tempe; 1804 | 1805 | // fix tolerance for error recognition 1806 | // sgp4fix am is fixed from the previous nm check 1807 | if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/ ) 1808 | { 1809 | // printf("# error em %f\n", em); 1810 | satrec.error = 1; 1811 | // sgp4fix to return if there is an error in eccentricity 1812 | return false; 1813 | } 1814 | // sgp4fix fix tolerance to avoid a divide by zero 1815 | if (em < 1.0e-6) 1816 | em = 1.0e-6; 1817 | mm = mm + satrec.no * templ; 1818 | xlm = mm + argpm + nodem; 1819 | emsq = em * em; 1820 | temp = 1.0 - emsq; 1821 | 1822 | nodem = floatmod(nodem, twopi); 1823 | argpm = floatmod(argpm, twopi); 1824 | xlm = floatmod(xlm, twopi); 1825 | mm = floatmod(xlm - argpm - nodem, twopi); 1826 | 1827 | /* ----------------- compute extra mean quantities ------------- */ 1828 | sinim = sin(inclm); 1829 | cosim = cos(inclm); 1830 | 1831 | /* -------------------- add lunar-solar periodics -------------- */ 1832 | ep = em; 1833 | xincp = inclm; 1834 | argpp = argpm; 1835 | nodep = nodem; 1836 | mp = mm; 1837 | sinip = sinim; 1838 | cosip = cosim; 1839 | if (satrec.method == 'd') 1840 | { 1841 | dpper 1842 | ( 1843 | satrec.e3, satrec.ee2, satrec.peo, 1844 | satrec.pgho, satrec.pho, satrec.pinco, 1845 | satrec.plo, satrec.se2, satrec.se3, 1846 | satrec.sgh2, satrec.sgh3, satrec.sgh4, 1847 | satrec.sh2, satrec.sh3, satrec.si2, 1848 | satrec.si3, satrec.sl2, satrec.sl3, 1849 | satrec.sl4, satrec.t, satrec.xgh2, 1850 | satrec.xgh3, satrec.xgh4, satrec.xh2, 1851 | satrec.xh3, satrec.xi2, satrec.xi3, 1852 | satrec.xl2, satrec.xl3, satrec.xl4, 1853 | satrec.zmol, satrec.zmos, satrec.inclo, 1854 | 'n', ep, xincp, nodep, argpp, mp, satrec.operationmode 1855 | ); 1856 | if (xincp < 0.0) 1857 | { 1858 | xincp = -xincp; 1859 | nodep = nodep + pi; 1860 | argpp = argpp - pi; 1861 | } 1862 | if ((ep < 0.0 ) || ( ep > 1.0)) 1863 | { 1864 | // printf("# error ep %f\n", ep); 1865 | satrec.error = 3; 1866 | // sgp4fix add return 1867 | return false; 1868 | } 1869 | } // if method = d 1870 | 1871 | /* -------------------- long period periodics ------------------ */ 1872 | if (satrec.method == 'd') 1873 | { 1874 | sinip = sin(xincp); 1875 | cosip = cos(xincp); 1876 | satrec.aycof = -0.5*j3oj2*sinip; 1877 | // sgp4fix for divide by zero for xincp = 180 deg 1878 | if (fabs(cosip+1.0) > 1.5e-12) 1879 | satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); 1880 | else 1881 | satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; 1882 | } 1883 | axnl = ep * cos(argpp); 1884 | temp = 1.0 / (am * (1.0 - ep * ep)); 1885 | aynl = ep* sin(argpp) + temp * satrec.aycof; 1886 | xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; 1887 | 1888 | /* --------------------- solve kepler's equation --------------- */ 1889 | u = floatmod(xl - nodep, twopi); 1890 | eo1 = u; 1891 | tem5 = 9999.9; 1892 | ktr = 1; 1893 | // sgp4fix for kepler iteration 1894 | // the following iteration needs better limits on corrections 1895 | while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) 1896 | { 1897 | sineo1 = sin(eo1); 1898 | coseo1 = cos(eo1); 1899 | tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; 1900 | tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; 1901 | if(fabs(tem5) >= 0.95) 1902 | tem5 = tem5 > 0.0 ? 0.95 : -0.95; 1903 | eo1 = eo1 + tem5; 1904 | ktr = ktr + 1; 1905 | } 1906 | 1907 | /* ------------- short period preliminary quantities ----------- */ 1908 | ecose = axnl*coseo1 + aynl*sineo1; 1909 | esine = axnl*sineo1 - aynl*coseo1; 1910 | el2 = axnl*axnl + aynl*aynl; 1911 | pl = am*(1.0-el2); 1912 | if (pl < 0.0) 1913 | { 1914 | // printf("# error pl %f\n", pl); 1915 | satrec.error = 4; 1916 | // sgp4fix add return 1917 | return false; 1918 | } 1919 | else 1920 | { 1921 | rl = am * (1.0 - ecose); 1922 | rdotl = sqrt(am) * esine/rl; 1923 | rvdotl = sqrt(pl) / rl; 1924 | betal = sqrt(1.0 - el2); 1925 | temp = esine / (1.0 + betal); 1926 | sinu = am / rl * (sineo1 - aynl - axnl * temp); 1927 | cosu = am / rl * (coseo1 - axnl + aynl * temp); 1928 | su = atan2(sinu, cosu); 1929 | sin2u = (cosu + cosu) * sinu; 1930 | cos2u = 1.0 - 2.0 * sinu * sinu; 1931 | temp = 1.0 / pl; 1932 | temp1 = 0.5 * j2 * temp; 1933 | temp2 = temp1 * temp; 1934 | 1935 | /* -------------- update for short period periodics ------------ */ 1936 | if (satrec.method == 'd') 1937 | { 1938 | cosisq = cosip * cosip; 1939 | satrec.con41 = 3.0*cosisq - 1.0; 1940 | satrec.x1mth2 = 1.0 - cosisq; 1941 | satrec.x7thm1 = 7.0*cosisq - 1.0; 1942 | } 1943 | mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + 1944 | 0.5 * temp1 * satrec.x1mth2 * cos2u; 1945 | su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; 1946 | xnode = nodep + 1.5 * temp2 * cosip * sin2u; 1947 | xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; 1948 | mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; 1949 | rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + 1950 | 1.5 * satrec.con41) / xke; 1951 | 1952 | /* --------------------- orientation vectors ------------------- */ 1953 | sinsu = sin(su); 1954 | cossu = cos(su); 1955 | snod = sin(xnode); 1956 | cnod = cos(xnode); 1957 | sini = sin(xinc); 1958 | cosi = cos(xinc); 1959 | xmx = -snod * cosi; 1960 | xmy = cnod * cosi; 1961 | ux = xmx * sinsu + cnod * cossu; 1962 | uy = xmy * sinsu + snod * cossu; 1963 | uz = sini * sinsu; 1964 | vx = xmx * cossu - cnod * sinsu; 1965 | vy = xmy * cossu - snod * sinsu; 1966 | vz = sini * cossu; 1967 | 1968 | /* --------- position and velocity (in km and km/sec) ---------- */ 1969 | r[0] = (mrt * ux)* radiusearthkm; 1970 | r[1] = (mrt * uy)* radiusearthkm; 1971 | r[2] = (mrt * uz)* radiusearthkm; 1972 | v[0] = (mvt * ux + rvdot * vx) * vkmpersec; 1973 | v[1] = (mvt * uy + rvdot * vy) * vkmpersec; 1974 | v[2] = (mvt * uz + rvdot * vz) * vkmpersec; 1975 | } // if pl > 0 1976 | 1977 | // sgp4fix for decaying satellites 1978 | if (mrt < 1.0) 1979 | { 1980 | // printf("# decay condition %11.6f \n",mrt); 1981 | satrec.error = 6; 1982 | return false; 1983 | } 1984 | 1985 | //#include "debug7.cpp" 1986 | return true; 1987 | } // end sgp4 1988 | 1989 | 1990 | /* ----------------------------------------------------------------------------- 1991 | * 1992 | * function gstime 1993 | * 1994 | * this function finds the greenwich sidereal time. 1995 | * 1996 | * author : david vallado 719-573-2600 1 mar 2001 1997 | * 1998 | * inputs description range / units 1999 | * jdut1 - julian date in ut1 days from 4713 bc 2000 | * 2001 | * outputs : 2002 | * gstime - greenwich sidereal time 0 to 2pi rad 2003 | * 2004 | * locals : 2005 | * temp - temporary variable for doubles rad 2006 | * tut1 - julian centuries from the 2007 | * jan 1, 2000 12 h epoch (ut1) 2008 | * 2009 | * coupling : 2010 | * none 2011 | * 2012 | * references : 2013 | * vallado 2004, 191, eq 3-45 2014 | * --------------------------------------------------------------------------- */ 2015 | 2016 | double gstime 2017 | ( 2018 | double jdut1 2019 | ) 2020 | { 2021 | const double twopi = 2.0 * pi; 2022 | const double deg2rad = pi / 180.0; 2023 | double temp, tut1; 2024 | 2025 | tut1 = (jdut1 - 2451545.0) / 36525.0; 2026 | temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + 2027 | (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec 2028 | temp = floatmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad 2029 | 2030 | // ------------------------ check quadrants --------------------- 2031 | if (temp < 0.0) 2032 | temp += twopi; 2033 | 2034 | return temp; 2035 | } // end gstime 2036 | 2037 | 2038 | /* ----------------------------------------------------------------------------- 2039 | * 2040 | * function getgravconst 2041 | * 2042 | * this function gets constants for the propagator. note that mu is identified to 2043 | * facilitiate comparisons with newer models. the common useage is wgs72. 2044 | * 2045 | * author : david vallado 719-573-2600 21 jul 2006 2046 | * 2047 | * inputs : 2048 | * whichconst - which set of constants to use wgs72old, wgs72, wgs84 2049 | * 2050 | * outputs : 2051 | * tumin - minutes in one time unit 2052 | * mu - earth gravitational parameter 2053 | * radiusearthkm - radius of the earth in km 2054 | * xke - reciprocal of tumin 2055 | * j2, j3, j4 - un-normalized zonal harmonic values 2056 | * j3oj2 - j3 divided by j2 2057 | * 2058 | * locals : 2059 | * 2060 | * coupling : 2061 | * none 2062 | * 2063 | * references : 2064 | * norad spacetrack report #3 2065 | * vallado, crawford, hujsak, kelso 2006 2066 | --------------------------------------------------------------------------- */ 2067 | 2068 | void getgravconst 2069 | ( 2070 | gravconsttype whichconst, 2071 | double& tumin, 2072 | double& mu, 2073 | double& radiusearthkm, 2074 | double& xke, 2075 | double& j2, 2076 | double& j3, 2077 | double& j4, 2078 | double& j3oj2 2079 | ) 2080 | { 2081 | 2082 | switch (whichconst) 2083 | { 2084 | // -- wgs-72 low precision str#3 constants -- 2085 | case wgs72old: 2086 | mu = 398600.79964; // in km3 / s2 2087 | radiusearthkm = 6378.135; // km 2088 | xke = 0.0743669161; 2089 | tumin = 1.0 / xke; 2090 | j2 = 0.001082616; 2091 | j3 = -0.00000253881; 2092 | j4 = -0.00000165597; 2093 | j3oj2 = j3 / j2; 2094 | break; 2095 | // ------------ wgs-72 constants ------------ 2096 | case wgs72: 2097 | mu = 398600.8; // in km3 / s2 2098 | radiusearthkm = 6378.135; // km 2099 | xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); 2100 | tumin = 1.0 / xke; 2101 | j2 = 0.001082616; 2102 | j3 = -0.00000253881; 2103 | j4 = -0.00000165597; 2104 | j3oj2 = j3 / j2; 2105 | break; 2106 | case wgs84: 2107 | // ------------ wgs-84 constants ------------ 2108 | mu = 398600.5; // in km3 / s2 2109 | radiusearthkm = 6378.137; // km 2110 | xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); 2111 | tumin = 1.0 / xke; 2112 | j2 = 0.00108262998905; 2113 | j3 = -0.00000253215306; 2114 | j4 = -0.00000161098761; 2115 | j3oj2 = j3 / j2; 2116 | break; 2117 | default: 2118 | //fprintf(stderr,"unknown gravity option (%d)\n",whichconst); 2119 | break; 2120 | } 2121 | 2122 | } // end getgravconst 2123 | 2124 | 2125 | 2126 | 2127 | 2128 | -------------------------------------------------------------------------------- /src/sgp4unit.h: -------------------------------------------------------------------------------- 1 | #ifndef _sgp4unit_ 2 | #define _sgp4unit_ 3 | /* ---------------------------------------------------------------- 4 | * 5 | * sgp4unit.h 6 | * 7 | * this file contains the sgp4 procedures for analytical propagation 8 | * of a satellite. the code was originally released in the 1980 and 1986 9 | * spacetrack papers. a detailed discussion of the theory and history 10 | * may be found in the 2006 aiaa paper by vallado, crawford, hujsak, 11 | * and kelso. 12 | * 13 | * companion code for 14 | * fundamentals of astrodynamics and applications 15 | * 2007 16 | * by david vallado 17 | * 18 | * (w) 719-573-2600, email dvallado@agi.com 19 | * 20 | * current : 21 | * 30 Dec 11 david vallado 22 | * consolidate updated code version 23 | * 30 Aug 10 david vallado 24 | * delete unused variables in initl 25 | * replace pow inetger 2, 3 with multiplies for speed 26 | * changes : 27 | * 3 Nov 08 david vallado 28 | * put returns in for error codes 29 | * 29 sep 08 david vallado 30 | * fix atime for faster operation in dspace 31 | * add operationmode for afspc (a) or improved (i) 32 | * performance mode 33 | * 20 apr 07 david vallado 34 | * misc fixes for constants 35 | * 11 aug 06 david vallado 36 | * chg lyddane choice back to strn3, constants, misc doc 37 | * 15 dec 05 david vallado 38 | * misc fixes 39 | * 26 jul 05 david vallado 40 | * fixes for paper 41 | * note that each fix is preceded by a 42 | * comment with "sgp4fix" and an explanation of 43 | * what was changed 44 | * 10 aug 04 david vallado 45 | * 2nd printing baseline working 46 | * 14 may 01 david vallado 47 | * 2nd edition baseline 48 | * 80 norad 49 | * original baseline 50 | * ---------------------------------------------------------------- */ 51 | 52 | #include 53 | #include 54 | #define SGP4Version "SGP4 Version 2011-12-30" 55 | 56 | #define pi 3.14159265358979323846 57 | 58 | // -------------------------- structure declarations ---------------------------- 59 | typedef enum 60 | { 61 | wgs72old, 62 | wgs72, 63 | wgs84 64 | } gravconsttype; 65 | 66 | typedef struct elsetrec 67 | { 68 | long int satnum; 69 | int epochyr, epochtynumrev; 70 | int error; 71 | char operationmode; 72 | char init, method; 73 | 74 | /* Near Earth */ 75 | int isimp; 76 | double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , 77 | delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , 78 | t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , 79 | nodecf; 80 | 81 | /* Deep Space */ 82 | int irez; 83 | double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , 84 | d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , 85 | dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , 86 | plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , 87 | si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , 88 | xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , 89 | xl4 , xlamo , zmol , zmos , atime , xli , xni; 90 | 91 | double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , 92 | bstar , rcse , inclo , nodeo , ecco , argpo , mo , 93 | no; 94 | } elsetrec; 95 | 96 | 97 | // --------------------------- function declarations ---------------------------- 98 | bool sgp4init 99 | ( 100 | gravconsttype whichconst, char opsmode, const int satn, const double epoch, 101 | const double xbstar, const double xecco, const double xargpo, 102 | const double xinclo, const double xmo, const double xno, 103 | const double xnodeo, elsetrec& satrec 104 | ); 105 | 106 | bool sgp4 107 | ( 108 | gravconsttype whichconst, elsetrec& satrec, double tsince, 109 | double r[3], double v[3] 110 | ); 111 | 112 | double gstime 113 | ( 114 | double jdut1 115 | ); 116 | 117 | void getgravconst 118 | ( 119 | gravconsttype whichconst, 120 | double& tumin, 121 | double& mu, 122 | double& radiusearthkm, 123 | double& xke, 124 | double& j2, 125 | double& j3, 126 | double& j4, 127 | double& j3oj2 128 | ); 129 | 130 | 131 | #endif 132 | 133 | -------------------------------------------------------------------------------- /src/visible.cpp: -------------------------------------------------------------------------------- 1 | #include "visible.h" 2 | #include 3 | 4 | #define sunradius 695500 //km 5 | #define earthradius 6378.137 //km 6 | #define au 149597871 //km 7 | 8 | /* 9 | This file contains miscellaneous functions required for calculating visible satellites. 10 | Some functions were originally written for Matlab as companion code for "Fundamentals of Astrodynamics 11 | and Applications" by David Vallado (2007). (w) 719-573-2600, email dvallado@agi.com 12 | 13 | Ported to C++ by Hopperpop with some modifications, November 2015. 14 | */ 15 | 16 | /* 17 | % ------------------------------------------------------------------------------ 18 | % 19 | % function sun 20 | % 21 | % this function calculates the geocentric equatorial position vector 22 | % the sun given the julian date. this is the low precision formula and 23 | % is valid for years from 1950 to 2050. accuaracy of apparent coordinates 24 | % is 0.01 degrees. notice many of the calculations are performed in 25 | % degrees, and are not changed until later. this is due to the fact that 26 | % the almanac uses degrees exclusively in their formulations. 27 | % 28 | % author : david vallado 719-573-2600 27 may 2002 29 | % 30 | % revisions 31 | % vallado - fix mean lon of sun 7 mat 2004 32 | % 33 | % inputs description range / units 34 | % jdC - julian date days from 4713 bc 35 | % 36 | % outputs : 37 | % rsun - ijk position vector of the sun km 38 | 39 | % locals : 40 | % meanlong - mean longitude 41 | % meananomaly - mean anomaly 42 | % eclplong - ecliptic longitude 43 | % obliquity - mean obliquity of the ecliptic 44 | % tut1 - julian centuries of ut1 from 45 | % jan 1, 2000 12h 46 | % ttdb - julian centuries of tdb from 47 | % jan 1, 2000 12h 48 | % hr - hours 0 .. 24 10 49 | % min - minutes 0 .. 59 15 50 | % sec - seconds 0.0 .. 59.99 30.00 51 | % temp - temporary variable 52 | % deg - degrees 53 | % 54 | % coupling : 55 | % none. 56 | % 57 | % references : 58 | % vallado 2007, 281, alg 29, ex 5-1 59 | % 60 | % [rsun,rtasc,decl] = sun ( jdC ); 61 | % ------------------------------------------------------------------------------ 62 | */ 63 | 64 | void sun ( double jd, double rsun[3] ){ 65 | 66 | double twopi = 2.0*pi; 67 | double deg2rad = pi/180.0; 68 | 69 | // ------------------------- implementation ----------------- 70 | // ------------------- initialize values -------------------- 71 | double tut1 = ( jd - 2451545.0 )/ 36525.0; 72 | 73 | double meanlong = 280.460 + 36000.77 * tut1; 74 | meanlong = floatmod( meanlong, 360.0 ); //deg 75 | 76 | double meananomaly = 357.5277233 + 35999.05034 * tut1; 77 | meananomaly = floatmod( meananomaly*deg2rad,twopi ); //rad 78 | if ( meananomaly < 0.0 ){ 79 | meananomaly= twopi + meananomaly; 80 | } 81 | 82 | double eclplong= meanlong + 1.914666471 * sin(meananomaly)+ 0.019994643 * sin(2.0 *meananomaly); //deg 83 | eclplong= floatmod( eclplong, 360.0 ); //deg 84 | 85 | double obliquity= 23.439291 - 0.0130042 * tut1; //deg 86 | 87 | eclplong = eclplong *deg2rad; 88 | obliquity = obliquity *deg2rad; 89 | 90 | // --------- find magnitude of sun vector, ) components ------ 91 | double magr= 1.000140612 - 0.016708617 *cos( meananomaly ) - 0.000139589 *cos( 2.0 *meananomaly ); // in au's 92 | 93 | rsun[0]= magr*cos( eclplong )*au; 94 | rsun[1]= magr*cos(obliquity)*sin(eclplong)*au; 95 | rsun[2]= magr*sin(obliquity)*sin(eclplong)*au; 96 | 97 | } 98 | 99 | //returns angle between sun surface and earth surface, from the viewpoint of the satellite 100 | double Sgp4::visiblewrap(double jdCe) { 101 | double rsun[3]; //vector between earth and sun 102 | double razell[3]; 103 | 104 | sun(jdC, rsun); //calculate sun poistion vector 105 | rv2azel(rsun, siteLatRad, siteLonRad, siteAlt, jdC, razell); //calc sun satEl 106 | 107 | double rsunsat[3]; //vector between sat and sun 108 | double rearth[3]; 109 | double magsunsat, magearth; 110 | double phiearth, phisun, phi; 111 | double rnomearth, rnomsun; 112 | 113 | sgp4wrap(jdCe); 114 | 115 | rearth[0] = -ro[0]; 116 | rearth[1] = -ro[1]; 117 | rearth[2] = -ro[2]; 118 | 119 | rsunsat[0] = rsun[0] + rearth[0]; 120 | rsunsat[1] = rsun[1] + rearth[1]; 121 | rsunsat[2] = rsun[2] + rearth[2]; 122 | 123 | magsunsat = mag(rsunsat); 124 | magearth = mag(rearth); 125 | 126 | phiearth = asin(earthradius / magearth); 127 | phisun = asin(sunradius / magsunsat); 128 | 129 | phi = acos(dot(rearth, rsunsat) / magsunsat / magearth); 130 | 131 | return phi - phisun - phiearth; ///grens op bijschaduw 132 | //return -phiearth + phisun + phi; ///grens op echte schaduw 133 | 134 | } 135 | 136 | 137 | //calculate if satellite is visible 138 | int16_t Sgp4::visible(bool& notdark, double& deltaphi){ 139 | 140 | double rsun[3]; //vector between earth and sun 141 | double razell[3]; 142 | 143 | sun(jdC, rsun); //calculate sun poistion vector 144 | rv2azel(rsun, siteLatRad, siteLonRad, siteAlt, jdC, razell); //calc sun satEl 145 | 146 | sunEl = razell[2] * 180 / pi; 147 | sunAz = razell[1] * 180 / pi; 148 | notdark = (razell[2] > sunoffset); //sun aboven -6° => not dark enough 149 | 150 | double rsunsat[3]; //vector between sat and sun 151 | double rearth[3]; 152 | double magsunsat, magearth; 153 | double phiearth, phisun, phi; 154 | double rnomearth, rnomsun; 155 | 156 | rearth[0] = -ro[0]; 157 | rearth[1] = -ro[1]; 158 | rearth[2] = -ro[2]; 159 | 160 | rsunsat[0] = rsun[0] + rearth[0]; 161 | rsunsat[1] = rsun[1] + rearth[1]; 162 | rsunsat[2] = rsun[2] + rearth[2]; 163 | 164 | magsunsat = mag(rsunsat); 165 | magearth = mag(rearth); 166 | 167 | phiearth = asin(earthradius/magearth); 168 | phisun = asin(sunradius/magsunsat); 169 | 170 | phi = acos(dot(rearth,rsunsat)/magsunsat/magearth); 171 | deltaphi = phi - phisun - phiearth; 172 | 173 | if (phiearth > phisun && phi < phiearth - phisun){ //umbral eclipse 174 | return 0; 175 | } 176 | 177 | if (phiearth < phisun && phi < phisun - phiearth){ //partial eclipse 178 | return (int16_t)(( 1 - phiearth*phiearth/(phisun*phisun) )*1000); //approach to the surface coverage with a square sun and earth 179 | } 180 | 181 | if (fabs(phiearth-phisun) < phi && phi < phiearth + phisun){ //penumbral eclipse 182 | if (phiearth > phisun){ 183 | return (int16_t)(( (phisun+phi-phiearth)/(phisun*2.0) )*1000); //approach to the surface coverage with a square sun and earth 184 | }else{ 185 | return (int16_t)(( 1 - phiearth*(phisun - phi + phiearth)/(2.0*phisun*phisun) )*1000); //approach to the surface coverage with a square sun and earth 186 | } 187 | } 188 | 189 | return 1000; //no eclipse => visible 190 | 191 | } 192 | 193 | int16_t Sgp4::visible() { 194 | bool notdark; 195 | double deltaphi; 196 | int16_t viss = visible(notdark,deltaphi); 197 | if (notdark) { 198 | return -1; 199 | } 200 | else { 201 | return viss; 202 | } 203 | } 204 | -------------------------------------------------------------------------------- /src/visible.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file contains miscellaneous functions required for calculating visible satellits. 3 | Functions were originally written for Matlab as companion code for "Fundamentals of Astrodynamics 4 | and Applications" by David Vallado (2007). (w) 719-573-2600, email dvallado@agi.com 5 | 6 | Ported to C++ by Bram Gurdebeke with some modifications, November 2015. 7 | */ 8 | 9 | #ifndef _visible_ 10 | #define _visible_ 11 | 12 | #include "sgp4unit.h" 13 | #include "sgp4ext.h" 14 | #include "sgp4pred.h" 15 | #include 16 | 17 | void sun ( double jd, double rsun[3] ); //calculates sun coordinates 18 | 19 | 20 | #endif 21 | --------------------------------------------------------------------------------