├── .github └── workflows │ └── build.yml ├── Makefile ├── README.md ├── attic └── ba3010e.txt ├── checkProg.c ├── checkProgSimple.c ├── csgp4.h ├── csgp4_simple.h ├── os_generic.h ├── spacestations.txt └── trackonly.c /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build Test 2 | on: 3 | push: 4 | pull_request: 5 | jobs: 6 | Build-for-Linux: 7 | permissions: 8 | statuses: write 9 | runs-on: ubuntu-latest 10 | env: 11 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 12 | steps: 13 | - uses: actions/checkout@v4 14 | with: 15 | ref: ${{ github.event.pull_request.head.sha }} 16 | submodules: recursive 17 | - name: Install more dependencies 18 | run: | 19 | sudo apt-get install -y \ 20 | make \ 21 | build-essential 22 | - name: Build 23 | run: make clean test 24 | 25 | 26 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | all : test 2 | 3 | ifeq ($(GITHUB_TOKEN),) 4 | define GH_ADDSTATUS 5 | true 6 | endef 7 | else 8 | define GH_ADDSTATUS 9 | curl -L --silent --output /dev/null \ 10 | -X POST \ 11 | -H "Accept: application/vnd.github+json" \ 12 | -H "Authorization: Bearer ${GITHUB_TOKEN}" \ 13 | -H "X-GitHub-Api-Version: 2022-11-28" \ 14 | https://api.github.com/repos/${GITHUB_REPOSITORY}/statuses/${GITHUB_WORKFLOW_SHA} \ 15 | -d '{"state":$(2),"context":$(1),"target_url":"https://github.com/${GITHUB_REPOSITORY}/actions/runs/${GITHUB_RUN_ID}"}' 16 | endef 17 | # Need context and state: (error, failure, pending, success) 18 | # Can have target_url, description 19 | # Can also do something like ( (cat status.txt | tr '\n' '
' > content.txt); (echo '{"state":$(2),"context":$(1)}' | jq --rawfile content content.txt '."description" |= $$content' > payload.json);, and -d '@payload.json' 20 | endif 21 | 22 | checkProg : checkProg.c csgp4.h 23 | gcc -g -Os -flto -o $@ $< -lm 24 | objdump -S $@ > $@.lst 25 | 26 | checkProg.float : checkProg.c 27 | gcc -g -Os -flto -o $@ $< -lm -DCSGP4_USE_FLOAT=1 28 | objdump -S $@ > $@.lst 29 | 30 | checkProgSimple : checkProgSimple.c 31 | gcc -g -Os -flto -o $@ $< -lm -DCSGP4_USE_FLOAT=1 -pedantic -Wall 32 | objdump -S $@ > $@.lst 33 | 34 | trackonly : trackonly.c csgp4.h 35 | gcc -g -Os -flto -o $@ $< -lm 36 | 37 | spacestations.txt : 38 | wget "https://celestrak.org/NORAD/elements/gp.php?GROUP=stations&FORMAT=tle" -O spacestations.txt 39 | 40 | test : checkProg spacestations.txt trackonly checkProg.float checkProgSimple 41 | (./checkProg spacestations.txt && $(call GH_ADDSTATUS,"Double precision","success")) || $(call GH_ADDSTATUS,"Double precision","failure") 42 | (./checkProg.float spacestations.txt && $(call GH_ADDSTATUS,"Single precision","success")) || $(call GH_ADDSTATUS,"Single precision","failure") 43 | (./checkProgSimple && $(call GH_ADDSTATUS,"Simple Test","success")) || $(call GH_ADDSTATUS,"Simple Test","failure") 44 | size checkProg checkProg.float checkProgSimple 45 | 46 | clean : 47 | rm -rf *.o *~ checkProg trackonly checkProg.float checkProgSimple 48 | 49 | 50 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # csgp4 2 | 3 | C, header-only port of David Vallado's SGP4Lib, for use in embedded, and unusual situations. 4 | 5 | Approximate values per-core on a AMD 5800X: 6 | 7 | | Operation | Run Time | 8 | | --- | --- | 9 | | Init | 0.3993 us/iteration | 10 | | Run | 0.2270 us/iteration | 11 | | Init and Run (ISS) | 0.4059 us/iteration | 12 | | Init and Run (Beyond Geostationary) | 1.0818 us/iteration | 13 | 14 | This is kind of rough, so USE AT YOUR OWN RISK. All I've done to validate is that the orbital tracks betwen this and the Python SGP4 lib come up similiarly. 15 | 16 | ## How to use 17 | 18 | Check both [checkProg.c](https://github.com/cnlohr/csgp4/blob/master/checkProg.c) and [checkProgSimple.c](https://github.com/cnlohr/csgp4/blob/master/checkProgSimple.c) for general usage. More detailed information follows: 19 | 20 | ### How to use csgp4.h 21 | 22 | Just `#include csgp4.h` at the top of your program, from there... 23 | 24 | The idea is that you can either read directly from a TLE file, which is acquirable from celestrak, you can see general usage here https://github.com/cnlohr/csgp4/blob/master/checkProg.c#L8-L29 - but in general, you open the file, and pass it to `ParseFileOrString`. With the same function, you can also also pass in a string containing the TLEs you want to load instead. 25 | 26 | Once the TLEs are loaded, you can use `sgp4` to compute the position of your desired satellite at the desired time. 27 | 28 | Output is in the TEME (Earth Intertial Frame), and input times are the relative time difference between the epoch and the time in seconds. 29 | 30 | ### How to use csgp4_simple.h 31 | 32 | csgp4_simple.h is a lot like csgp4.h, however, it assumes you have already parsed the TLE records, and it also just computes the object's position at a time and does not initialize. It is geared for situations where you can already pull an object's TLE. For instance, on a microcontroller pulling data from a server that already runs the full csgp4 processing, or on the GPU. 33 | 34 | ## Floating point performance 35 | 36 | We also test single precision floating point performance, and in general, over the course of a day for most satellites, it is off by a few meters, or up to 5 or 6 km over the course of a month, as compared to double precision. 37 | 38 | Care should be taken surroinding the date format sent in. In many places in code, we split times into "days" and "fractional days", i.e. `jd` and `jdfrac` (Julian days being days since 4713 bc). Only once you have subtracted your epoch and this time should you squish the values together into one float. This is because when floats become very large, their precision for handling small numbers gets worse and worse. For instance, at 8 million, you can discern 8,000,000 and 8,000,000.5 but, at 17,000,000, 17,000,000.5 just resolves to 17,000,000. 39 | 40 | ## TODO 41 | * Figure out why Deep Space is off some. 42 | * Can we avoid extra call to sgp4 in sgp4init? 43 | * Is meanMotion1, meanMotion2 used at all? Can we just remove them? (and ndot) 44 | * Make it so `sgp4init_simple` can take epoch as epocf and epoch fraction. 45 | 46 | 47 | ## Resources 48 | 49 | * SGP4 Transliterated from https://celestrak.org/software/vallado-sw.php, specifically https://celestrak.org/software/vallado/cs.zip 50 | * https://en.wikipedia.org/wiki/Two-line_element_set 51 | * https://spaceaware.io/ 52 | * https://cdn.digitalarsenal.io/celestrak/NORAD/elements/catalog.txt 53 | * https://celestrak.org/NORAD/elements/ 54 | * Space Stations: https://celestrak.org/NORAD/elements/gp.php?GROUP=stations&FORMAT=tle 55 | * https://en.wikipedia.org/wiki/Julian_day 56 | 57 | -------------------------------------------------------------------------------- /attic/ba3010e.txt: -------------------------------------------------------------------------------- 1 | ./checkProg spacestations.txt 2 | Read 32 objects. 3 | ISS (ZARYA) 1714236293.509056 4 | CSS (TIANHE) 1714214059.362432 5 | ISS (NAUKA) 1714218099.022080 6 | FREGAT DEB 1714202211.709728 7 | CSS (WENTIAN) 1714214059.362432 8 | CSS (MENGTIAN) 1714214059.362432 9 | ISS DEB [SPX-28 IPA FSE] 1714222096.388928 10 | SHENZHOU-17 (SZ-17) 1714214059.362432 11 | ISS DEB 1714218139.850400 12 | PROGRESS-MS 25 1714218099.022080 13 | BEAK 1714217372.575200 14 | TIANZHOU-7 1714214059.362432 15 | CYGNUS NG-20 1714218099.022080 16 | PROGRESS-MS 26 1714218099.022080 17 | CREW DRAGON 8 1714218099.022080 18 | DRAGON CRS-30 1714218099.022080 19 | SOYUZ-MS 25 1714218099.022080 20 | MICROORBITER-1 1714221937.009440 21 | CURTIS 1714222093.490208 22 | KASHIWA 1714227446.803104 23 | 1998-067WJ 1714222966.152672 24 | 1998-067WK 1714222774.128672 25 | 1998-067WL 1714223264.394240 26 | 1998-067WM 1714223058.877152 27 | 1998-067WN 1714223145.530304 28 | SHENZHOU-18 (SZ-18) 1714103626.172544 29 | CZ-2F R/B 1714243160.121408 30 | CZ-2F DEB 1714103801.938656 31 | CZ-2F DEB 1714131083.702016 32 | CZ-2F DEB 1714163330.972736 33 | 1998-067WP 1714222884.053664 34 | 1998-067WQ 1714222882.050048 35 | 2460428.197842 0.052158 2024 04 27 17:59:60.00 /[ 75.10818239] -4582.66496156 -4356.87495057 2475.14722152 [6790.399933] 5.036095087 -2.259127911 5.318856346 36 | Python / Our SGP4 Disagreement: -0.000242 0.000152 -0.000179 RMS: 0.000337 km 37 | PASS 38 | Python / Our SGP4 Disagreement: -0.000000 -0.000000 0.000000 RMS: 0.000000 km 39 | PASS 40 | Init: 0.493773 us/iteration 41 | Run: 0.224904 us/iteration 42 | -------------------------------------------------------------------------------- /checkProg.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "os_generic.h" 3 | 4 | #include "csgp4.h" 5 | 6 | int main( int argc, char ** argv ) 7 | { 8 | if( argc != 2 ) 9 | { 10 | fprintf( stderr, "Error: Usage: checkProg [.txt TLE file]\n" ); 11 | return -6; 12 | } 13 | 14 | FILE * f = fopen( argv[1], "r" ); 15 | if( !f || ferror( f ) ) 16 | { 17 | fprintf( stderr, "Error: could not open %s\n", argv[1] ); 18 | return -6; 19 | } 20 | 21 | struct TLEObject * obj = 0; 22 | int numObjects = 0; 23 | int r = ParseFileOrString( f, 0, &obj, &numObjects ); 24 | if( r ) 25 | { 26 | fprintf( stderr, "Error: Parsing failed\n" ); 27 | return r; 28 | } 29 | printf( "Read %d objects.\n", numObjects ); 30 | if( numObjects < 1 ) 31 | { 32 | fprintf( stderr, "Did not read any objects\n" ); 33 | return -4; 34 | } 35 | int i; 36 | for( i = 0; i < numObjects; i++ ) 37 | { 38 | struct TLEObject * o = &obj[i]; 39 | //double diff = OGGetAbsoluteTime() - o->epoch; 40 | printf( "%24s %f\n", o->objectName, o->epoch );//, diff, OGGetAbsoluteTime(), o->epoch ); 41 | } 42 | 43 | 44 | 45 | 46 | if( 1 ) 47 | { 48 | struct TLEObject * o = &obj[0]; 49 | 50 | // Let's just pick out ISS (Zarya) 51 | struct elsetrec satrec; 52 | 53 | if( ConvertTLEToSGP4( &satrec, o, 0, 0, 0 ) ) 54 | { 55 | printf( "failed to convert tle\n" ); 56 | return -5; 57 | } 58 | 59 | // set start/stop times for propagation, in minutes. 60 | double startmfe = (OGGetAbsoluteTime() - o->epoch)/60.0; // Convert to minutes. 61 | double stopmfe = startmfe + 45.0; 62 | double deltamin = 1.0; 63 | 64 | double tsince = startmfe; 65 | while ((tsince < stopmfe) && (satrec.error == 0)) 66 | { 67 | SGPF ro[3], vo[3]; 68 | 69 | if(tsince > stopmfe) 70 | tsince = stopmfe; 71 | 72 | // .25us per call 73 | sgp4 (&satrec, tsince, ro, vo); 74 | #if 0 75 | double jd = satrec.jdsatepoch + satrec.jdsatepochF; 76 | double jdfrac = tsince/1440.0; 77 | int year, mon, day, hr, min; 78 | double sec; 79 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 80 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f /", jd, jdfrac, year, mon, day, hr, min, sec ); 81 | #endif 82 | printf( "%16.8f %16.8f %16.8f %16.8f [%f] %12.9f %12.9f %12.9f\n", 83 | tsince,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2]); 84 | 85 | tsince = tsince + deltamin; 86 | } 87 | } 88 | 89 | double pysgp4[3] = { -4582.664719456509, -4356.875102968861, 2475.1474001054107 }; 90 | double pysgp4v[3] = { 5.036095414394779, -2.2591278380385664, 5.3188560672302145 }; 91 | SGPF ro[3], vo[3]; 92 | double rmse; 93 | 94 | if( 1 ) 95 | { 96 | struct TLEObject * ss = 0; 97 | struct elsetrec iss; 98 | int numSS = 0; 99 | r = ParseFileOrString( 0, "" 100 | "ISS (ZARYA) \n" 101 | "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995\n" 102 | "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710\n", 103 | &ss, &numSS ); 104 | if( r || numSS != 1 ) 105 | { 106 | fprintf( stderr, "Error: SS can't load.\n" ); 107 | return -5; 108 | } 109 | if( ConvertTLEToSGP4( &iss, &ss[0], 0, 0, 0 ) ) 110 | { 111 | printf( "failed to convert tle\n" ); 112 | return -5; 113 | } 114 | puts( ss->objectName ); 115 | double startmfe = (1714240800 - ss->epoch)/60.0; 116 | sgp4 (&iss, startmfe, ro, vo); 117 | 118 | SGPF jd = ss->jdsatepoch + floor(startmfe/1440.0); 119 | SGPF jdfrac = fmod(startmfe/1440.0, 1.0) + ss->jdsatepochF; 120 | int year, mon, day, hr, min; 121 | SGPF sec; 122 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 123 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 124 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n", 125 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2])); 126 | 127 | /* 128 | These are from the following python code: 129 | 130 | from sgp4.api import jday 131 | from sgp4.api import Satrec 132 | s = "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995" 133 | t = "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710" 134 | satellite = Satrec.twoline2rv(s, t) 135 | jd, fr = jday(2024, 4, 27, 18, 0, 0) 136 | e, r, v = satellite.sgp4(jd, fr) 137 | >>> r 138 | (-4582.664719456509, -4356.875102968861, 2475.1474001054107) 139 | >>> v 140 | (5.036095414394779, -2.2591278380385664, 5.3188560672302145) 141 | 142 | For some reason, they disagree withh these coords by a few km. 143 | From https://nasa-public-data.s3.amazonaws.com/iss-coords/current/ISS_OEM/ISS.OEM_J2K_EPH.txt 144 | 2024-04-27T18:00:00.000 -4587.597832963610 -4337.824413854870 2498.905090951730 5.05095945730461 -2.27273088144798 5.29906134757423 145 | 1714240800 = Sat Apr 27 2024 18:00:00 GMT+0000 146 | */ 147 | pysgp4[0] = -4582.664719456509; 148 | pysgp4[1] = -4356.875102968861; 149 | pysgp4[2] = 2475.1474001054107; 150 | pysgp4v[0] = 5.036095414394779; 151 | pysgp4v[1] = -2.2591278380385664; 152 | pysgp4v[2] = 5.3188560672302145; 153 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 154 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 155 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 156 | rmse ); 157 | if( rmse < 0.005 ) 158 | { 159 | printf( "PASS\n" ); 160 | } 161 | else 162 | { 163 | printf( "FAIL\n" ); 164 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 165 | return -5; 166 | } 167 | 168 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 169 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 170 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 171 | vrmse ); 172 | if( vrmse < 0.00005 ) 173 | { 174 | printf( "PASS\n" ); 175 | } 176 | else 177 | { 178 | printf( "FAIL\n" ); 179 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed. Fail\n" ); 180 | return -6; 181 | } 182 | 183 | /* And forwarded out a day... 184 | jd, fr = jday(2024, 4, 28, 18, 0, 0) 185 | e, r, v = satellite.sgp4(jd, fr) 186 | >>> r 187 | (-5357.955394050937, -2614.4147285937256, 3236.6540120050795) 188 | >>> v 189 | (0.04217831766517118, -6.0043223779983865, -4.77009742984791) 190 | */ 191 | } 192 | 193 | if( 1 ) 194 | { 195 | struct TLEObject * ss = 0; 196 | struct elsetrec iss; 197 | int numSS = 0; 198 | r = ParseFileOrString( 0, "" 199 | "ISS (ZARYA) \n" 200 | "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995\n" 201 | "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710\n", 202 | &ss, &numSS ); 203 | if( r || numSS != 1 ) 204 | { 205 | fprintf( stderr, "Error: SS can't load.\n" ); 206 | return -5; 207 | } 208 | if( ConvertTLEToSGP4( &iss, &ss[0], 0, 0, 0 ) ) 209 | { 210 | printf( "failed to convert tle\n" ); 211 | return -5; 212 | } 213 | puts( ss->objectName ); 214 | double startmfe = (1714327200 - ss->epoch)/60.0; 215 | sgp4 (&iss, startmfe, ro, vo); 216 | 217 | double jd = ss->jdsatepoch + floor(startmfe/1440.0); 218 | double jdfrac = fmod(startmfe/1440.0, 1.0) + ss->jdsatepochF; 219 | int year, mon, day, hr, min; 220 | SGPF sec; 221 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 222 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 223 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n", 224 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2])); 225 | 226 | pysgp4[0] = 4435.874337686209; 227 | pysgp4[1] = 4191.117631955163; 228 | pysgp4[2] = -2991.3331751931737; 229 | pysgp4v[0] = -5.401088744185615; 230 | pysgp4v[1] = 2.177125902892223; 231 | pysgp4v[2] = -4.972896867609246; 232 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 233 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 234 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 235 | rmse ); 236 | if( rmse < 0.005 + CSGP4_USE_FLOAT * .05 ) 237 | { 238 | printf( "PASS\n" ); 239 | } 240 | else 241 | { 242 | printf( "FAIL\n" ); 243 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 244 | return -5; 245 | } 246 | 247 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 248 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 249 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 250 | vrmse ); 251 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .001 ) 252 | { 253 | printf( "PASS\n" ); 254 | } 255 | else 256 | { 257 | printf( "FAIL\n" ); 258 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 259 | return -6; 260 | } 261 | } 262 | 263 | 264 | // Forward a day, but, with Arase, a higher orbit satellite, ALSO TEST wth only init. 265 | /* 266 | from sgp4.api import jday 267 | from sgp4.api import Satrec 268 | s = "1 41896U 16080A 24120.85435253 .00000361 00000+0 77632-3 0 9994" 269 | t = "2 41896 31.9360 317.6417 7003302 291.8988 10.4729 2.55468020 68280" 270 | satellite = Satrec.twoline2rv(s, t) 271 | jd, fr = jday(2024, 5, 2, 18, 0, 0) 272 | e, r, v = satellite.sgp4(jd, fr) 273 | >>> r 274 | (13105.747459057653, 29909.425386662235, 19138.1545898884) 275 | >>> v 276 | (-1.3853033635228822, 1.227640540353242, -0.039183565331240844) 277 | */ 278 | if( 1 ) 279 | { 280 | struct TLEObject * ss_arase = 0; 281 | struct elsetrec iss_arase; 282 | int numSS_arase = 0; 283 | r = ParseFileOrString( 0, "" 284 | "ARASE (ERG) \n" 285 | "1 41896U 16080A 24120.85435253 .00000361 00000+0 77632-3 0 9994\n" 286 | "2 41896 31.9360 317.6417 7003302 291.8988 10.4729 2.55468020 68280\n", 287 | &ss_arase, &numSS_arase ); 288 | if( r || numSS_arase != 1 ) 289 | { 290 | fprintf( stderr, "Error: SS can't load.\n" ); 291 | return -5; 292 | } 293 | 294 | double startmfe = (1714672800 - ss_arase->epoch)/60.0; 295 | // sgp4 (&iss_arase, startmfe, ro, vo); 296 | if( ConvertTLEToSGP4( &iss_arase, &ss_arase[0], startmfe, ro, vo ) ) 297 | { 298 | printf( "failed to convert tle\n" ); 299 | return -5; 300 | } 301 | puts( ss_arase->objectName ); 302 | 303 | double jd = ss_arase->jdsatepoch + floor(startmfe/1440.0); 304 | double jdfrac = fmod(startmfe/1440.0, 1.0) + ss_arase->jdsatepochF; 305 | int year, mon, day, hr, min; 306 | SGPF sec; 307 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 308 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 309 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n", 310 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2])); 311 | 312 | pysgp4[0] = 13105.747459057653; 313 | pysgp4[1] = 29909.425386662235; 314 | pysgp4[2] = 19138.1545898884; 315 | pysgp4v[0] = -1.3853033635228822; 316 | pysgp4v[1] = 1.227640540353242; 317 | pysgp4v[2] = -0.039183565331240844; 318 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 319 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 320 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 321 | rmse ); 322 | if( rmse < 0.0005 + CSGP4_USE_FLOAT * .03 ) 323 | { 324 | printf( "PASS\n" ); 325 | } 326 | else 327 | { 328 | printf( "FAIL\n" ); 329 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 330 | return -5; 331 | } 332 | 333 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 334 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 335 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 336 | vrmse ); 337 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .002 ) 338 | { 339 | printf( "PASS\n" ); 340 | } 341 | else 342 | { 343 | printf( "FAIL\n" ); 344 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 345 | return -6; 346 | } 347 | } 348 | 349 | // Forward a day, but, with THEMIS, a much higher orbit satellite 350 | /* 351 | from sgp4.api import jday 352 | from sgp4.api import Satrec 353 | s = "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999" 354 | t = "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071" 355 | satellite = Satrec.twoline2rv(s, t) 356 | jd, fr = jday(2024, 5, 2, 18, 0, 0) 357 | e, r, v = satellite.sgp4(jd, fr) 358 | >>> r 359 | (13105.747459057653, 29909.425386662235, 19138.1545898884) 360 | >>> v 361 | (-1.3853033635228822, 1.227640540353242, -0.039183565331240844) 362 | */ 363 | { 364 | struct TLEObject * ss_THEMIS = 0; 365 | struct elsetrec iss_THEMIS; 366 | int numSS_THEMIS = 0; 367 | r = ParseFileOrString( 0, "" 368 | "THEMIS E \n" 369 | "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999\n" 370 | "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071\n", 371 | &ss_THEMIS, &numSS_THEMIS ); 372 | if( r || numSS_THEMIS != 1 ) 373 | { 374 | fprintf( stderr, "Error: SS can't load.\n" ); 375 | return -5; 376 | } 377 | if( ConvertTLEToSGP4( &iss_THEMIS, &ss_THEMIS[0], 0, 0, 0 ) ) 378 | { 379 | printf( "failed to convert tle\n" ); 380 | return -5; 381 | } 382 | puts( ss_THEMIS->objectName ); 383 | double startmfe = (1714672800 - ss_THEMIS->epoch)/60.0; 384 | sgp4 (&iss_THEMIS, startmfe, ro, vo); 385 | 386 | //Dumpelsetrec( &iss_THEMIS ); 387 | 388 | double jd = ss_THEMIS->jdsatepoch + floor(startmfe/1440.0); 389 | double jdfrac = fmod(startmfe/1440.0, 1.0) + ss_THEMIS->jdsatepochF; 390 | int year, mon, day, hr, min; 391 | SGPF sec; 392 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 393 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 394 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n", 395 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2])); 396 | 397 | pysgp4[0] = 12197.874919034643; 398 | pysgp4[1] = 48838.6479258657; 399 | pysgp4[2] = 3134.8786450225966; 400 | pysgp4v[0] = -1.96146751028316; 401 | pysgp4v[1] = -1.7885483032900817; 402 | pysgp4v[2] = -0.32311386956142407; 403 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 404 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 405 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 406 | rmse ); 407 | if( rmse < 0.0005 + CSGP4_USE_FLOAT * .065 ) 408 | { 409 | printf( "PASS\n" ); 410 | } 411 | else 412 | { 413 | printf( "FAIL\n" ); 414 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 415 | return -5; 416 | } 417 | 418 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 419 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 420 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 421 | vrmse ); 422 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .001 ) 423 | { 424 | printf( "PASS\n" ); 425 | } 426 | else 427 | { 428 | printf( "FAIL\n" ); 429 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 430 | return -6; 431 | } 432 | } 433 | 434 | 435 | /* And forwarded out a month... 436 | jd, fr = jday(2024, 5, 27, 18, 0, 0) 437 | e, r, v = satellite.sgp4(jd, fr) 438 | >>> r 439 | (5099.551520031815, 1808.2683576301836, -4104.365753671076) 440 | >>> v 441 | (0.7417244009740825, 6.593295105250736, 3.825415802504736) 442 | */ 443 | if( 1 ) 444 | { 445 | struct TLEObject * ss = 0; 446 | struct elsetrec iss; 447 | int numSS = 0; 448 | r = ParseFileOrString( 0, "" 449 | "ISS (ZARYA) \n" 450 | "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995\n" 451 | "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710\n", 452 | &ss, &numSS ); 453 | if( r || numSS != 1 ) 454 | { 455 | fprintf( stderr, "Error: SS can't load.\n" ); 456 | return -5; 457 | } 458 | if( ConvertTLEToSGP4( &iss, &ss[0], 0, 0, 0 ) ) 459 | { 460 | printf( "failed to convert tle\n" ); 461 | return -5; 462 | } 463 | puts( ss->objectName ); 464 | double startmfe = (1716832800 - ss->epoch)/60.0; 465 | sgp4 (&iss, startmfe, ro, vo); 466 | 467 | double jd = ss->jdsatepoch + floor(startmfe/1440.0); 468 | double jdfrac = fmod(startmfe/1440.0, 1.0) + ss->jdsatepochF; 469 | int year, mon, day, hr, min; 470 | SGPF sec; 471 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 472 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 473 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n", 474 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2])); 475 | 476 | pysgp4[0] = 5099.551520031815; 477 | pysgp4[1] = 1808.2683576301836; 478 | pysgp4[2] = -4104.365753671076; 479 | pysgp4v[0] = 0.7417244009740825; 480 | pysgp4v[1] = 6.593295105250736; 481 | pysgp4v[2] = 3.825415802504736; 482 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 483 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 484 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 485 | rmse ); 486 | if( rmse < 0.005 + CSGP4_USE_FLOAT * 5 ) 487 | { 488 | printf( "PASS\n" ); 489 | } 490 | else 491 | { 492 | printf( "FAIL\n" ); 493 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 494 | return -5; 495 | } 496 | 497 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 498 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 499 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 500 | vrmse ); 501 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .006 ) 502 | { 503 | printf( "PASS\n" ); 504 | } 505 | else 506 | { 507 | printf( "FAIL\n" ); 508 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 509 | return -6; 510 | } 511 | 512 | } 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | // Perf test 524 | { 525 | struct TLEObject * ss = 0; 526 | struct elsetrec iss; 527 | int numss = 0; 528 | r = ParseFileOrString( 0, "" 529 | "ISS (ZARYA) \n" 530 | "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995\n" 531 | "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710\n", 532 | &ss, &numss ); 533 | if( r || numss != 1 ) 534 | { 535 | fprintf( stderr, "Error: SS can't load.\n" ); 536 | return -5; 537 | } 538 | if( ConvertTLEToSGP4( &iss, &ss[0], 0, 0, 0 ) ) 539 | { 540 | printf( "failed to convert tle\n" ); 541 | return -5; 542 | } 543 | int iter; 544 | double dStartSetup = OGGetAbsoluteTime(); 545 | for( iter = 0; iter < 1000000; iter++ ) 546 | { 547 | ConvertTLEToSGP4( &iss, &ss[0], 0, 0, 0 ); 548 | } 549 | double dEndSetup = OGGetAbsoluteTime(); 550 | double dStartRun = OGGetAbsoluteTime(); 551 | for( iter = 0; iter < 1000000; iter++ ) 552 | { 553 | double startmfe = (1714240800 + iter - ss->epoch)/60.0; 554 | sgp4 (&iss, startmfe, ro, vo); 555 | } 556 | double dEndRun = OGGetAbsoluteTime(); 557 | double dStartFull = OGGetAbsoluteTime(); 558 | for( iter = 0; iter < 1000000; iter++ ) 559 | { 560 | double startmfe = (1714240800 + iter - ss->epoch)/60.0; 561 | ConvertTLEToSGP4( &iss, &ss[0], 0, 0, 0 ); 562 | sgp4 (&iss, startmfe, ro, vo); 563 | } 564 | double dEndFull = OGGetAbsoluteTime(); 565 | double dStartFullInit = OGGetAbsoluteTime(); 566 | for( iter = 0; iter < 1000000; iter++ ) 567 | { 568 | double startmfe = (1714240800 + iter - ss->epoch)/60.0; 569 | ConvertTLEToSGP4( &iss, &ss[0], startmfe, ro, vo ); 570 | } 571 | double dEndFullInit = OGGetAbsoluteTime(); 572 | printf( "Init: %.4f us/iteration\n", dEndSetup - dStartSetup ); 573 | printf( "Run: %.4f us/iteration\n", dEndRun - dStartRun ); 574 | printf( "Full: %.4f us/iteration\n", dEndFull - dStartFull ); 575 | printf( "FullAtInit: %.4f us/iteration\n", dEndFullInit - dStartFullInit ); 576 | 577 | numss = 0; 578 | r = ParseFileOrString( 0, "" 579 | "THEMIS E \n" 580 | "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999\n" 581 | "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071\n", 582 | &ss, &numss ); 583 | if( r || numss != 1 ) 584 | { 585 | fprintf( stderr, "Error: THEMIS can't load.\n" ); 586 | return -5; 587 | } 588 | if( ConvertTLEToSGP4( &iss, &ss[0], 0, 0, 0 ) ) 589 | { 590 | printf( "failed to convert tle\n" ); 591 | return -5; 592 | } 593 | dStartFullInit = OGGetAbsoluteTime(); 594 | for( iter = 0; iter < 1000000; iter++ ) 595 | { 596 | double startmfe = (1714240800 + iter - ss->epoch)/60.0; 597 | ConvertTLEToSGP4( &iss, &ss[0], startmfe, ro, vo ); 598 | } 599 | dEndFullInit = OGGetAbsoluteTime(); 600 | printf( "Deep Space Full At Init: %.4f us/iteration\n", dEndFullInit - dStartFullInit ); 601 | 602 | 603 | 604 | } 605 | 606 | 607 | return 0; 608 | } 609 | 610 | -------------------------------------------------------------------------------- /checkProgSimple.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "os_generic.h" 3 | 4 | #include "csgp4_simple.h" 5 | 6 | #include "csgp4.h" 7 | 8 | CSGP4_DECORATOR int ConvertTLEToSGP4GPU( struct TLEObject * obj, SGPF initial_time, SGPF * initial_r, SGPF* initial_v, SGPF * a_alta_altp ) 9 | { 10 | // can use 'a' or 'i' methods. 11 | // * epoch - epoch time in days from jan 0, 1950. 0 hr 12 | // But, jdsatepoch is in days from 4713 bc 13 | int r = sgp4init_simple( obj->jdsatepoch-2433281.5 + obj->jdsatepochF, obj->dragTerm, 14 | obj->meanMotion1, obj->meanMotion2, obj->eccentricity, obj->argumentOfPerigee, obj->inclination, obj->meanAnomaly, obj->meanMotion, 15 | obj->rightAscensionOfTheAscendingNode, initial_time, initial_r, initial_v, a_alta_altp ); 16 | if( r ) 17 | { 18 | fprintf( stderr, "error: error %d set on sgp4init_simple\n", r ); 19 | exit( -5 ); 20 | } 21 | return 0; 22 | } 23 | 24 | 25 | int main( int argc, char ** argv ) 26 | { 27 | puts( "Simple Tests"); 28 | void feenableexcept( int i ); 29 | feenableexcept(FE_DIVBYZERO|FE_INVALID|FE_OVERFLOW); 30 | 31 | double pysgp4[3] = { -4582.664719456509, -4356.875102968861, 2475.1474001054107 }; 32 | double pysgp4v[3] = { 5.036095414394779, -2.2591278380385664, 5.3188560672302145 }; 33 | SGPF ro[3], vo[3], a_alta_altp[3]; 34 | double rmse; 35 | 36 | if( 1 ) 37 | { 38 | struct TLEObject * ss = 0; 39 | int numSS = 0; 40 | int r = ParseFileOrString( 0, "" 41 | "ISS (ZARYA) \n" 42 | "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995\n" 43 | "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710\n", 44 | &ss, &numSS ); 45 | if( r || numSS != 1 ) 46 | { 47 | fprintf( stderr, "Error: SS can't load.\n" ); 48 | return -5; 49 | } 50 | double startmfe = (1714240800 - ss->epoch)/60.0; 51 | if( ConvertTLEToSGP4GPU( &ss[0], startmfe, ro, vo, a_alta_altp ) ) 52 | { 53 | printf( "failed to convert tle\n" ); 54 | return -5; 55 | } 56 | puts( ss->objectName ); 57 | 58 | SGPF jd = ss->jdsatepoch + floor(startmfe/1440.0); 59 | SGPF jdfrac = FMOD(startmfe/1440.0, 1.0) + ss->jdsatepochF; 60 | int year, mon, day, hr, min; 61 | SGPF sec; 62 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 63 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 64 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n %16.9f %16.9f %16.9f\n", 65 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2]), 66 | (a_alta_altp[0]-1)*radiusearthkm, a_alta_altp[1]*radiusearthkm, a_alta_altp[2]*radiusearthkm 67 | ); 68 | 69 | /* 70 | These are from the following python code: 71 | 72 | from sgp4.api import jday 73 | from sgp4.api import Satrec 74 | s = "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995" 75 | t = "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710" 76 | satellite = Satrec.twoline2rv(s, t) 77 | jd, fr = jday(2024, 4, 27, 18, 0, 0) 78 | e, r, v = satellite.sgp4(jd, fr) 79 | >>> r 80 | (-4582.664719456509, -4356.875102968861, 2475.1474001054107) 81 | >>> v 82 | (5.036095414394779, -2.2591278380385664, 5.3188560672302145) 83 | 84 | For some reason, they disagree withh these coords by a few km. 85 | From https://nasa-public-data.s3.amazonaws.com/iss-coords/current/ISS_OEM/ISS.OEM_J2K_EPH.txt 86 | 2024-04-27T18:00:00.000 -4587.597832963610 -4337.824413854870 2498.905090951730 5.05095945730461 -2.27273088144798 5.29906134757423 87 | 1714240800 = Sat Apr 27 2024 18:00:00 GMT+0000 88 | */ 89 | pysgp4[0] = -4582.664719456509; 90 | pysgp4[1] = -4356.875102968861; 91 | pysgp4[2] = 2475.1474001054107; 92 | pysgp4v[0] = 5.036095414394779; 93 | pysgp4v[1] = -2.2591278380385664; 94 | pysgp4v[2] = 5.3188560672302145; 95 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 96 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 97 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 98 | rmse ); 99 | if( rmse < 0.005 ) 100 | { 101 | printf( "PASS\n" ); 102 | } 103 | else 104 | { 105 | printf( "FAIL\n" ); 106 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 107 | return -5; 108 | } 109 | 110 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 111 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 112 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 113 | vrmse ); 114 | if( vrmse < 0.00005 ) 115 | { 116 | printf( "PASS\n" ); 117 | } 118 | else 119 | { 120 | printf( "FAIL\n" ); 121 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed. Fail\n" ); 122 | return -6; 123 | } 124 | 125 | /* And forwarded out a day... 126 | jd, fr = jday(2024, 4, 28, 18, 0, 0) 127 | e, r, v = satellite.sgp4(jd, fr) 128 | >>> r 129 | (-5357.955394050937, -2614.4147285937256, 3236.6540120050795) 130 | >>> v 131 | (0.04217831766517118, -6.0043223779983865, -4.77009742984791) 132 | */ 133 | } 134 | 135 | 136 | if( 1 ) 137 | { 138 | struct TLEObject * ss = 0; 139 | int numSS = 0; 140 | int r = ParseFileOrString( 0, "" 141 | "ISS (ZARYA) \n" 142 | "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995\n" 143 | "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710\n", 144 | &ss, &numSS ); 145 | if( r || numSS != 1 ) 146 | { 147 | fprintf( stderr, "Error: SS can't load.\n" ); 148 | return -5; 149 | } 150 | puts( ss->objectName ); 151 | double startmfe = (1714327200 - ss->epoch)/60.0; 152 | if( ConvertTLEToSGP4GPU( &ss[0], startmfe, ro, vo, a_alta_altp ) ) 153 | { 154 | printf( "failed to convert tle\n" ); 155 | return -5; 156 | } 157 | 158 | double jd = ss->jdsatepoch + floor(startmfe/1440.0); 159 | double jdfrac = FMOD(startmfe/1440.0, 1.0) + ss->jdsatepochF; 160 | int year, mon, day, hr, min; 161 | SGPF sec; 162 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 163 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 164 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n %16.9f %16.9f %16.9f\n", 165 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2]), 166 | (a_alta_altp[0]-1)*radiusearthkm, a_alta_altp[1]*radiusearthkm, a_alta_altp[2]*radiusearthkm 167 | ); 168 | 169 | pysgp4[0] = 4435.874337686209; 170 | pysgp4[1] = 4191.117631955163; 171 | pysgp4[2] = -2991.3331751931737; 172 | pysgp4v[0] = -5.401088744185615; 173 | pysgp4v[1] = 2.177125902892223; 174 | pysgp4v[2] = -4.972896867609246; 175 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 176 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 177 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 178 | rmse ); 179 | if( rmse < 0.005 + CSGP4_USE_FLOAT * .05 ) 180 | { 181 | printf( "PASS\n" ); 182 | } 183 | else 184 | { 185 | printf( "FAIL\n" ); 186 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 187 | return -5; 188 | } 189 | 190 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 191 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 192 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 193 | vrmse ); 194 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .001 ) 195 | { 196 | printf( "PASS\n" ); 197 | } 198 | else 199 | { 200 | printf( "FAIL\n" ); 201 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 202 | return -6; 203 | } 204 | } 205 | 206 | 207 | // Forward a day, but, with Arase, a higher orbit satellite, ALSO TEST wth only init. 208 | /* 209 | from sgp4.api import jday 210 | from sgp4.api import Satrec 211 | s = "1 41896U 16080A 24120.85435253 .00000361 00000+0 77632-3 0 9994" 212 | t = "2 41896 31.9360 317.6417 7003302 291.8988 10.4729 2.55468020 68280" 213 | satellite = Satrec.twoline2rv(s, t) 214 | jd, fr = jday(2024, 5, 2, 18, 0, 0) 215 | e, r, v = satellite.sgp4(jd, fr) 216 | >>> r 217 | (13105.747459057653, 29909.425386662235, 19138.1545898884) 218 | >>> v 219 | (-1.3853033635228822, 1.227640540353242, -0.039183565331240844) 220 | */ 221 | if( 1 ) 222 | { 223 | struct TLEObject * ss_arase = 0; 224 | int numSS_arase = 0; 225 | int r = ParseFileOrString( 0, "" 226 | "ARASE (ERG) \n" 227 | "1 41896U 16080A 24120.85435253 .00000361 00000+0 77632-3 0 9994\n" 228 | "2 41896 31.9360 317.6417 7003302 291.8988 10.4729 2.55468020 68280\n", 229 | &ss_arase, &numSS_arase ); 230 | if( r || numSS_arase != 1 ) 231 | { 232 | fprintf( stderr, "Error: SS can't load.\n" ); 233 | return -5; 234 | } 235 | 236 | double startmfe = (1714672800 - ss_arase->epoch)/60.0; 237 | if( ConvertTLEToSGP4GPU( &ss_arase[0], startmfe, ro, vo, a_alta_altp ) ) 238 | { 239 | printf( "failed to convert tle\n" ); 240 | return -5; 241 | } 242 | puts( ss_arase->objectName ); 243 | 244 | double jd = ss_arase->jdsatepoch + floor(startmfe/1440.0); 245 | double jdfrac = FMOD(startmfe/1440.0, 1.0) + ss_arase->jdsatepochF; 246 | int year, mon, day, hr, min; 247 | SGPF sec; 248 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 249 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 250 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n %16.9f %16.9f %16.9f\n", 251 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2]), 252 | (a_alta_altp[0]-1)*radiusearthkm, a_alta_altp[1]*radiusearthkm, a_alta_altp[2]*radiusearthkm 253 | ); 254 | 255 | pysgp4[0] = 13105.747459057653; 256 | pysgp4[1] = 29909.425386662235; 257 | pysgp4[2] = 19138.1545898884; 258 | pysgp4v[0] = -1.3853033635228822; 259 | pysgp4v[1] = 1.227640540353242; 260 | pysgp4v[2] = -0.039183565331240844; 261 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 262 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 263 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 264 | rmse ); 265 | if( rmse < 0.0005 + CSGP4_USE_FLOAT * .03 ) 266 | { 267 | printf( "PASS\n" ); 268 | } 269 | else 270 | { 271 | printf( "FAIL\n" ); 272 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 273 | return -5; 274 | } 275 | 276 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 277 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 278 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 279 | vrmse ); 280 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .002 ) 281 | { 282 | printf( "PASS\n" ); 283 | } 284 | else 285 | { 286 | printf( "FAIL\n" ); 287 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 288 | return -6; 289 | } 290 | } 291 | 292 | // Forward a day, but, with THEMIS, a much higher orbit satellite 293 | /* 294 | from sgp4.api import jday 295 | from sgp4.api import Satrec 296 | s = "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999" 297 | t = "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071" 298 | satellite = Satrec.twoline2rv(s, t) 299 | jd, fr = jday(2024, 5, 2, 18, 0, 0) 300 | e, r, v = satellite.sgp4(jd, fr) 301 | >>> r 302 | (13105.747459057653, 29909.425386662235, 19138.1545898884) 303 | >>> v 304 | (-1.3853033635228822, 1.227640540353242, -0.039183565331240844) 305 | */ 306 | { 307 | struct TLEObject * ss_THEMIS = 0; 308 | int numSS_THEMIS = 0; 309 | int r = ParseFileOrString( 0, "" 310 | "THEMIS E \n" 311 | "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999\n" 312 | "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071\n", 313 | &ss_THEMIS, &numSS_THEMIS ); 314 | if( r || numSS_THEMIS != 1 ) 315 | { 316 | fprintf( stderr, "Error: SS can't load.\n" ); 317 | return -5; 318 | } 319 | puts( ss_THEMIS->objectName ); 320 | double startmfe = (1714672800 - ss_THEMIS->epoch)/60.0; 321 | if( ConvertTLEToSGP4GPU( &ss_THEMIS[0], startmfe, ro, vo, a_alta_altp ) ) 322 | { 323 | printf( "failed to convert tle\n" ); 324 | return -5; 325 | } 326 | 327 | //Dumpelsetrec( &iss_THEMIS ); 328 | 329 | double jd = ss_THEMIS->jdsatepoch + floor(startmfe/1440.0); 330 | double jdfrac = FMOD(startmfe/1440.0, 1.0) + ss_THEMIS->jdsatepochF; 331 | int year, mon, day, hr, min; 332 | SGPF sec; 333 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 334 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 335 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n %16.9f %16.9f %16.9f\n", 336 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2]), 337 | (a_alta_altp[0]-1)*radiusearthkm, a_alta_altp[1]*radiusearthkm, a_alta_altp[2]*radiusearthkm 338 | ); 339 | 340 | pysgp4[0] = 12197.874919034643; 341 | pysgp4[1] = 48838.6479258657; 342 | pysgp4[2] = 3134.8786450225966; 343 | pysgp4v[0] = -1.96146751028316; 344 | pysgp4v[1] = -1.7885483032900817; 345 | pysgp4v[2] = -0.32311386956142407; 346 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 347 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 348 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 349 | rmse ); 350 | if( rmse < 0.0005 + CSGP4_USE_FLOAT * .065 ) 351 | { 352 | printf( "PASS\n" ); 353 | } 354 | else 355 | { 356 | printf( "FAIL\n" ); 357 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 358 | return -5; 359 | } 360 | 361 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 362 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 363 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 364 | vrmse ); 365 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .001 ) 366 | { 367 | printf( "PASS\n" ); 368 | } 369 | else 370 | { 371 | printf( "FAIL\n" ); 372 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 373 | return -6; 374 | } 375 | } 376 | 377 | 378 | // Forward a month, but, with THEMIS, a much higher orbit satellite 379 | /* 380 | from sgp4.api import jday 381 | from sgp4.api import Satrec 382 | s = "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999" 383 | t = "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071" 384 | satellite = Satrec.twoline2rv(s, t) 385 | jd, fr = jday(2024, 5, 3, 18, 0, 0) 386 | e, r, v = satellite.sgp4(jd, fr) 387 | >>> r 388 | (60379.65507502841, 48486.94962512441, 9400.346222767417) 389 | >>> v 390 | (0.031163591993459357, 1.2500171299737175, 0.037585511101346745) 391 | */ 392 | { 393 | struct TLEObject * ss_THEMIS = 0; 394 | int numSS_THEMIS = 0; 395 | int r = ParseFileOrString( 0, "" 396 | "THEMIS E \n" 397 | "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999\n" 398 | "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071\n", 399 | &ss_THEMIS, &numSS_THEMIS ); 400 | if( r || numSS_THEMIS != 1 ) 401 | { 402 | fprintf( stderr, "Error: SS can't load.\n" ); 403 | return -5; 404 | } 405 | puts( ss_THEMIS->objectName ); 406 | double startmfe = (1717178400 - ss_THEMIS->epoch)/60.0; 407 | if( ConvertTLEToSGP4GPU( &ss_THEMIS[0], startmfe, ro, vo, a_alta_altp ) ) 408 | { 409 | printf( "failed to convert tle\n" ); 410 | return -5; 411 | } 412 | 413 | //Dumpelsetrec( &iss_THEMIS ); 414 | 415 | double jd = ss_THEMIS->jdsatepoch + floor(startmfe/1440.0); 416 | double jdfrac = FMOD(startmfe/1440.0, 1.0) + ss_THEMIS->jdsatepochF; 417 | int year, mon, day, hr, min; 418 | SGPF sec; 419 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 420 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 421 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n %16.9f %16.9f %16.9f\n", 422 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2]), 423 | (a_alta_altp[0]-1)*radiusearthkm, a_alta_altp[1]*radiusearthkm, a_alta_altp[2]*radiusearthkm 424 | ); 425 | 426 | pysgp4[0] = 60379.65507502841; 427 | pysgp4[1] = 48486.94962512441; 428 | pysgp4[2] = 9400.346222767417; 429 | pysgp4v[0] = 0.031163591993459357; 430 | pysgp4v[1] = 1.2500171299737175; 431 | pysgp4v[2] = 0.037585511101346745; 432 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 433 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 434 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 435 | rmse ); 436 | if( rmse < 0.0005 + CSGP4_USE_FLOAT * .365 ) 437 | { 438 | printf( "PASS\n" ); 439 | } 440 | else 441 | { 442 | printf( "FAIL\n" ); 443 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 444 | return -5; 445 | } 446 | 447 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 448 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 449 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 450 | vrmse ); 451 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .001 ) 452 | { 453 | printf( "PASS\n" ); 454 | } 455 | else 456 | { 457 | printf( "FAIL\n" ); 458 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 459 | return -6; 460 | } 461 | } 462 | 463 | 464 | /* And forwarded out a month... 465 | jd, fr = jday(2024, 5, 27, 18, 0, 0) 466 | e, r, v = satellite.sgp4(jd, fr) 467 | >>> r 468 | (5099.551520031815, 1808.2683576301836, -4104.365753671076) 469 | >>> v 470 | (0.7417244009740825, 6.593295105250736, 3.825415802504736) 471 | */ 472 | if( 1 ) 473 | { 474 | struct TLEObject * ss = 0; 475 | int numSS = 0; 476 | int r = ParseFileOrString( 0, "" 477 | "ISS (ZARYA) \n" 478 | "1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995\n" 479 | "2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710\n", 480 | &ss, &numSS ); 481 | if( r || numSS != 1 ) 482 | { 483 | fprintf( stderr, "Error: SS can't load.\n" ); 484 | return -5; 485 | } 486 | 487 | double startmfe = (1716832800 - ss->epoch)/60.0; 488 | if( ConvertTLEToSGP4GPU( &ss[0], startmfe, ro, vo,a_alta_altp ) ) 489 | { 490 | printf( "failed to convert tle\n" ); 491 | return -5; 492 | } 493 | puts( ss->objectName ); 494 | double jd = ss->jdsatepoch + floor(startmfe/1440.0); 495 | double jdfrac = FMOD(startmfe/1440.0, 1.0) + ss->jdsatepochF; 496 | int year, mon, day, hr, min; 497 | SGPF sec; 498 | invjday( jd, jdfrac, &year, &mon, &day, &hr, &min, &sec ); 499 | printf( "%f %f %04d %02d %02d %02d:%02d:%05.02f\n", jd, jdfrac, year, mon, day, hr, min, sec ); 500 | printf( "[Δt%14.8f] %16.8f %16.8f %16.8f %16.8f \n %16.9f %16.9f %16.9f %16.9f\n %16.9f %16.9f %16.9f\n", 501 | startmfe,ro[0],ro[1],ro[2], sqrt(ro[0]*ro[0]+ro[1]*ro[1]+ro[2]*ro[2]),vo[0],vo[1],vo[2], sqrt(vo[0]*vo[0]+vo[1]*vo[1]+vo[2]*vo[2]), 502 | (a_alta_altp[0]-1)*radiusearthkm, a_alta_altp[1]*radiusearthkm, a_alta_altp[2]*radiusearthkm 503 | ); 504 | 505 | pysgp4[0] = 5099.551520031815; 506 | pysgp4[1] = 1808.2683576301836; 507 | pysgp4[2] = -4104.365753671076; 508 | pysgp4v[0] = 0.7417244009740825; 509 | pysgp4v[1] = 6.593295105250736; 510 | pysgp4v[2] = 3.825415802504736; 511 | rmse = sqrt( (ro[0] - pysgp4[0])*(ro[0] - pysgp4[0]) + (ro[1] - pysgp4[1]) * (ro[1] - pysgp4[1]) + ( ro[2] - pysgp4[2] ) * (ro[2] - pysgp4[2]) ); 512 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km ... ", 513 | ro[0] - pysgp4[0], ro[1] - pysgp4[1], ro[2] - pysgp4[2], 514 | rmse ); 515 | if( rmse < 0.005 + CSGP4_USE_FLOAT * 5 ) 516 | { 517 | printf( "PASS\n" ); 518 | } 519 | else 520 | { 521 | printf( "FAIL\n" ); 522 | fprintf( stderr, "Error: SGP Algorithm disagrees in position. Fail\n" ); 523 | return -5; 524 | } 525 | 526 | double vrmse = sqrt( (vo[0] - pysgp4v[0])*(vo[0] - pysgp4v[0]) + (vo[1] - pysgp4v[1]) * (vo[1] - pysgp4v[1]) + ( vo[2] - pysgp4v[2] ) * (vo[2] - pysgp4v[2]) ); 527 | printf( "Python / Our SGP4 Disagreement: %f %f %f RMS: %f km/s ... ", 528 | vo[0] - pysgp4v[0], vo[1] - pysgp4v[1], vo[2] - pysgp4v[2], 529 | vrmse ); 530 | if( vrmse < 0.00005 + CSGP4_USE_FLOAT * .006 ) 531 | { 532 | printf( "PASS\n" ); 533 | } 534 | else 535 | { 536 | printf( "FAIL\n" ); 537 | fprintf( stderr, "Error: SGP Algorithm disagrees in speed (%f). Fail\n", vrmse ); 538 | return -6; 539 | } 540 | 541 | } 542 | 543 | // Perf test 544 | { 545 | struct TLEObject * ss = 0; 546 | int numss = 0; 547 | int iter = 0; 548 | numss = 0; 549 | int r = ParseFileOrString( 0, "" 550 | "THEMIS E \n" 551 | "1 30798U 07004E 24121.56859868 -.00000520 00000+0 00000+0 0 9999\n" 552 | "2 30798 7.9414 283.2653 8322119 302.3633 341.0652 0.87832507 59071\n", 553 | &ss, &numss ); 554 | if( r || numss != 1 ) 555 | { 556 | fprintf( stderr, "Error: THEMIS can't load.\n" ); 557 | return -5; 558 | } 559 | double dStartFullInit = OGGetAbsoluteTime(); 560 | for( iter = 0; iter < 1000000; iter++ ) 561 | { 562 | double startmfe = (1714240800 + iter - ss->epoch)/60.0; 563 | ConvertTLEToSGP4GPU( &ss[0], startmfe, ro, vo, 0 ); 564 | } 565 | double dEndFullInit = OGGetAbsoluteTime(); 566 | printf( "Deep Space Full At Init: %.4f us/iteration\n", dEndFullInit - dStartFullInit ); 567 | } 568 | 569 | 570 | return 0; 571 | } 572 | -------------------------------------------------------------------------------- /csgp4.h: -------------------------------------------------------------------------------- 1 | // Single-file C header transliterated from David Vallado's SGP4Lib.cs, version: "SGP4 Version 2020-03-12"; 2 | // https://celestrak.org/software/vallado-sw.php 3 | // I could not find a license, but assume whatever license the original code is under. 4 | // I (Charles Lohr) just ported the code back to C from C#. 5 | // I cannot guaratee this code for precision or fitness beyond my simple tests in associated files. 6 | // USE AT YOUR OWN RISK 7 | // 8 | // 9 | // Please note, all SGP-Specific code is in the second part of the file. Other features like 10 | // TLE reading code is in the first part. 11 | #ifndef _CSGP4_H 12 | #define _CSGP4_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef __cplusplus 20 | #define CSGP4_OUT * 21 | #else 22 | #define CSGP4_OUT * restrict 23 | #endif 24 | 25 | #define SGPPI 3.1415926535897932384626433 26 | 27 | #define SGP4_FROM_EPOCH_DAYS 2440587.5 28 | 29 | // Default to double calculations. 30 | #ifndef CSGP4_USE_FLOAT 31 | #define CSGP4_USE_FLOAT 0 32 | #endif 33 | 34 | #ifndef CSGP4_INIT 35 | #define CSGP4_INIT 1 36 | #endif 37 | 38 | 39 | #ifndef CSGP4_DECORATOR 40 | #define CSGP4_DECORATOR static 41 | #endif 42 | 43 | 44 | 45 | 46 | #if CSGP4_USE_FLOAT 47 | 48 | // Use single precision. 49 | #define SGPF float 50 | #define FLOOR floorf 51 | #define FABS fabsf 52 | #define COS cosf 53 | #define SIN sinf 54 | #define ATAN2 atan2f 55 | #define SQRT sqrtf 56 | #define POW powf 57 | #define FMOD fmodf 58 | #else 59 | // Use double precision. 60 | #define SGPF double 61 | #define FLOOR floor 62 | #define FABS fabs 63 | #define COS cos 64 | #define SIN sin 65 | #define ATAN2 atan2 66 | #define SQRT sqrt 67 | #define POW pow 68 | #define FMOD fmod 69 | #endif 70 | 71 | 72 | 73 | 74 | enum gravconsttype { wgs72old, wgs72, wgs84 }; // wgs72 is the standard and should be used with JSPOC TLEs 75 | 76 | // For SGP4 - this is the internal state (between init and prediction) 77 | struct elsetrec 78 | { 79 | int error; 80 | char operationmode; 81 | char init, method; 82 | 83 | /* Near Earth */ 84 | int isimp; 85 | 86 | SGPF aycof, con41, cc1, cc4, cc5, d2, d3, d4, 87 | delmo, eta, argpdot, omgcof, sinmao, t, t2cof, t3cof, 88 | t4cof, t5cof, x1mth2, x7thm1, mdot, nodedot, xlcof, xmcof, nodecf; 89 | 90 | 91 | SGPF bstar, inclo, nodeo, ecco, argpo, mo; 92 | 93 | /* Deep Space */ 94 | int irez; 95 | SGPF d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, 96 | d5421, d5433, dedt, del1, del2, del3, didt, dmdt, 97 | dnodt, domdt, e3, ee2, peo, pgho, pho, pinco, 98 | plo, se2, se3, sgh2, sgh3, sgh4, sh2, sh3, 99 | si2, si3, sl2, sl3, sl4, gsto, xfact, xgh2, 100 | xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, 101 | xl4, xlamo, zmol, zmos, atime, xli, xni; 102 | // sgp4fix add unkozai'd variable 103 | SGPF no_unkozai; 104 | // sgp4fix add singly averaged variables 105 | SGPF am, em, im, Om, om, mm, nm; 106 | // sgp4fix add constant parameters to eliminate mutliple calls during execution 107 | SGPF tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2; 108 | 109 | 110 | #if CSGP4_INIT 111 | SGPF a /* Not actually used in algo but fun to look at */; 112 | SGPF no_kozai,rcse; 113 | SGPF ndot /* Not actually used in algo*/; 114 | SGPF nddot /* Not actually used in algo*/; 115 | SGPF alta /* Not used in algo, but cool anyway to look at */; 116 | SGPF altp /* Not used in algo, but cool anyway to look at */; 117 | #endif 118 | }; 119 | 120 | // Our TLE Objects - this is just the information available for individual tracked objects, provided by 121 | // data sources on the web. 122 | struct TLEObject 123 | { 124 | int valid; // 0 for invalid, 1 for name, 2 for line 1, 4 for line 2 (Should be 7) 125 | 126 | // This is what "should" get passed around. (Below this line is actual useful data) 127 | 128 | char objectName[28]; // Actually maxes at 25; [0] is the classification designator, but this pads 129 | char internationalDesignator[12]; // Actually maxes at 8, but we pad. 130 | 131 | SGPF jdsatepochF; 132 | SGPF jdsatepoch; 133 | SGPF meanMotion1; // Not actually used in algo. 134 | SGPF meanMotion2; // Not actually used in algo. 135 | SGPF dragTerm; // I.e. 34469-3 = 0.00034469 136 | SGPF inclination; 137 | SGPF rightAscensionOfTheAscendingNode; 138 | SGPF eccentricity; // 0115263 = 0.0115263 139 | SGPF argumentOfPerigee; 140 | SGPF meanAnomaly; 141 | SGPF meanMotion; 142 | 143 | // Data that need not be passed around. 144 | int revolutionNumberAtEpoch; 145 | int elementSetNumber; 146 | uint32_t catalogNumber; 147 | double epoch; // In Unix Time. 148 | }; 149 | 150 | 151 | CSGP4_DECORATOR double ConvertEpochYearAndDayToUnix( int epochYear, double epochDay ); 152 | 153 | CSGP4_DECORATOR int ParseFileOrString( FILE * f, const char * sLineSet, struct TLEObject ** objects, int * numObjects ); 154 | 155 | CSGP4_DECORATOR int ConvertTLEToSGP4( struct elsetrec * satrec, struct TLEObject * obj, SGPF initial_time, SGPF * initial_r, SGPF* initial_v ); 156 | 157 | CSGP4_DECORATOR void sgp4 158 | ( 159 | struct elsetrec * satrec, SGPF tsince, 160 | SGPF r[3], SGPF v[3] 161 | ); 162 | 163 | 164 | CSGP4_DECORATOR void sgp4init 165 | ( 166 | enum gravconsttype whichconst, char opsmode/*, char * satn*/, SGPF epoch, 167 | SGPF xbstar, SGPF xndot, SGPF xnddot, SGPF xecco, SGPF xargpo, 168 | SGPF xinclo, SGPF xmo, SGPF xno_kozai, 169 | SGPF xnodeo, SGPF initial_time, SGPF * initial_r, SGPF * initial_v, struct elsetrec * satrec 170 | ); 171 | 172 | CSGP4_DECORATOR void days2mdhms 173 | ( 174 | int year, SGPF days, 175 | int * mon, int * day, int * hr, int * minute, SGPF CSGP4_OUT second 176 | ); 177 | 178 | CSGP4_DECORATOR void jday 179 | ( 180 | int year, int mon, int day, int hr, int minute, SGPF sec, 181 | SGPF CSGP4_OUT jd, SGPF CSGP4_OUT jdFrac 182 | ); 183 | 184 | CSGP4_DECORATOR void invjday 185 | ( 186 | SGPF jd, SGPF jdFrac, 187 | int * year, int * mon, int * day, 188 | int * hr, int * minute, SGPF CSGP4_OUT second 189 | ); 190 | 191 | CSGP4_DECORATOR float ParseFixedEponential( const char * le, int lineno, int * aborted ) 192 | { 193 | int i; 194 | // TODO: Handle aborted case. 195 | 196 | // Get rid of white space. 197 | while( le[0] == ' ' ) le++; 198 | 199 | int len = strlen( le ); 200 | int eportion = 0; 201 | int iportion = 0; 202 | for( i = len-1; i >= 0; i-- ) 203 | { 204 | char c = le[i]; 205 | if( c == '-' || c == '+' ) 206 | { 207 | eportion = atoi( le + i ); 208 | break; 209 | } 210 | } 211 | int iportiondigits = i; 212 | if( le[0] == '-' ) iportiondigits--; 213 | if( le[0] == '+' || le[0] == ' ' ) { iportiondigits--; le++; } 214 | iportion = atoi( le ); 215 | float ret = iportion; 216 | int shiftdown = iportiondigits - eportion; 217 | for( i = 0; i < shiftdown; i++ ) 218 | { 219 | ret /= 10; 220 | } 221 | for( i = 0; i < -shiftdown; i++ ) 222 | { 223 | ret *= 10; 224 | } 225 | return ret; 226 | } 227 | 228 | 229 | CSGP4_DECORATOR double ConvertEpochYearAndDayToUnix( int epochYear, double epochDay ) 230 | { 231 | epochYear = ( epochYear > 56 ) ? ( epochYear + 1900 ) : ( epochYear + 2000 ); 232 | int year; 233 | int day = 0; 234 | for( year = 1970; year < epochYear; year++ ) 235 | { 236 | if (year % 400 == 0) { 237 | day += 366; 238 | } else if (year % 100 == 0) { 239 | day += 365; 240 | } else if (year % 4 == 0) { 241 | day += 366; 242 | } else { 243 | day += 365; 244 | } 245 | } 246 | 247 | epochDay--; // It's 1 indexed. 248 | 249 | return ( day + epochDay ) * 60 * 60 * 24; 250 | } 251 | 252 | 253 | CSGP4_DECORATOR int ParseFileOrString( FILE * f, const char * sLineSet, struct TLEObject ** objects, int * numObjects ) 254 | { 255 | int i; 256 | ssize_t s; 257 | char line[256]; 258 | int lineno = 0; 259 | 260 | int thisValid = 0; 261 | int aborted = 0; 262 | int ret = 0; 263 | struct TLEObject * thisObject; 264 | 265 | const double deg2rad = SGPPI / 180.0; // 0.0174532925199433 266 | const double xpdotp = 1440.0 / (2.0 *SGPPI); // 229.1831180523293 267 | 268 | while( 1 ) 269 | { 270 | if( f ) 271 | { 272 | char * lp = fgets( line, sizeof(line), f ); 273 | if( !lp ) 274 | break; 275 | s = strlen( line ); 276 | } 277 | else if( sLineSet ) 278 | { 279 | char c; 280 | s = 0; 281 | while( ( c = *(sLineSet++) ) ) { 282 | line[s++] = c; 283 | if( c == '\n' ) break; 284 | } 285 | if( c == 0 ) break; 286 | line[s] = 0; 287 | } 288 | if( line[s-1] == '\r' || line[s-1] == '\n' ) s--; 289 | if( line[s-1] == '\r' || line[s-1] == '\n' ) s--; 290 | lineno++; 291 | if( thisValid == 0 ) 292 | { 293 | int nObject = *numObjects; 294 | if( !aborted ) 295 | ++*numObjects; 296 | else 297 | ret = -5; 298 | 299 | aborted = 0; 300 | *objects = (struct TLEObject*)realloc( *objects, sizeof( **objects ) * *numObjects ); 301 | thisObject = *objects + nObject; 302 | memset( thisObject, 0, sizeof( *thisObject ) ); 303 | thisObject->objectName[0] = ' '; 304 | thisObject->objectName[1] = ' '; 305 | memcpy( thisObject->objectName + 2, line, 24 ); 306 | 307 | int i; 308 | for( i = 24; i > 0; i-- ) 309 | if( thisObject->objectName[i] == ' ' ) 310 | thisObject->objectName[i] = 0; 311 | else 312 | break; 313 | 314 | thisValid = 1; 315 | continue; 316 | } 317 | 318 | if( line[1] != ' ' ) 319 | { 320 | fprintf( stderr, "Parsing error on line %d - unexpected char at space 1\n", lineno ); 321 | aborted = 1; 322 | continue; 323 | } 324 | 325 | int checksum_check, checksum; 326 | 327 | switch( line[0] ) 328 | { 329 | case '1': 330 | { 331 | // 1 25544U 98067A 24108.06679608 .00019473 00000+0 34469-3 0 9999 332 | if( s < 68 ) 333 | { 334 | fprintf( stderr, "Parsing error on line %d; too short\n", lineno ); 335 | aborted = 1; 336 | continue; 337 | } 338 | checksum_check = 0; 339 | for( i = 0; i < s-1; i++ ) 340 | { 341 | char c = line[i]; 342 | if( c == '-' ) checksum_check++; 343 | if( c >= '0' && c <= '9' ) checksum_check += c - '0'; 344 | } 345 | 346 | 347 | thisObject->objectName[0] = line[7]; 348 | line[7] = 0; 349 | thisObject->catalogNumber = atoi( line + 2 ); 350 | memcpy( thisObject->internationalDesignator, line + 9, 8 ); 351 | 352 | checksum = atoi( line + 68 ); 353 | line[68] = 0; 354 | if( checksum != checksum_check % 10 ) 355 | { 356 | fprintf( stderr, "Checkum error on line %d. %d != %d\n", lineno, checksum, checksum_check % 10 ); 357 | aborted = 1; 358 | continue; 359 | } 360 | 361 | thisObject->elementSetNumber = atoi( line + 64 ); 362 | line[61] = 0; 363 | thisObject->dragTerm = ParseFixedEponential( line + 53, lineno, &aborted ); 364 | line[52] = 0; 365 | thisObject->meanMotion2 = ParseFixedEponential( line + 44, lineno, &aborted ) / (xpdotp*1440.0*1440); 366 | line[43] = 0; 367 | thisObject->meanMotion1 = atof( line + 33 ) / (xpdotp*1440.0); 368 | line[32] = 0; 369 | double epochDay = atof( line + 20 ); 370 | line[20] = 0; 371 | double epochYear = atoi( line + 18 ); 372 | line[17] = 0; 373 | 374 | // ---------------------------------------------------------------- 375 | // find sgp4epoch time of element set 376 | // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch) 377 | // and minutes from the epoch (time) 378 | // ---------------------------------------------------------------- 379 | 380 | // ---------------- temp fix for years from 1957-2056 ------------------- 381 | // --------- correct fix will occur when year is 4-digit in tle --------- 382 | int mon, day, hr, minute, year; 383 | SGPF sec; 384 | if (epochYear < 57) 385 | year = epochYear + 2000; 386 | else 387 | year = epochYear + 1900; 388 | days2mdhms(year, epochDay, &mon, &day, &hr, &minute, &sec); 389 | jday(year, mon, day, hr, minute, sec, &thisObject->jdsatepoch, &thisObject->jdsatepochF); 390 | thisObject->epoch = ConvertEpochYearAndDayToUnix( epochYear, epochDay ); 391 | break; 392 | } 393 | case '2': 394 | { 395 | // 2 25544 51.6384 258.4693 0004986 70.1481 42.8477 15.50291806449066 396 | thisValid = 0; // Finalizing this record. 397 | 398 | if( s < 68 ) 399 | { 400 | fprintf( stderr, "Parsing error on line %d; too short\n", lineno ); 401 | aborted = 1; 402 | continue; 403 | } 404 | checksum_check = 0; 405 | for( i = 0; i < s-1; i++ ) 406 | { 407 | char c = line[i]; 408 | if( c == '-' ) checksum_check++; 409 | if( c >= '0' && c <= '9' ) checksum_check += c - '0'; 410 | } 411 | 412 | int catalogNumber = atoi( line + 2 ); 413 | if( thisObject->catalogNumber != catalogNumber ) 414 | { 415 | fprintf( stderr, "Error: Mismatching catalog numbers at line %d\n", lineno ); 416 | aborted = 1; 417 | } 418 | 419 | thisObject->objectName[0] = line[7]; 420 | memcpy( thisObject->internationalDesignator, line + 9, 8 ); 421 | 422 | checksum = atoi( line + 68 ); 423 | line[68] = 0; 424 | if( checksum != checksum_check % 10 ) 425 | { 426 | fprintf( stderr, "Checkum error on line %d. %d != %d\n", lineno, checksum, checksum_check % 10 ); 427 | aborted = 1; 428 | continue; 429 | } 430 | 431 | thisObject->revolutionNumberAtEpoch = atoi( line + 63 ); 432 | line[63] = 0; 433 | thisObject->meanMotion = atof( line + 52 ) / xpdotp; 434 | line[51] = 0; 435 | thisObject->meanAnomaly = atof( line + 43 ) * deg2rad; 436 | line[42] = 0; 437 | thisObject->argumentOfPerigee = atof( line + 34 ) * deg2rad; 438 | line[33] = 0; 439 | 440 | thisObject->eccentricity = atof( line + 26 ); 441 | if( line[26] == '-' ) thisObject->eccentricity *= 0.000001; 442 | else thisObject->eccentricity *= 0.0000001; 443 | line[25] = 0; 444 | 445 | thisObject->rightAscensionOfTheAscendingNode = atof( line + 17 ) * deg2rad; 446 | line[16] = 0; 447 | thisObject->inclination = atof( line + 8 ) * deg2rad ; 448 | line[7] = 0; 449 | thisObject->valid = 1; 450 | 451 | break; 452 | } 453 | default: 454 | fprintf( stderr, "Unknown record type %c on line %d\n", line[0], lineno ); 455 | } 456 | } 457 | 458 | // If last element is complete, drop it. 459 | if( *numObjects && thisValid ) --*numObjects; 460 | 461 | return ret; 462 | } 463 | 464 | 465 | 466 | 467 | #if CSGP4_INIT 468 | 469 | CSGP4_DECORATOR int ConvertTLEToSGP4( struct elsetrec * satrec, struct TLEObject * obj, SGPF initial_time, SGPF * initial_r, SGPF* initial_v ) 470 | { 471 | if( !obj->valid ) return -1; 472 | 473 | enum gravconsttype whichconst = wgs72; 474 | satrec->no_kozai = obj->meanMotion; 475 | satrec->ecco = obj->eccentricity; 476 | satrec->inclo = obj->inclination; 477 | satrec->nodeo = obj->rightAscensionOfTheAscendingNode; 478 | satrec->argpo = obj->argumentOfPerigee; 479 | satrec->mo = obj->meanAnomaly; 480 | satrec->nddot = obj->meanMotion2; 481 | satrec->bstar = obj->dragTerm; 482 | satrec->ndot = obj->meanMotion1; 483 | 484 | // can use 'a' or 'i' methods. 485 | // * epoch - epoch time in days from jan 0, 1950. 0 hr 486 | // But, jdsatepoch is in days from 4713 bc 487 | sgp4init( whichconst, 'a', /*satrec->satnum,*/ obj->jdsatepoch-2433281.5 + obj->jdsatepochF, satrec->bstar, 488 | satrec->ndot, satrec->nddot, satrec->ecco, satrec->argpo, satrec->inclo, satrec->mo, satrec->no_kozai, 489 | satrec->nodeo, initial_time, initial_r, initial_v, satrec ); 490 | 491 | return 0; 492 | } 493 | 494 | #endif 495 | 496 | 497 | ////////////////////////////////////////////////////////////////////////////// 498 | // SGP4 Code below here ////////////////////////////////////////////////////// 499 | ////////////////////////////////////////////////////////////////////////////// 500 | 501 | /* ----------------------------------------------------------------------------- 502 | * 503 | * function gstime 504 | * 505 | * this function finds the greenwich sidereal time. 506 | * 507 | * author : david vallado 719-573-2600 1 mar 2001 508 | * translated to c: charles lohr 2024-04-28 509 | * 510 | * inputs description range / units 511 | * jdut1 - julian date in ut1 days from 4713 bc 512 | * 513 | * outputs : 514 | * gstime - greenwich sidereal time 0 to 2pi rad 515 | * 516 | * locals : 517 | * temp - temporary variable for SGPFs rad 518 | * tut1 - julian centuries from the 519 | * jan 1, 2000 12 h epoch (ut1) 520 | * 521 | * coupling : 522 | * none 523 | * 524 | * references : 525 | * vallado 2004, 191, eq 3-45 526 | * --------------------------------------------------------------------------- */ 527 | 528 | CSGP4_DECORATOR SGPF gstime 529 | ( 530 | SGPF jdut1 531 | ) 532 | { 533 | const SGPF twopi = 2.0 * SGPPI; 534 | const SGPF deg2rad = SGPPI / 180.0; 535 | SGPF temp, tut1; 536 | 537 | tut1 = (jdut1 - 2451545.0) / 36525.0; 538 | temp = -6.2e-6 * tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + 539 | (876600.0 * 3600 + 8640184.812866) * tut1 + 67310.54841; // sec 540 | temp = FMOD(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad 541 | 542 | // ------------------------ check quadrants --------------------- 543 | if (temp < 0.0) 544 | temp += twopi; 545 | 546 | return temp; 547 | } // end gstime 548 | 549 | 550 | /* ----------------------------------------------------------------------------- 551 | * 552 | * function getgravconst 553 | * 554 | * this function gets constants for the propagator. note that mu is identified to 555 | * facilitiate comparisons with newer models. the common useage is wgs72. 556 | * 557 | * author : david vallado 719-573-2600 21 jul 2006 558 | * translated to c: charles lohr 2024-04-28 559 | * 560 | * inputs : 561 | * whichconst - which set of constants to use wgs72old, wgs72, wgs84 562 | * 563 | * outputs : 564 | * tumin - minutes in one time unit 565 | * mu - earth gravitational parameter 566 | * radiusearthkm - radius of the earth in km 567 | * xke - reciprocal of tumin 568 | * j2, j3, j4 - un-normalized zonal harmonic values 569 | * j3oj2 - j3 divided by j2 570 | * 571 | * locals : 572 | * 573 | * coupling : 574 | * none 575 | * 576 | * references : 577 | * norad spacetrack report #3 578 | * vallado, crawford, hujsak, kelso 2006 579 | --------------------------------------------------------------------------- */ 580 | 581 | CSGP4_DECORATOR void getgravconst 582 | ( 583 | enum gravconsttype whichconst, 584 | SGPF CSGP4_OUT tumin, 585 | SGPF CSGP4_OUT mu, 586 | SGPF CSGP4_OUT radiusearthkm, 587 | SGPF CSGP4_OUT xke, 588 | SGPF CSGP4_OUT j2, 589 | SGPF CSGP4_OUT j3, 590 | SGPF CSGP4_OUT j4, 591 | SGPF CSGP4_OUT j3oj2 592 | ) 593 | { 594 | 595 | switch (whichconst) 596 | { 597 | // -- wgs-72 low precision str#3 constants -- 598 | case wgs72old: 599 | *mu = 398600.79964; // in km3 / s2 600 | *radiusearthkm = 6378.135; // km 601 | *xke = 0.0743669161; // reciprocal of tumin 602 | *tumin = 1.0 / *xke; 603 | *j2 = 0.001082616; 604 | *j3 = -0.00000253881; 605 | *j4 = -0.00000165597; 606 | *j3oj2 = *j3 / *j2; 607 | break; 608 | // ------------ wgs-72 constants ------------ 609 | case wgs72: 610 | *mu = 398600.8; // in km3 / s2 611 | *radiusearthkm = 6378.135; // km 612 | *xke = 60.0 / SQRT(*radiusearthkm * *radiusearthkm * *radiusearthkm / *mu); 613 | *tumin = 1.0 / *xke; 614 | *j2 = 0.001082616; 615 | *j3 = -0.00000253881; 616 | *j4 = -0.00000165597; 617 | *j3oj2 = *j3 / *j2; 618 | break; 619 | case wgs84: 620 | // ------------ wgs-84 constants ------------ 621 | *mu = 398600.5; // in km3 / s2 622 | *radiusearthkm = 6378.137; // km 623 | *xke = 60.0 / SQRT(*radiusearthkm * *radiusearthkm * *radiusearthkm / *mu); 624 | *tumin = 1.0 / *xke; 625 | *j2 = 0.00108262998905; 626 | *j3 = -0.00000253215306; 627 | *j4 = -0.00000161098761; 628 | *j3oj2 = *j3 / *j2; 629 | break; 630 | default: 631 | //fprintf(stderr, "unknown gravity option (%d)\n", whichconst); 632 | *mu = 0.0; // in km3 / s2 633 | *radiusearthkm = 0.0; // km 634 | *xke = 0.0; 635 | *tumin = 0.0; 636 | *j2 = 0.0; 637 | *j3 = 0.0; 638 | *j4 = 0.0; 639 | *j3oj2 = 0.0; 640 | 641 | break; 642 | } 643 | 644 | } // end getgravconst 645 | 646 | 647 | 648 | /*----------------------------------------------------------------------------- 649 | * 650 | * procedure dspace 651 | * 652 | * this procedure provides deep space contributions to mean elements for 653 | * perturbing third body. these effects have been averaged over one 654 | * revolution of the sun and moon. for earth resonance effects, the 655 | * effects have been averaged over no revolutions of the satellite. 656 | * (mean motion) 657 | * 658 | * author : david vallado 719-573-2600 28 jun 2005 659 | * translated to c: charles lohr 2024-04-28 660 | * 661 | * inputs : 662 | * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 663 | * dedt - 664 | * del1, del2, del3 - 665 | * didt - 666 | * dmdt - 667 | * dnodt - 668 | * domdt - 669 | * irez - flag for resonance 0-none, 1-one day, 2-half day 670 | * argpo - argument of perigee 671 | * argpdot - argument of perigee dot (rate) 672 | * t - time 673 | * tc - 674 | * gsto - gst 675 | * xfact - 676 | * xlamo - 677 | * no - mean motion 678 | * atime - 679 | * em - eccentricity 680 | * ft - 681 | * argpm - argument of perigee 682 | * inclm - inclination 683 | * xli - 684 | * mm - mean anomaly 685 | * xni - mean motion 686 | * nodem - right ascension of ascending node 687 | * 688 | * outputs : 689 | * atime - 690 | * em - eccentricity 691 | * argpm - argument of perigee 692 | * inclm - inclination 693 | * xli - 694 | * mm - mean anomaly 695 | * xni - 696 | * nodem - right ascension of ascending node 697 | * dndt - 698 | * nm - mean motion 699 | * 700 | * locals : 701 | * delt - 702 | * ft - 703 | * theta - 704 | * x2li - 705 | * x2omi - 706 | * xl - 707 | * xldot - 708 | * xnddt - 709 | * xndt - 710 | * xomi - 711 | * 712 | * coupling : 713 | * none - 714 | * 715 | * references : 716 | * hoots, roehrich, norad spacetrack report #3 1980 717 | * hoots, norad spacetrack report #6 1986 718 | * hoots, schumacher and glover 2004 719 | * vallado, crawford, hujsak, kelso 2006 720 | ----------------------------------------------------------------------------*/ 721 | 722 | CSGP4_DECORATOR void dspace 723 | ( 724 | int irez, 725 | SGPF d2201, SGPF d2211, SGPF d3210, SGPF d3222, SGPF d4410, 726 | SGPF d4422, SGPF d5220, SGPF d5232, SGPF d5421, SGPF d5433, 727 | SGPF dedt, SGPF del1, SGPF del2, SGPF del3, SGPF didt, 728 | SGPF dmdt, SGPF dnodt, SGPF domdt, SGPF argpo, SGPF argpdot, 729 | SGPF t, SGPF tc, SGPF gsto, SGPF xfact, SGPF xlamo, 730 | SGPF no, 731 | SGPF CSGP4_OUT atime, SGPF CSGP4_OUT em, SGPF CSGP4_OUT argpm, SGPF CSGP4_OUT inclm, SGPF CSGP4_OUT xli, 732 | SGPF CSGP4_OUT mm, SGPF CSGP4_OUT xni, SGPF CSGP4_OUT nodem, SGPF CSGP4_OUT dndt, SGPF CSGP4_OUT nm 733 | ) 734 | { 735 | const SGPF twopi = 2.0 * SGPPI; 736 | int iretn; //, iret; 737 | SGPF delt, ft, theta, x2li, x2omi, xl, xldot, xnddt, xndt, xomi, g22, g32, 738 | g44, g52, g54, fasx2, fasx4, fasx6, rptim, step2, stepn, stepp; 739 | 740 | fasx2 = 0.13130908; 741 | fasx4 = 2.8843198; 742 | fasx6 = 0.37448087; 743 | g22 = 5.7686396; 744 | g32 = 0.95240898; 745 | g44 = 1.8014998; 746 | g52 = 1.0508330; 747 | g54 = 4.4108898; 748 | rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 749 | stepp = 720.0; 750 | stepn = -720.0; 751 | step2 = 259200.0; 752 | 753 | /* ----------- calculate deep space resonance effects ----------- */ 754 | *dndt = 0.0; 755 | theta = FMOD( (gsto + tc * rptim), twopi ); 756 | *em = *em + dedt * t; 757 | 758 | *inclm = *inclm + didt * t; 759 | *argpm = *argpm + domdt * t; 760 | *nodem = *nodem + dnodt * t; 761 | *mm = *mm + dmdt * t; 762 | 763 | // sgp4fix for negative inclinations 764 | // the following if statement should be commented out 765 | // if (inclm < 0.0) 766 | // { 767 | // inclm = -inclm; 768 | // argpm = argpm - Math.PI; 769 | // nodem = nodem + Math.PI; 770 | // } 771 | 772 | /* - update resonances : numerical (euler-maclaurin) integration - */ 773 | /* ------------------------- epoch restart ---------------------- */ 774 | // sgp4fix for propagator problems 775 | // the following integration works for negative time steps and periods 776 | // the specific changes are unknown because the original code was so convoluted 777 | // sgp4fix set initial values for c# 778 | xndt = 0.0; 779 | xnddt = 0.0; 780 | xldot = 0.0; 781 | 782 | // sgp4fix take out atime = 0.0 and fix for faster operation 783 | ft = 0.0; 784 | if (irez != 0) 785 | { 786 | // sgp4fix streamline check 787 | if ((*atime == 0.0) || (t * *atime <= 0.0) || (FABS(t) < FABS(*atime))) 788 | { 789 | *atime = 0.0; 790 | *xni = no; 791 | *xli = xlamo; 792 | } 793 | // sgp4fix move check outside loop 794 | if (t > 0.0) 795 | delt = stepp; 796 | else 797 | delt = stepn; 798 | 799 | iretn = 381; // added for do loop 800 | //iret = 0; // added for loop 801 | while (iretn == 381) 802 | { 803 | /* ------------------- dot terms calculated ------------- */ 804 | /* ----------- near - synchronous resonance terms ------- */ 805 | if (irez != 2) 806 | { 807 | xndt = del1 * SIN(*xli - fasx2) + del2 * SIN(2.0 * (*xli - fasx4)) + 808 | del3 * SIN(3.0 * (*xli - fasx6)); 809 | xldot = *xni + xfact; 810 | xnddt = del1 * COS(*xli - fasx2) + 811 | 2.0 * del2 * COS(2.0 * (*xli - fasx4)) + 812 | 3.0 * del3 * COS(3.0 * (*xli - fasx6)); 813 | xnddt = xnddt * xldot; 814 | } 815 | else 816 | { 817 | /* --------- near - half-day resonance terms -------- */ 818 | xomi = argpo + argpdot * *atime; 819 | x2omi = xomi + xomi; 820 | x2li = *xli + *xli; 821 | xndt = d2201 * SIN(x2omi + *xli - g22) + d2211 * SIN(*xli - g22) + 822 | d3210 * SIN(xomi + *xli - g32) + d3222 * SIN(-xomi + *xli - g32) + 823 | d4410 * SIN(x2omi + x2li - g44) + d4422 * SIN(x2li - g44) + 824 | d5220 * SIN(xomi + *xli - g52) + d5232 * SIN(-xomi + *xli - g52) + 825 | d5421 * SIN(xomi + x2li - g54) + d5433 * SIN(-xomi + x2li - g54); 826 | xldot = *xni + xfact; 827 | xnddt = d2201 * COS(x2omi + *xli - g22) + d2211 * COS(*xli - g22) + 828 | d3210 * COS(xomi + *xli - g32) + d3222 * COS(-xomi + *xli - g32) + 829 | d5220 * COS(xomi + *xli - g52) + d5232 * COS(-xomi + *xli - g52) + 830 | 2.0 * (d4410 * COS(x2omi + x2li - g44) + 831 | d4422 * COS(x2li - g44) + d5421 * COS(xomi + x2li - g54) + 832 | d5433 * COS(-xomi + x2li - g54)); 833 | xnddt = xnddt * xldot; 834 | } 835 | 836 | /* ----------------------- integrator ------------------- */ 837 | // sgp4fix move end checks to end of routine 838 | if (FABS(t - *atime) >= stepp) 839 | { 840 | //iret = 0; 841 | iretn = 381; 842 | } 843 | else // exit here 844 | { 845 | ft = t - *atime; 846 | iretn = 0; 847 | } 848 | 849 | if (iretn == 381) 850 | { 851 | *xli = *xli + xldot * delt + xndt * step2; 852 | *xni = *xni + xndt * delt + xnddt * step2; 853 | *atime = *atime + delt; 854 | } 855 | } // while iretn = 381 856 | 857 | *nm = *xni + xndt * ft + xnddt * ft * ft * 0.5; 858 | xl = *xli + xldot * ft + xndt * ft * ft * 0.5; 859 | if (irez != 1) 860 | { 861 | *mm = xl - 2.0 * *nodem + 2.0 * theta; 862 | *dndt = *nm - no; 863 | } 864 | else 865 | { 866 | *mm = xl - *nodem - *argpm + theta; 867 | *dndt = *nm - no; 868 | } 869 | *nm = no + *dndt; 870 | } 871 | 872 | //#include "debug4.cpp" 873 | } // end dsspace 874 | 875 | #if CSGP4_INIT 876 | 877 | /*----------------------------------------------------------------------------- 878 | * 879 | * procedure dsinit 880 | * 881 | * this procedure provides deep space contributions to mean motion dot due 882 | * to geopotential resonance with half day and one day orbits. 883 | * 884 | * author : david vallado 719-573-2600 28 jun 2005 885 | * translated to c: charles lohr 2024-04-28 886 | * 887 | * inputs : 888 | * xke - reciprocal of tumin 889 | * cosim, sinim- 890 | * emsq - eccentricity squared 891 | * argpo - argument of perigee 892 | * s1, s2, s3, s4, s5 - 893 | * ss1, ss2, ss3, ss4, ss5 - 894 | * sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - 895 | * t - time 896 | * tc - 897 | * gsto - greenwich sidereal time rad 898 | * mo - mean anomaly 899 | * mdot - mean anomaly dot (rate) 900 | * no - mean motion 901 | * nodeo - right ascension of ascending node 902 | * nodedot - right ascension of ascending node dot (rate) 903 | * xpidot - 904 | * z1, z3, z11, z13, z21, z23, z31, z33 - 905 | * eccm - eccentricity 906 | * argpm - argument of perigee 907 | * inclm - inclination 908 | * mm - mean anomaly 909 | * xn - mean motion 910 | * nodem - right ascension of ascending node 911 | * 912 | * outputs : 913 | * em - eccentricity 914 | * argpm - argument of perigee 915 | * inclm - inclination 916 | * mm - mean anomaly 917 | * nm - mean motion 918 | * nodem - right ascension of ascending node 919 | * irez - flag for resonance 0-none, 1-one day, 2-half day 920 | * atime - 921 | * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 922 | * dedt - 923 | * didt - 924 | * dmdt - 925 | * dndt - 926 | * dnodt - 927 | * domdt - 928 | * del1, del2, del3 - 929 | * ses , sghl , sghs , sgs , shl , shs , sis , sls 930 | * theta - 931 | * xfact - 932 | * xlamo - 933 | * xli - 934 | * xni 935 | * 936 | * locals : 937 | * ainv2 - 938 | * aonv - 939 | * cosisq - 940 | * eoc - 941 | * f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - 942 | * g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - 943 | * sini2 - 944 | * temp - 945 | * temp1 - 946 | * theta - 947 | * xno2 - 948 | * 949 | * coupling : 950 | * getgravconst- no longer used 951 | * 952 | * references : 953 | * hoots, roehrich, norad spacetrack report #3 1980 954 | * hoots, norad spacetrack report #6 1986 955 | * hoots, schumacher and glover 2004 956 | * vallado, crawford, hujsak, kelso 2006 957 | ----------------------------------------------------------------------------*/ 958 | 959 | CSGP4_DECORATOR void dsinit 960 | ( 961 | // sgp4fix just send in xke as a constant and eliminate getgravconst call 962 | // gravconsttype whichconst, 963 | SGPF xke, 964 | SGPF cosim, SGPF emsq, SGPF argpo, SGPF s1, SGPF s2, 965 | SGPF s3, SGPF s4, SGPF s5, SGPF sinim, SGPF ss1, 966 | SGPF ss2, SGPF ss3, SGPF ss4, SGPF ss5, SGPF sz1, 967 | SGPF sz3, SGPF sz11, SGPF sz13, SGPF sz21, SGPF sz23, 968 | SGPF sz31, SGPF sz33, SGPF t, SGPF tc, SGPF gsto, 969 | SGPF mo, SGPF mdot, SGPF no, SGPF nodeo, SGPF nodedot, 970 | SGPF xpidot, SGPF z1, SGPF z3, SGPF z11, SGPF z13, 971 | SGPF z21, SGPF z23, SGPF z31, SGPF z33, SGPF ecco, 972 | SGPF eccsq, SGPF CSGP4_OUT em, SGPF CSGP4_OUT argpm, SGPF CSGP4_OUT inclm, SGPF CSGP4_OUT mm, 973 | SGPF CSGP4_OUT nm, SGPF CSGP4_OUT nodem, 974 | int * irez, 975 | SGPF CSGP4_OUT atime, SGPF CSGP4_OUT d2201, SGPF CSGP4_OUT d2211, SGPF CSGP4_OUT d3210, SGPF CSGP4_OUT d3222, 976 | SGPF CSGP4_OUT d4410, SGPF CSGP4_OUT d4422, SGPF CSGP4_OUT d5220, SGPF CSGP4_OUT d5232, SGPF CSGP4_OUT d5421, 977 | SGPF CSGP4_OUT d5433, SGPF CSGP4_OUT dedt, SGPF CSGP4_OUT didt, SGPF CSGP4_OUT dmdt, SGPF CSGP4_OUT dndt, 978 | SGPF CSGP4_OUT dnodt, SGPF CSGP4_OUT domdt, SGPF CSGP4_OUT del1, SGPF CSGP4_OUT del2, SGPF CSGP4_OUT del3, 979 | SGPF CSGP4_OUT xfact, SGPF CSGP4_OUT xlamo, SGPF CSGP4_OUT xli, SGPF CSGP4_OUT xni 980 | ) 981 | { 982 | /* --------------------- local variables ------------------------ */ 983 | const SGPF twopi = 2.0 * SGPPI; 984 | 985 | SGPF ainv2, aonv = 0.0, cosisq, eoc, f220, f221, f311, 986 | f321, f322, f330, f441, f442, f522, f523, 987 | f542, f543, g200, g201, g211, g300, g310, 988 | g322, g410, g422, g520, g521, g532, g533, 989 | ses, sgs, sghl, sghs, shs, shll, sis, 990 | sini2, sls, temp, temp1, theta, xno2, q22, 991 | q31, q33, root22, root44, root54, rptim, root32, 992 | root52, x2o3, znl, emo, zns, emsqo; 993 | 994 | q22 = 1.7891679e-6; 995 | q31 = 2.1460748e-6; 996 | q33 = 2.2123015e-7; 997 | root22 = 1.7891679e-6; 998 | root44 = 7.3636953e-9; 999 | root54 = 2.1765803e-9; 1000 | rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 1001 | root32 = 3.7393792e-7; 1002 | root52 = 1.1428639e-7; 1003 | x2o3 = 2.0 / 3.0; 1004 | znl = 1.5835218e-4; 1005 | zns = 1.19459e-5; 1006 | 1007 | // sgp4fix identify constants and allow alternate values 1008 | // just xke is used here so pass it in rather than have multiple calls 1009 | // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1010 | 1011 | /* -------------------- deep space initialization ------------ */ 1012 | *irez = 0; 1013 | if ((*nm < 0.0052359877) && (*nm > 0.0034906585)) 1014 | *irez = 1; 1015 | if ((*nm >= 8.26e-3) && (*nm <= 9.24e-3) && (*em >= 0.5)) 1016 | *irez = 2; 1017 | 1018 | /* ------------------------ do solar terms ------------------- */ 1019 | ses = ss1 * zns * ss5; 1020 | sis = ss2 * zns * (sz11 + sz13); 1021 | sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); 1022 | sghs = ss4 * zns * (sz31 + sz33 - 6.0); 1023 | shs = -zns * ss2 * (sz21 + sz23); 1024 | // sgp4fix for 180 deg incl 1025 | if ((*inclm < 5.2359877e-2) || (*inclm > SGPPI - 5.2359877e-2)) 1026 | shs = 0.0; 1027 | if (sinim != 0.0) 1028 | shs = shs / sinim; 1029 | sgs = sghs - cosim * shs; 1030 | 1031 | /* ------------------------- do lunar terms ------------------ */ 1032 | *dedt = ses + s1 * znl * s5; 1033 | *didt = sis + s2 * znl * (z11 + z13); 1034 | *dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); 1035 | sghl = s4 * znl * (z31 + z33 - 6.0); 1036 | shll = -znl * s2 * (z21 + z23); 1037 | // sgp4fix for 180 deg incl 1038 | if ((*inclm < 5.2359877e-2) || (*inclm > SGPPI - 5.2359877e-2)) 1039 | shll = 0.0; 1040 | *domdt = sgs + sghl; 1041 | *dnodt = shs; 1042 | if (sinim != 0.0) 1043 | { 1044 | *domdt = *domdt - cosim / sinim * shll; 1045 | *dnodt = *dnodt + shll / sinim; 1046 | } 1047 | 1048 | /* ----------- calculate deep space resonance effects -------- */ 1049 | *dndt = 0.0; 1050 | theta = FMOD((gsto + tc * rptim), twopi ); 1051 | *em = *em + *dedt * t; 1052 | *inclm = *inclm + *didt * t; 1053 | *argpm = *argpm + *domdt * t; 1054 | *nodem = *nodem + *dnodt * t; 1055 | *mm = *mm + *dmdt * t; 1056 | // sgp4fix for negative inclinations 1057 | // the following if statement should be commented out 1058 | //if (inclm < 0.0) 1059 | // { 1060 | // inclm = -inclm; 1061 | // argpm = argpm - Math.PI; 1062 | // nodem = nodem + Math.PI; 1063 | // } 1064 | 1065 | /* -------------- initialize the resonance terms ------------- */ 1066 | if (*irez != 0) 1067 | { 1068 | aonv = POW(*nm / xke, x2o3); 1069 | 1070 | /* ---------- geopotential resonance for 12 hour orbits ------ */ 1071 | if (*irez == 2) 1072 | { 1073 | cosisq = cosim * cosim; 1074 | emo = *em; 1075 | *em = ecco; 1076 | emsqo = emsq; 1077 | emsq = eccsq; 1078 | eoc = *em * emsq; 1079 | g201 = -0.306 - (*em - 0.64) * 0.440; 1080 | 1081 | if (*em <= 0.65) 1082 | { 1083 | g211 = 3.616 - 13.2470 * *em + 16.2900 * emsq; 1084 | g310 = -19.302 + 117.3900 * *em - 228.4190 * emsq + 156.5910 * eoc; 1085 | g322 = -18.9068 + 109.7927 * *em - 214.6334 * emsq + 146.5816 * eoc; 1086 | g410 = -41.122 + 242.6940 * *em - 471.0940 * emsq + 313.9530 * eoc; 1087 | g422 = -146.407 + 841.8800 * *em - 1629.014 * emsq + 1083.4350 * eoc; 1088 | g520 = -532.114 + 3017.977 * *em - 5740.032 * emsq + 3708.2760 * eoc; 1089 | } 1090 | else 1091 | { 1092 | g211 = -72.099 + 331.819 * *em - 508.738 * emsq + 266.724 * eoc; 1093 | g310 = -346.844 + 1582.851 * *em - 2415.925 * emsq + 1246.113 * eoc; 1094 | g322 = -342.585 + 1554.908 * *em - 2366.899 * emsq + 1215.972 * eoc; 1095 | g410 = -1052.797 + 4758.686 * *em - 7193.992 * emsq + 3651.957 * eoc; 1096 | g422 = -3581.690 + 16178.110 * *em - 24462.770 * emsq + 12422.520 * eoc; 1097 | if (*em > 0.715) 1098 | g520 = -5149.66 + 29936.92 * *em - 54087.36 * emsq + 31324.56 * eoc; 1099 | else 1100 | g520 = 1464.74 - 4664.75 * *em + 3763.64 * emsq; 1101 | } 1102 | if (*em < 0.7) 1103 | { 1104 | g533 = -919.22770 + 4988.6100 * *em - 9064.7700 * emsq + 5542.21 * eoc; 1105 | g521 = -822.71072 + 4568.6173 * *em - 8491.4146 * emsq + 5337.524 * eoc; 1106 | g532 = -853.66600 + 4690.2500 * *em - 8624.7700 * emsq + 5341.4 * eoc; 1107 | } 1108 | else 1109 | { 1110 | g533 = -37995.780 + 161616.52 * *em - 229838.20 * emsq + 109377.94 * eoc; 1111 | g521 = -51752.104 + 218913.95 * *em - 309468.16 * emsq + 146349.42 * eoc; 1112 | g532 = -40023.880 + 170470.89 * *em - 242699.48 * emsq + 115605.82 * eoc; 1113 | } 1114 | 1115 | sini2 = sinim * sinim; 1116 | f220 = 0.75 * (1.0 + 2.0 * cosim + cosisq); 1117 | f221 = 1.5 * sini2; 1118 | f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); 1119 | f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); 1120 | f441 = 35.0 * sini2 * f220; 1121 | f442 = 39.3750 * sini2 * sini2; 1122 | f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim - 5.0 * cosisq) + 1123 | 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq)); 1124 | f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + 1125 | 10.0 * cosisq) + 6.56250012 * (1.0 + 2.0 * cosim - 3.0 * cosisq)); 1126 | f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim + cosisq * 1127 | (-12.0 + 8.0 * cosim + 10.0 * cosisq)); 1128 | f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim + cosisq * 1129 | (12.0 + 8.0 * cosim - 10.0 * cosisq)); 1130 | xno2 = *nm * *nm; 1131 | ainv2 = aonv * aonv; 1132 | temp1 = 3.0 * xno2 * ainv2; 1133 | temp = temp1 * root22; 1134 | *d2201 = temp * f220 * g201; 1135 | *d2211 = temp * f221 * g211; 1136 | temp1 = temp1 * aonv; 1137 | temp = temp1 * root32; 1138 | *d3210 = temp * f321 * g310; 1139 | *d3222 = temp * f322 * g322; 1140 | temp1 = temp1 * aonv; 1141 | temp = 2.0 * temp1 * root44; 1142 | *d4410 = temp * f441 * g410; 1143 | *d4422 = temp * f442 * g422; 1144 | temp1 = temp1 * aonv; 1145 | temp = temp1 * root52; 1146 | *d5220 = temp * f522 * g520; 1147 | *d5232 = temp * f523 * g532; 1148 | temp = 2.0 * temp1 * root54; 1149 | *d5421 = temp * f542 * g521; 1150 | *d5433 = temp * f543 * g533; 1151 | *xlamo = FMOD( (mo + nodeo + nodeo - theta - theta), twopi ); 1152 | *xfact = mdot + *dmdt + 2.0 * (nodedot + *dnodt - rptim) - no; 1153 | *em = emo; 1154 | emsq = emsqo; 1155 | } 1156 | 1157 | /* ---------------- synchronous resonance terms -------------- */ 1158 | if (*irez == 1) 1159 | { 1160 | g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); 1161 | g310 = 1.0 + 2.0 * emsq; 1162 | g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); 1163 | f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); 1164 | f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); 1165 | f330 = 1.0 + cosim; 1166 | f330 = 1.875 * f330 * f330 * f330; 1167 | *del1 = 3.0 * *nm * *nm * aonv * aonv; 1168 | *del2 = 2.0 * *del1 * f220 * g200 * q22; 1169 | *del3 = 3.0 * *del1 * f330 * g300 * q33 * aonv; 1170 | *del1 = *del1 * f311 * g310 * q31 * aonv; 1171 | *xlamo = FMOD(mo + nodeo + argpo - theta, twopi ); 1172 | *xfact = mdot + xpidot - rptim + *dmdt + *domdt + *dnodt - no; 1173 | } 1174 | 1175 | /* ------------ for sgp4, initialize the integrator ---------- */ 1176 | *xli = *xlamo; 1177 | *xni = no; 1178 | *atime = 0.0; 1179 | *nm = no + *dndt; 1180 | } 1181 | 1182 | //#include "debug3.cpp" 1183 | } // end dsinit 1184 | 1185 | /*----------------------------------------------------------------------------- 1186 | * 1187 | * procedure initl 1188 | * 1189 | * this procedure initializes the spg4 propagator. all the initialization is 1190 | * consolidated here instead of having multiple loops inside other routines. 1191 | * 1192 | * author : david vallado 719-573-2600 28 jun 2005 1193 | * translated to c: charles lohr 2024-04-28 1194 | * 1195 | * inputs : 1196 | * satn - satellite number - not needed, placed in satrec 1197 | * xke - reciprocal of tumin 1198 | * j2 - j2 zonal harmonic 1199 | * ecco - eccentricity 0.0 - 1.0 1200 | * epoch - epoch time in days from jan 0, 1950. 0 hr 1201 | * inclo - inclination of satellite 1202 | * no - mean motion of satellite 1203 | * 1204 | * outputs : 1205 | * ainv - 1.0 / a 1206 | * ao - semi major axis 1207 | * con41 - 1208 | * con42 - 1.0 - 5.0 cos(i) 1209 | * cosio - cosine of inclination 1210 | * cosio2 - cosio squared 1211 | * eccsq - eccentricity squared 1212 | * method - flag for deep space 'd', 'n' 1213 | * omeosq - 1.0 - ecco * ecco 1214 | * posq - semi-parameter squared 1215 | * rp - radius of perigee 1216 | * rteosq - square root of (1.0 - ecco*ecco) 1217 | * sinio - sine of inclination 1218 | * gsto - gst at time of observation rad 1219 | * no - mean motion of satellite 1220 | * 1221 | * locals : 1222 | * ak - 1223 | * d1 - 1224 | * del - 1225 | * adel - 1226 | * po - 1227 | * 1228 | * coupling : 1229 | * getgravconst- no longer used 1230 | * gstime - find greenwich sidereal time from the julian date 1231 | * 1232 | * references : 1233 | * hoots, roehrich, norad spacetrack report #3 1980 1234 | * hoots, norad spacetrack report #6 1986 1235 | * hoots, schumacher and glover 2004 1236 | * vallado, crawford, hujsak, kelso 2006 1237 | ----------------------------------------------------------------------------*/ 1238 | 1239 | CSGP4_DECORATOR void initl 1240 | ( 1241 | // sgp4fix satn not needed. include in satrec in case needed later 1242 | // int satn, 1243 | // sgp4fix just pass in xke and j2 1244 | // gravconsttype whichconst, 1245 | SGPF xke, SGPF j2, 1246 | SGPF ecco, SGPF epoch, SGPF inclo, SGPF no_kozai, 1247 | 1248 | // Output params: 1249 | char * method, 1250 | SGPF CSGP4_OUT ainv, SGPF CSGP4_OUT ao, SGPF CSGP4_OUT con41, SGPF CSGP4_OUT con42, SGPF CSGP4_OUT cosio, 1251 | SGPF CSGP4_OUT cosio2, SGPF CSGP4_OUT eccsq, SGPF CSGP4_OUT omeosq, SGPF CSGP4_OUT posq, 1252 | SGPF CSGP4_OUT rp, SGPF CSGP4_OUT rteosq, SGPF CSGP4_OUT sinio, SGPF CSGP4_OUT gsto, 1253 | char opsmode, SGPF CSGP4_OUT no_unkozai 1254 | ) 1255 | { 1256 | /* --------------------- local variables ------------------------ */ 1257 | SGPF ak, d1, del, adel, po, x2o3; 1258 | 1259 | // sgp4fix use old way of finding gst 1260 | SGPF ds70; 1261 | SGPF ts70, tfrac, c1, thgr70, fk5r, c1p2p; 1262 | const SGPF twopi = 2.0 * SGPPI; 1263 | 1264 | /* ----------------------- earth constants ---------------------- */ 1265 | // sgp4fix identify constants and allow alternate values 1266 | // only xke and j2 are used here so pass them in directly 1267 | // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1268 | x2o3 = 2.0 / 3.0; 1269 | 1270 | /* ------------- calculate auxillary epoch quantities ---------- */ 1271 | *eccsq = ecco * ecco; 1272 | *omeosq = 1.0 - *eccsq; 1273 | *rteosq = SQRT(*omeosq); 1274 | *cosio = COS(inclo); 1275 | *cosio2 = *cosio * *cosio; 1276 | 1277 | /* ------------------ un-kozai the mean motion ----------------- */ 1278 | ak = POW(xke / no_kozai, x2o3); 1279 | d1 = 0.75 * j2 * (3.0 * *cosio2 - 1.0) / (*rteosq * *omeosq); 1280 | del = d1 / (ak * ak); 1281 | adel = ak * (1.0 - del * del - del * 1282 | (1.0 / 3.0 + 134.0 * del * del / 81.0)); 1283 | del = d1 / (adel * adel); 1284 | *no_unkozai = no_kozai / (1.0 + del); 1285 | 1286 | *ao = POW(xke / (*no_unkozai), x2o3); 1287 | *sinio = SIN(inclo); 1288 | po = *ao * *omeosq; 1289 | *con42 = 1.0 - 5.0 * *cosio2; 1290 | *con41 = -*con42 - *cosio2 - *cosio2; 1291 | *ainv = 1.0 / *ao; 1292 | *posq = po * po; 1293 | *rp = *ao * (1.0 - ecco); 1294 | *method = 'n'; 1295 | 1296 | // sgp4fix modern approach to finding sidereal time 1297 | if (opsmode == 'a') 1298 | { 1299 | // sgp4fix use old way of finding gst 1300 | // count integer number of days from 0 jan 1970 1301 | ts70 = epoch - 7305.0; 1302 | ds70 = FLOOR(ts70 + 1.0e-8); 1303 | tfrac = ts70 - ds70; 1304 | // find greenwich location at epoch 1305 | c1 = 1.72027916940703639e-2; 1306 | thgr70 = 1.7321343856509374; 1307 | fk5r = 5.07551419432269442e-15; 1308 | c1p2p = c1 + twopi; 1309 | *gsto = FMOD(thgr70 + c1 * ds70 + c1p2p * tfrac + ts70 * ts70 * fk5r, twopi); 1310 | if (*gsto < 0.0) 1311 | *gsto = *gsto + twopi; 1312 | } 1313 | else 1314 | *gsto = gstime(epoch + 2433281.5); 1315 | 1316 | //#include "debug5.cpp" 1317 | } // end initl 1318 | 1319 | #endif 1320 | 1321 | /*----------------------------------------------------------------------------- 1322 | * 1323 | * procedure dscom 1324 | * 1325 | * this procedure provides deep space common items used by both the secular 1326 | * and periodics subroutines. input is provided as shown. this routine 1327 | * used to be called dpper, but the functions inside weren't well organized. 1328 | * 1329 | * author : david vallado 719-573-2600 28 jun 2005 1330 | * translated to c: charles lohr 2024-04-28 1331 | * 1332 | * inputs : 1333 | * epoch - 1334 | * ep - eccentricity 1335 | * argpp - argument of perigee 1336 | * tc - 1337 | * inclp - inclination 1338 | * nodep - right ascension of ascending node 1339 | * np - mean motion 1340 | * 1341 | * outputs : 1342 | * sinim , cosim , sinomm , cosomm , snodm , cnodm 1343 | * day - 1344 | * e3 - 1345 | * ee2 - 1346 | * em - eccentricity 1347 | * emsq - eccentricity squared 1348 | * gam - 1349 | * peo - 1350 | * pgho - 1351 | * pho - 1352 | * pinco - 1353 | * plo - 1354 | * rtemsq - 1355 | * se2, se3 - 1356 | * sgh2, sgh3, sgh4 - 1357 | * sh2, sh3, si2, si3, sl2, sl3, sl4 - 1358 | * s1, s2, s3, s4, s5, s6, s7 - 1359 | * ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - 1360 | * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - 1361 | * xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 1362 | * nm - mean motion 1363 | * z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - 1364 | * zmol - 1365 | * zmos - 1366 | * 1367 | * locals : 1368 | * a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - 1369 | * betasq - 1370 | * cc - 1371 | * ctem, stem - 1372 | * x1, x2, x3, x4, x5, x6, x7, x8 - 1373 | * xnodce - 1374 | * xnoi - 1375 | * zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , 1376 | * zcosi , zsini , zcosil , zsinil , 1377 | * zx - 1378 | * zy - 1379 | * 1380 | * coupling : 1381 | * none. 1382 | * 1383 | * references : 1384 | * hoots, roehrich, norad spacetrack report #3 1980 1385 | * hoots, norad spacetrack report #6 1986 1386 | * hoots, schumacher and glover 2004 1387 | * vallado, crawford, hujsak, kelso 2006 1388 | ----------------------------------------------------------------------------*/ 1389 | 1390 | CSGP4_DECORATOR void dscom 1391 | ( 1392 | SGPF epoch, SGPF ep, SGPF argpp, SGPF tc, SGPF inclp, 1393 | SGPF nodep, SGPF np, 1394 | SGPF CSGP4_OUT snodm, SGPF CSGP4_OUT cnodm, SGPF CSGP4_OUT sinim, SGPF CSGP4_OUT cosim, SGPF CSGP4_OUT sinomm, 1395 | SGPF CSGP4_OUT cosomm, SGPF CSGP4_OUT day, SGPF CSGP4_OUT e3, SGPF CSGP4_OUT ee2, SGPF CSGP4_OUT em, 1396 | SGPF CSGP4_OUT emsq, SGPF CSGP4_OUT gam, SGPF CSGP4_OUT peo, SGPF CSGP4_OUT pgho, SGPF CSGP4_OUT pho, 1397 | SGPF CSGP4_OUT pinco, SGPF CSGP4_OUT plo, SGPF CSGP4_OUT rtemsq, SGPF CSGP4_OUT se2, SGPF CSGP4_OUT se3, 1398 | SGPF CSGP4_OUT sgh2, SGPF CSGP4_OUT sgh3, SGPF CSGP4_OUT sgh4, SGPF CSGP4_OUT sh2, SGPF CSGP4_OUT sh3, 1399 | SGPF CSGP4_OUT si2, SGPF CSGP4_OUT si3, SGPF CSGP4_OUT sl2, SGPF CSGP4_OUT sl3, SGPF CSGP4_OUT sl4, 1400 | SGPF CSGP4_OUT s1, SGPF CSGP4_OUT s2, SGPF CSGP4_OUT s3, SGPF CSGP4_OUT s4, SGPF CSGP4_OUT s5, 1401 | SGPF CSGP4_OUT s6, SGPF CSGP4_OUT s7, SGPF CSGP4_OUT ss1, SGPF CSGP4_OUT ss2, SGPF CSGP4_OUT ss3, 1402 | SGPF CSGP4_OUT ss4, SGPF CSGP4_OUT ss5, SGPF CSGP4_OUT ss6, SGPF CSGP4_OUT ss7, SGPF CSGP4_OUT sz1, 1403 | SGPF CSGP4_OUT sz2, SGPF CSGP4_OUT sz3, SGPF CSGP4_OUT sz11, SGPF CSGP4_OUT sz12, SGPF CSGP4_OUT sz13, 1404 | SGPF CSGP4_OUT sz21, SGPF CSGP4_OUT sz22, SGPF CSGP4_OUT sz23, SGPF CSGP4_OUT sz31, SGPF CSGP4_OUT sz32, 1405 | SGPF CSGP4_OUT sz33, SGPF CSGP4_OUT xgh2, SGPF CSGP4_OUT xgh3, SGPF CSGP4_OUT xgh4, SGPF CSGP4_OUT xh2, 1406 | SGPF CSGP4_OUT xh3, SGPF CSGP4_OUT xi2, SGPF CSGP4_OUT xi3, SGPF CSGP4_OUT xl2, SGPF CSGP4_OUT xl3, 1407 | SGPF CSGP4_OUT xl4, SGPF CSGP4_OUT nm, SGPF CSGP4_OUT z1, SGPF CSGP4_OUT z2, SGPF CSGP4_OUT z3, 1408 | SGPF CSGP4_OUT z11, SGPF CSGP4_OUT z12, SGPF CSGP4_OUT z13, SGPF CSGP4_OUT z21, SGPF CSGP4_OUT z22, 1409 | SGPF CSGP4_OUT z23, SGPF CSGP4_OUT z31, SGPF CSGP4_OUT z32, SGPF CSGP4_OUT z33, SGPF CSGP4_OUT zmol, 1410 | SGPF CSGP4_OUT zmos 1411 | ) 1412 | { 1413 | /* -------------------------- constants ------------------------- */ 1414 | const SGPF zes = 0.01675; 1415 | const SGPF zel = 0.05490; 1416 | const SGPF c1ss = 2.9864797e-6; 1417 | const SGPF c1l = 4.7968065e-7; 1418 | const SGPF zsinis = 0.39785416; 1419 | const SGPF zcosis = 0.91744867; 1420 | const SGPF zcosgs = 0.1945905; 1421 | const SGPF zsings = -0.98088458; 1422 | const SGPF twopi = 2.0 * SGPPI; 1423 | 1424 | /* --------------------- local variables ------------------------ */ 1425 | int lsflg; 1426 | SGPF a1, a2, a3, a4, a5, a6, a7, 1427 | a8, a9, a10, betasq, cc, ctem, stem, 1428 | x1, x2, x3, x4, x5, x6, x7, 1429 | x8, xnodce, xnoi, zcosg, zcosgl, zcosh, zcoshl, 1430 | zcosi, zcosil, zsing, zsingl, zsinh, zsinhl, zsini, 1431 | zsinil, zx, zy; 1432 | 1433 | // sgp4fix - initialize the parameters for c# 1434 | *ss1 = 0.0; 1435 | *ss2 = 0.0; 1436 | *ss3 = 0.0; 1437 | *ss4 = 0.0; 1438 | *ss5 = 0.0; 1439 | *ss6 = 0.0; 1440 | *ss7 = 0.0; 1441 | *s1 = 0.0; 1442 | *s2 = 0.0; 1443 | *s3 = 0.0; 1444 | *s4 = 0.0; 1445 | *s5 = 0.0; 1446 | *s6 = 0.0; 1447 | *s7 = 0.0; 1448 | *sz11 = 0.0; 1449 | *sz12 = 0.0; 1450 | *sz13 = 0.0; 1451 | *sz1 = 0.0; 1452 | *sz2 = 0.0; 1453 | *sz3 = 0.0; 1454 | *sz21 = 0.0; 1455 | *sz22 = 0.0; 1456 | *sz23 = 0.0; 1457 | *sz31 = 0.0; 1458 | *sz32 = 0.0; 1459 | *sz33 = 0.0; 1460 | *z13 = 0.0; 1461 | *z21 = 0.0; 1462 | *z1 = 0.0; 1463 | *z2 = 0.0; 1464 | *z3 = 0.0; 1465 | *z11 = 0.0; 1466 | *z12 = 0.0; 1467 | *z31 = 0.0; 1468 | *z21 = 0.0; 1469 | *z22 = 0.0; 1470 | *z23 = 0.0; 1471 | *z32 = 0.0; 1472 | *z33 = 0.0; 1473 | 1474 | *nm = np; 1475 | *em = ep; 1476 | *snodm = SIN(nodep); 1477 | *cnodm = COS(nodep); 1478 | *sinomm = SIN(argpp); 1479 | *cosomm = COS(argpp); 1480 | *sinim = SIN(inclp); 1481 | *cosim = COS(inclp); 1482 | *emsq = *em * *em; 1483 | betasq = 1.0 - *emsq; 1484 | *rtemsq = SQRT(betasq); 1485 | 1486 | /* ----------------- initialize lunar solar terms --------------- */ 1487 | *peo = 0.0; 1488 | *pinco = 0.0; 1489 | *plo = 0.0; 1490 | *pgho = 0.0; 1491 | *pho = 0.0; 1492 | *day = epoch + 18261.5 + tc / 1440.0; 1493 | xnodce = FMOD(4.5236020 - 9.2422029e-4 * *day, twopi); 1494 | stem = SIN(xnodce); 1495 | ctem = COS(xnodce); 1496 | zcosil = 0.91375164 - 0.03568096 * ctem; 1497 | zsinil = SQRT(1.0 - zcosil * zcosil); 1498 | zsinhl = 0.089683511 * stem / zsinil; 1499 | zcoshl = SQRT(1.0 - zsinhl * zsinhl); 1500 | *gam = 5.8351514 + 0.0019443680 * *day; 1501 | zx = 0.39785416 * stem / zsinil; 1502 | zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; 1503 | zx = ATAN2(zx, zy); 1504 | zx = *gam + zx - xnodce; 1505 | zcosgl = COS(zx); 1506 | zsingl = SIN(zx); 1507 | 1508 | /* ------------------------- do solar terms --------------------- */ 1509 | zcosg = zcosgs; 1510 | zsing = zsings; 1511 | zcosi = zcosis; 1512 | zsini = zsinis; 1513 | zcosh = *cnodm; 1514 | zsinh = *snodm; 1515 | cc = c1ss; 1516 | xnoi = 1.0 / *nm; 1517 | 1518 | for (lsflg = 1; lsflg <= 2; lsflg++) 1519 | { 1520 | a1 = zcosg * zcosh + zsing * zcosi * zsinh; 1521 | a3 = -zsing * zcosh + zcosg * zcosi * zsinh; 1522 | a7 = -zcosg * zsinh + zsing * zcosi * zcosh; 1523 | a8 = zsing * zsini; 1524 | a9 = zsing * zsinh + zcosg * zcosi * zcosh; 1525 | a10 = zcosg * zsini; 1526 | a2 = *cosim * a7 + *sinim * a8; 1527 | a4 = *cosim * a9 + *sinim * a10; 1528 | a5 = -*sinim * a7 + *cosim * a8; 1529 | a6 = -*sinim * a9 + *cosim * a10; 1530 | 1531 | x1 = a1 * *cosomm + a2 * *sinomm; 1532 | x2 = a3 * *cosomm + a4 * *sinomm; 1533 | x3 = -a1 * *sinomm + a2 * *cosomm; 1534 | x4 = -a3 * *sinomm + a4 * *cosomm; 1535 | x5 = a5 * *sinomm; 1536 | x6 = a6 * *sinomm; 1537 | x7 = a5 * *cosomm; 1538 | x8 = a6 * *cosomm; 1539 | 1540 | *z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; 1541 | *z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; 1542 | *z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; 1543 | *z1 = 3.0 * (a1 * a1 + a2 * a2) + *z31 * *emsq; 1544 | *z2 = 6.0 * (a1 * a3 + a2 * a4) + *z32 * *emsq; 1545 | *z3 = 3.0 * (a3 * a3 + a4 * a4) + *z33 * *emsq; 1546 | *z11 = -6.0 * a1 * a5 + *emsq * (-24.0 * x1 * x7 - 6.0 * x3 * x5); 1547 | *z12 = -6.0 * (a1 * a6 + a3 * a5) + *emsq * 1548 | (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); 1549 | *z13 = -6.0 * a3 * a6 + *emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); 1550 | *z21 = 6.0 * a2 * a5 + *emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); 1551 | *z22 = 6.0 * (a4 * a5 + a2 * a6) + *emsq * 1552 | (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); 1553 | *z23 = 6.0 * a4 * a6 + *emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); 1554 | *z1 = *z1 + *z1 + betasq * *z31; 1555 | *z2 = *z2 + *z2 + betasq * *z32; 1556 | *z3 = *z3 + *z3 + betasq * *z33; 1557 | *s3 = cc * xnoi; 1558 | *s2 = -0.5 * *s3 / *rtemsq; 1559 | *s4 = *s3 * *rtemsq; 1560 | *s1 = -15.0 * *em * *s4; 1561 | *s5 = x1 * x3 + x2 * x4; 1562 | *s6 = x2 * x3 + x1 * x4; 1563 | *s7 = x2 * x4 - x1 * x3; 1564 | 1565 | /* ----------------------- do lunar terms ------------------- */ 1566 | if (lsflg == 1) 1567 | { 1568 | *ss1 = *s1; 1569 | *ss2 = *s2; 1570 | *ss3 = *s3; 1571 | *ss4 = *s4; 1572 | *ss5 = *s5; 1573 | *ss6 = *s6; 1574 | *ss7 = *s7; 1575 | *sz1 = *z1; 1576 | *sz2 = *z2; 1577 | *sz3 = *z3; 1578 | *sz11 = *z11; 1579 | *sz12 = *z12; 1580 | *sz13 = *z13; 1581 | *sz21 = *z21; 1582 | *sz22 = *z22; 1583 | *sz23 = *z23; 1584 | *sz31 = *z31; 1585 | *sz32 = *z32; 1586 | *sz33 = *z33; 1587 | zcosg = zcosgl; 1588 | zsing = zsingl; 1589 | zcosi = zcosil; 1590 | zsini = zsinil; 1591 | zcosh = zcoshl * *cnodm + zsinhl * *snodm; 1592 | zsinh = *snodm * zcoshl - *cnodm * zsinhl; 1593 | cc = c1l; 1594 | } 1595 | } 1596 | 1597 | *zmol = FMOD( (4.7199672 + 0.22997150 * *day - *gam), twopi ); 1598 | *zmos = FMOD( (6.2565837 + 0.017201977 * *day), twopi ); 1599 | 1600 | /* ------------------------ do solar terms ---------------------- */ 1601 | *se2 = 2.0 * *ss1 * *ss6; 1602 | *se3 = 2.0 * *ss1 * *ss7; 1603 | *si2 = 2.0 * *ss2 * *sz12; 1604 | *si3 = 2.0 * *ss2 * (*sz13 - *sz11); 1605 | *sl2 = -2.0 * *ss3 * *sz2; 1606 | *sl3 = -2.0 * *ss3 * (*sz3 - *sz1); 1607 | *sl4 = -2.0 * *ss3 * (-21.0 - 9.0 * *emsq) * zes; 1608 | *sgh2 = 2.0 * *ss4 * *sz32; 1609 | *sgh3 = 2.0 * *ss4 * (*sz33 - *sz31); 1610 | *sgh4 = -18.0 * *ss4 * zes; 1611 | *sh2 = -2.0 * *ss2 * *sz22; 1612 | *sh3 = -2.0 * *ss2 * (*sz23 - *sz21); 1613 | 1614 | /* ------------------------ do lunar terms ---------------------- */ 1615 | *ee2 = 2.0 * *s1 * *s6; 1616 | *e3 = 2.0 * *s1 * *s7; 1617 | *xi2 = 2.0 * *s2 * *z12; 1618 | *xi3 = 2.0 * *s2 * (*z13 - *z11); 1619 | *xl2 = -2.0 * *s3 * *z2; 1620 | *xl3 = -2.0 * *s3 * (*z3 - *z1); 1621 | *xl4 = -2.0 * *s3 * (-21.0 - 9.0 * *emsq) * zel; 1622 | *xgh2 = 2.0 * *s4 * *z32; 1623 | *xgh3 = 2.0 * *s4 * (*z33 - *z31); 1624 | *xgh4 = -18.0 * *s4 * zel; 1625 | *xh2 = -2.0 * *s2 * *z22; 1626 | *xh3 = -2.0 * *s2 * (*z23 - *z21); 1627 | 1628 | //#include "debug2.cpp" 1629 | } // end dscom 1630 | 1631 | 1632 | 1633 | /* ----------------------------------------------------------------------------- 1634 | * 1635 | * procedure dpper 1636 | * 1637 | * this procedure provides deep space long period periodic contributions 1638 | * to the mean elements. by design, these periodics are zero at epoch. 1639 | * this used to be dscom which included initialization, but it's really a 1640 | * recurring function. 1641 | * 1642 | * author : david vallado 719-573-2600 28 jun 2005 1643 | * translated to c: charles lohr 2024-04-28 1644 | * 1645 | * inputs : 1646 | * e3 - 1647 | * ee2 - 1648 | * peo - 1649 | * pgho - 1650 | * pho - 1651 | * pinco - 1652 | * plo - 1653 | * se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - 1654 | * t - 1655 | * xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 1656 | * zmol - 1657 | * zmos - 1658 | * ep - eccentricity 0.0 - 1.0 1659 | * inclo - inclination - needed for lyddane modification 1660 | * nodep - right ascension of ascending node 1661 | * argpp - argument of perigee 1662 | * mp - mean anomaly 1663 | * 1664 | * outputs : 1665 | * ep - eccentricity 0.0 - 1.0 1666 | * inclp - inclination 1667 | * nodep - right ascension of ascending node 1668 | * argpp - argument of perigee 1669 | * mp - mean anomaly 1670 | * 1671 | * locals : 1672 | * alfdp - 1673 | * betdp - 1674 | * cosip , sinip , cosop , sinop , 1675 | * dalf - 1676 | * dbet - 1677 | * dls - 1678 | * f2, f3 - 1679 | * pe - 1680 | * pgh - 1681 | * ph - 1682 | * pinc - 1683 | * pl - 1684 | * sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , 1685 | * sll , sls 1686 | * xls - 1687 | * xnoh - 1688 | * zf - 1689 | * zm - 1690 | * 1691 | * coupling : 1692 | * none. 1693 | * 1694 | * references : 1695 | * hoots, roehrich, norad spacetrack report #3 1980 1696 | * hoots, norad spacetrack report #6 1986 1697 | * hoots, schumacher and glover 2004 1698 | * vallado, crawford, hujsak, kelso 2006 1699 | ----------------------------------------------------------------------------*/ 1700 | 1701 | CSGP4_DECORATOR void dpper 1702 | ( 1703 | SGPF e3, SGPF ee2, SGPF peo, SGPF pgho, SGPF pho, 1704 | SGPF pinco, SGPF plo, SGPF se2, SGPF se3, SGPF sgh2, 1705 | SGPF sgh3, SGPF sgh4, SGPF sh2, SGPF sh3, SGPF si2, 1706 | SGPF si3, SGPF sl2, SGPF sl3, SGPF sl4, SGPF t, 1707 | SGPF xgh2, SGPF xgh3, SGPF xgh4, SGPF xh2, SGPF xh3, 1708 | SGPF xi2, SGPF xi3, SGPF xl2, SGPF xl3, SGPF xl4, 1709 | SGPF zmol, SGPF zmos, SGPF inclo, 1710 | char init, 1711 | SGPF CSGP4_OUT ep, SGPF CSGP4_OUT inclp, SGPF CSGP4_OUT nodep, SGPF CSGP4_OUT argpp, SGPF CSGP4_OUT mp, 1712 | char opsmode 1713 | ) 1714 | { 1715 | /* --------------------- local variables ------------------------ */ 1716 | const SGPF twopi = 2.0 * SGPPI; 1717 | SGPF alfdp, betdp, cosip, cosop, dalf, dbet, dls, 1718 | f2, f3, pe, pgh, ph, pinc, pl, 1719 | sel, ses, sghl, sghs, shll, shs, sil, 1720 | sinip, sinop, sinzf, sis, sll, sls, xls, 1721 | xnoh, zf, zm, zel, zes, znl, zns; 1722 | 1723 | /* ---------------------- constants ----------------------------- */ 1724 | zns = 1.19459e-5; 1725 | zes = 0.01675; 1726 | znl = 1.5835218e-4; 1727 | zel = 0.05490; 1728 | 1729 | /* --------------- calculate time varying periodics ----------- */ 1730 | zm = zmos + zns * t; 1731 | // be sure that the initial call has time set to zero 1732 | if (init == 'y') 1733 | zm = zmos; 1734 | zf = zm + 2.0 * zes * SIN(zm); 1735 | sinzf = SIN(zf); 1736 | f2 = 0.5 * sinzf * sinzf - 0.25; 1737 | f3 = -0.5 * sinzf * COS(zf); 1738 | ses = se2 * f2 + se3 * f3; 1739 | sis = si2 * f2 + si3 * f3; 1740 | sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; 1741 | sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; 1742 | shs = sh2 * f2 + sh3 * f3; 1743 | zm = zmol + znl * t; 1744 | if (init == 'y') 1745 | zm = zmol; 1746 | zf = zm + 2.0 * zel * SIN(zm); 1747 | sinzf = SIN(zf); 1748 | f2 = 0.5 * sinzf * sinzf - 0.25; 1749 | f3 = -0.5 * sinzf * COS(zf); 1750 | sel = ee2 * f2 + e3 * f3; 1751 | sil = xi2 * f2 + xi3 * f3; 1752 | sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; 1753 | sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; 1754 | shll = xh2 * f2 + xh3 * f3; 1755 | pe = ses + sel; 1756 | pinc = sis + sil; 1757 | pl = sls + sll; 1758 | pgh = sghs + sghl; 1759 | ph = shs + shll; 1760 | 1761 | if (init == 'n') 1762 | { 1763 | pe = pe - peo; 1764 | pinc = pinc - pinco; 1765 | pl = pl - plo; 1766 | pgh = pgh - pgho; 1767 | ph = ph - pho; 1768 | *inclp = *inclp + pinc; 1769 | *ep = *ep + pe; 1770 | sinip = SIN(*inclp); 1771 | cosip = COS(*inclp); 1772 | 1773 | /* ----------------- apply periodics directly ------------ */ 1774 | // sgp4fix for lyddane choice 1775 | // strn3 used original inclination - this is technically feasible 1776 | // gsfc used perturbed inclination - also technically feasible 1777 | // probably best to readjust the 0.2 limit value and limit discontinuity 1778 | // 0.2 rad = 11.45916 deg 1779 | // use next line for original strn3 approach and original inclination 1780 | // if (inclo >= 0.2) 1781 | // use next line for gsfc version and perturbed inclination 1782 | if (*inclp >= 0.2) 1783 | { 1784 | ph = ph / sinip; 1785 | pgh = pgh - cosip * ph; 1786 | *argpp = *argpp + pgh; 1787 | *nodep = *nodep + ph; 1788 | *mp = *mp + pl; 1789 | } 1790 | else 1791 | { 1792 | /* ---- apply periodics with lyddane modification ---- */ 1793 | sinop = SIN(*nodep); 1794 | cosop = COS(*nodep); 1795 | alfdp = sinip * sinop; 1796 | betdp = sinip * cosop; 1797 | dalf = ph * cosop + pinc * cosip * sinop; 1798 | dbet = -ph * sinop + pinc * cosip * cosop; 1799 | alfdp = alfdp + dalf; 1800 | betdp = betdp + dbet; 1801 | *nodep = FMOD( *nodep, twopi ); 1802 | // sgp4fix for afspc written intrinsic functions 1803 | // nodep used without a trigonometric function ahead 1804 | if ((*nodep < 0.0) && (opsmode == 'a')) 1805 | *nodep = *nodep + twopi; 1806 | xls = *mp + *argpp + cosip * *nodep; 1807 | dls = pl + pgh - pinc * *nodep * sinip; 1808 | xls = xls + dls; 1809 | xnoh = *nodep; 1810 | *nodep = ATAN2(alfdp, betdp); 1811 | // sgp4fix for afspc written intrinsic functions 1812 | // nodep used without a trigonometric function ahead 1813 | if ((*nodep < 0.0) && (opsmode == 'a')) 1814 | *nodep = *nodep + twopi; 1815 | if (FABS(xnoh - *nodep) > SGPPI) 1816 | { 1817 | if (*nodep < xnoh) 1818 | *nodep = *nodep + twopi; 1819 | else 1820 | *nodep = *nodep - twopi; 1821 | } 1822 | *mp = *mp + pl; 1823 | *argpp = xls - *mp - cosip * *nodep; 1824 | } 1825 | } // if init == 'n' 1826 | 1827 | //#include "debug1.cpp" 1828 | } // end dpper 1829 | 1830 | 1831 | 1832 | 1833 | /*----------------------------------------------------------------------------- 1834 | * 1835 | * procedure sgp4 1836 | * 1837 | * this procedure is the sgp4 prediction model from space command. this is an 1838 | * updated and combined version of sgp4 and sdp4, which were originally 1839 | * published separately in spacetrack report #3. this version follows the 1840 | * methodology from the aiaa paper (2006) describing the history and 1841 | * development of the code. 1842 | * 1843 | * author : david vallado 719-573-2600 28 jun 2005 1844 | * translated to c: charles lohr 2024-04-28 1845 | * 1846 | * inputs : 1847 | * satrec - initialised structure from sgp4init() call. 1848 | * tsince - time since epoch (minutes) 1849 | * 1850 | * outputs : 1851 | * r - position vector km 1852 | * v - velocity km/sec 1853 | * return code - non-zero on error. 1854 | * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er 1855 | * 2 - mean motion less than 0.0 1856 | * 3 - pert elements, ecc < 0.0 or ecc > 1.0 1857 | * 4 - semi-latus rectum < 0.0 1858 | * 5 - epoch elements are sub-orbital 1859 | * 6 - satellite has decayed 1860 | * 1861 | * locals : 1862 | * am - 1863 | * axnl, aynl - 1864 | * betal - 1865 | * cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , 1866 | * sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , 1867 | * cosisq , cossu , sinsu , cosu , sinu 1868 | * delm - 1869 | * delomg - 1870 | * dndt - 1871 | * eccm - 1872 | * emsq - 1873 | * ecose - 1874 | * el2 - 1875 | * eo1 - 1876 | * eccp - 1877 | * esine - 1878 | * argpm - 1879 | * argpp - 1880 | * omgadf - 1881 | * pl - 1882 | * r - 1883 | * rtemsq - 1884 | * rdotl - 1885 | * rl - 1886 | * rvdot - 1887 | * rvdotl - 1888 | * su - 1889 | * t2 , t3 , t4 , tc 1890 | * tem5, temp , temp1 , temp2 , tempa , tempe , templ 1891 | * u , ux , uy , uz , vx , vy , vz 1892 | * inclm - inclination 1893 | * mm - mean anomaly 1894 | * nm - mean motion 1895 | * nodem - right asc of ascending node 1896 | * xinc - 1897 | * xincp - 1898 | * xl - 1899 | * xlm - 1900 | * mp - 1901 | * xmdf - 1902 | * xmx - 1903 | * xmy - 1904 | * nodedf - 1905 | * xnode - 1906 | * nodep - 1907 | * np - 1908 | * 1909 | * coupling : 1910 | * getgravconst- no longer used. Variables are conatined within satrec 1911 | * dpper 1912 | * dpspace 1913 | * 1914 | * references : 1915 | * hoots, roehrich, norad spacetrack report #3 1980 1916 | * hoots, norad spacetrack report #6 1986 1917 | * hoots, schumacher and glover 2004 1918 | * vallado, crawford, hujsak, kelso 2006 1919 | ----------------------------------------------------------------------------*/ 1920 | 1921 | CSGP4_DECORATOR void sgp4 1922 | ( 1923 | struct elsetrec * satrec, SGPF tsince, 1924 | SGPF r[3], SGPF v[3] 1925 | ) 1926 | { 1927 | SGPF am, axnl, aynl, betal, cosim, cnod, 1928 | cos2u, coseo1, cosi, cosip, cosisq, cossu, cosu, 1929 | delm, delomg, em, emsq, ecose, el2, eo1, 1930 | ep, esine, argpm, argpp, argpdf, pl, mrt = 0.0, 1931 | mvt, rdotl, rl, rvdot, rvdotl, sinim, 1932 | sin2u, sineo1, sini, sinip, sinsu, sinu, 1933 | snod, su, t2, t3, t4, tem5, temp, 1934 | temp1, temp2, tempa, tempe, templ, u, ux, 1935 | uy, uz, vx, vy, vz, inclm, mm, 1936 | nm, nodem, xinc, xincp, xl, xlm, mp, 1937 | xmdf, xmx, xmy, nodedf, xnode, nodep, tc, dndt, 1938 | twopi, x2o3, //, j2, j3, tumin, j4, xke, j3oj2, radiusearthkm, 1939 | vkmpersec, delmtemp; // mu, 1940 | int ktr; 1941 | 1942 | // assign initial values 1943 | r[0] = 0.0; 1944 | r[1] = 0.0; 1945 | r[2] = 0.0; 1946 | v[0] = 0.0; 1947 | v[1] = 0.0; 1948 | v[2] = 0.0; 1949 | 1950 | /* ------------------ set mathematical constants --------------- */ 1951 | // sgp4fix divisor for divide by zero check on inclination 1952 | // the old check used 1.0 + cos(Math.PI-1.0e-9), but then compared it to 1953 | // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 1954 | const SGPF temp4 = 1.5e-12; 1955 | twopi = 2.0 * SGPPI; 1956 | x2o3 = 2.0 / 3.0; 1957 | // sgp4fix identify constants and allow alternate values 1958 | // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1959 | vkmpersec = satrec->radiusearthkm * satrec->xke / 60.0; 1960 | 1961 | /* --------------------- clear sgp4 error flag ----------------- */ 1962 | satrec->t = tsince; 1963 | satrec->error = 0; 1964 | 1965 | /* ------- update for secular gravity and atmospheric drag ----- */ 1966 | xmdf = satrec->mo + satrec->mdot * satrec->t; 1967 | argpdf = satrec->argpo + satrec->argpdot * satrec->t; 1968 | nodedf = satrec->nodeo + satrec->nodedot * satrec->t; 1969 | argpm = argpdf; 1970 | mm = xmdf; 1971 | t2 = satrec->t * satrec->t; 1972 | nodem = nodedf + satrec->nodecf * t2; 1973 | tempa = 1.0 - satrec->cc1 * satrec->t; 1974 | tempe = satrec->bstar * satrec->cc4 * satrec->t; 1975 | templ = satrec->t2cof * t2; 1976 | 1977 | if (satrec->isimp != 1) 1978 | { 1979 | delomg = satrec->omgcof * satrec->t; 1980 | // sgp4fix use mutliply for speed instead of Math.POW 1981 | delmtemp = 1.0 + satrec->eta * COS(xmdf); 1982 | delm = satrec->xmcof * 1983 | (delmtemp * delmtemp * delmtemp - 1984 | satrec->delmo); 1985 | temp = delomg + delm; 1986 | mm = xmdf + temp; 1987 | argpm = argpdf - temp; 1988 | t3 = t2 * satrec->t; 1989 | t4 = t3 * satrec->t; 1990 | tempa = tempa - satrec->d2 * t2 - satrec->d3 * t3 - 1991 | satrec->d4 * t4; 1992 | tempe = tempe + satrec->bstar * satrec->cc5 * (SIN(mm) - 1993 | satrec->sinmao); 1994 | templ = templ + satrec->t3cof * t3 + t4 * (satrec->t4cof + 1995 | satrec->t * satrec->t5cof); 1996 | } 1997 | 1998 | nm = satrec->no_unkozai; 1999 | em = satrec->ecco; 2000 | inclm = satrec->inclo; 2001 | 2002 | if (satrec->method == 'd') 2003 | { 2004 | tc = satrec->t; 2005 | dspace 2006 | ( 2007 | satrec->irez, 2008 | satrec->d2201, satrec->d2211, satrec->d3210, 2009 | satrec->d3222, satrec->d4410, satrec->d4422, 2010 | satrec->d5220, satrec->d5232, satrec->d5421, 2011 | satrec->d5433, satrec->dedt, satrec->del1, 2012 | satrec->del2, satrec->del3, satrec->didt, 2013 | satrec->dmdt, satrec->dnodt, satrec->domdt, 2014 | satrec->argpo, satrec->argpdot, satrec->t, tc, 2015 | satrec->gsto, satrec->xfact, satrec->xlamo, 2016 | satrec->no_unkozai, &satrec->atime, 2017 | &em, &argpm, &inclm, &satrec->xli, &mm, &satrec->xni, 2018 | &nodem, &dndt, &nm 2019 | ); 2020 | } // if method = d 2021 | 2022 | if (nm <= 0.0) 2023 | { 2024 | // printf("# error nm %f\n", nm); 2025 | satrec->error = 2; 2026 | // sgp4fix add return 2027 | //return false; 2028 | } 2029 | 2030 | am = POW((satrec->xke / nm), x2o3) * tempa * tempa; 2031 | nm = satrec->xke / POW(am, 1.5); 2032 | em = em - tempe; 2033 | 2034 | // fix tolerance for error recognition 2035 | // sgp4fix am is fixed from the previous nm check 2036 | if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/ ) 2037 | { 2038 | // printf("# error em %f\n", em); 2039 | satrec->error = 1; 2040 | // sgp4fix to return if there is an error in eccentricity 2041 | //return false; 2042 | } 2043 | // sgp4fix fix tolerance to avoid a divide by zero 2044 | if (em < 1.0e-6) 2045 | em = 1.0e-6; 2046 | mm = mm + satrec->no_unkozai * templ; 2047 | xlm = mm + argpm + nodem; 2048 | emsq = em * em; 2049 | temp = 1.0 - emsq; 2050 | 2051 | nodem = FMOD((nodem), twopi ); 2052 | argpm = FMOD((argpm), twopi ); 2053 | xlm = FMOD((xlm), twopi ); 2054 | mm = FMOD((xlm - argpm - nodem), twopi); 2055 | 2056 | // sgp4fix recover singly averaged mean elements 2057 | satrec->am = am; 2058 | satrec->em = em; 2059 | satrec->im = inclm; 2060 | satrec->Om = nodem; 2061 | satrec->om = argpm; 2062 | satrec->mm = mm; 2063 | satrec->nm = nm; 2064 | 2065 | /* ----------------- compute extra mean quantities ------------- */ 2066 | sinim = SIN(inclm); 2067 | cosim = COS(inclm); 2068 | 2069 | /* -------------------- add lunar-solar periodics -------------- */ 2070 | ep = em; 2071 | xincp = inclm; 2072 | argpp = argpm; 2073 | nodep = nodem; 2074 | mp = mm; 2075 | sinip = sinim; 2076 | cosip = cosim; 2077 | if (satrec->method == 'd') 2078 | { 2079 | dpper 2080 | ( 2081 | satrec->e3, satrec->ee2, satrec->peo, 2082 | satrec->pgho, satrec->pho, satrec->pinco, 2083 | satrec->plo, satrec->se2, satrec->se3, 2084 | satrec->sgh2, satrec->sgh3, satrec->sgh4, 2085 | satrec->sh2, satrec->sh3, satrec->si2, 2086 | satrec->si3, satrec->sl2, satrec->sl3, 2087 | satrec->sl4, satrec->t, satrec->xgh2, 2088 | satrec->xgh3, satrec->xgh4, satrec->xh2, 2089 | satrec->xh3, satrec->xi2, satrec->xi3, 2090 | satrec->xl2, satrec->xl3, satrec->xl4, 2091 | satrec->zmol, satrec->zmos, satrec->inclo, 2092 | 'n', &ep, &xincp, &nodep, &argpp, &mp, satrec->operationmode 2093 | ); 2094 | if (xincp < 0.0) 2095 | { 2096 | xincp = -xincp; 2097 | nodep = nodep + SGPPI; 2098 | argpp = argpp - SGPPI; 2099 | } 2100 | if ((ep < 0.0) || (ep > 1.0)) 2101 | { 2102 | // printf("# error ep %f\n", ep); 2103 | satrec->error = 3; 2104 | // sgp4fix add return 2105 | //return false; 2106 | } 2107 | } // if method = d 2108 | 2109 | /* -------------------- long period periodics ------------------ */ 2110 | if (satrec->method == 'd') 2111 | { 2112 | sinip = SIN(xincp); 2113 | cosip = COS(xincp); 2114 | satrec->aycof = -0.5 * satrec->j3oj2 * sinip; 2115 | // sgp4fix for divide by zero for xincp = 180 deg 2116 | if (FABS(cosip + 1.0) > 1.5e-12) 2117 | satrec->xlcof = -0.25 * satrec->j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); 2118 | else 2119 | satrec->xlcof = -0.25 * satrec->j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; 2120 | } 2121 | axnl = ep * COS(argpp); 2122 | temp = 1.0 / (am * (1.0 - ep * ep)); 2123 | aynl = ep * SIN(argpp) + temp * satrec->aycof; 2124 | xl = mp + argpp + nodep + temp * satrec->xlcof * axnl; 2125 | 2126 | /* --------------------- solve kepler's equation --------------- */ 2127 | u = FMOD( (xl - nodep), twopi ); 2128 | eo1 = u; 2129 | tem5 = 9999.9; 2130 | ktr = 1; 2131 | // sgp4fix for c# intiialize 2132 | coseo1 = 0.0; 2133 | sineo1 = 0.0; 2134 | // sgp4fix for kepler iteration 2135 | // the following iteration needs better limits on corrections 2136 | while ((FABS(tem5) >= 1.0e-12) && (ktr <= 10)) 2137 | { 2138 | sineo1 = SIN(eo1); 2139 | coseo1 = COS(eo1); 2140 | tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; 2141 | tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; 2142 | if (FABS(tem5) >= 0.95) 2143 | tem5 = tem5 > 0.0 ? 0.95 : -0.95; 2144 | eo1 = eo1 + tem5; 2145 | ktr = ktr + 1; 2146 | } 2147 | 2148 | /* ------------- short period preliminary quantities ----------- */ 2149 | ecose = axnl * coseo1 + aynl * sineo1; 2150 | esine = axnl * sineo1 - aynl * coseo1; 2151 | el2 = axnl * axnl + aynl * aynl; 2152 | pl = am * (1.0 - el2); 2153 | if (pl < 0.0) 2154 | { 2155 | // printf("# error pl %f\n", pl); 2156 | satrec->error = 4; 2157 | // sgp4fix add return 2158 | //return false; 2159 | } 2160 | else 2161 | { 2162 | rl = am * (1.0 - ecose); 2163 | rdotl = SQRT(am) * esine / rl; 2164 | rvdotl = SQRT(pl) / rl; 2165 | betal = SQRT(1.0 - el2); 2166 | temp = esine / (1.0 + betal); 2167 | sinu = am / rl * (sineo1 - aynl - axnl * temp); 2168 | cosu = am / rl * (coseo1 - axnl + aynl * temp); 2169 | su = ATAN2(sinu, cosu); 2170 | sin2u = (cosu + cosu) * sinu; 2171 | cos2u = 1.0 - 2.0 * sinu * sinu; 2172 | temp = 1.0 / pl; 2173 | temp1 = 0.5 * satrec->j2 * temp; 2174 | temp2 = temp1 * temp; 2175 | 2176 | /* -------------- update for short period periodics ------------ */ 2177 | if (satrec->method == 'd') 2178 | { 2179 | cosisq = cosip * cosip; 2180 | satrec->con41 = 3.0 * cosisq - 1.0; 2181 | satrec->x1mth2 = 1.0 - cosisq; 2182 | satrec->x7thm1 = 7.0 * cosisq - 1.0; 2183 | } 2184 | mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec->con41) + 2185 | 0.5 * temp1 * satrec->x1mth2 * cos2u; 2186 | su = su - 0.25 * temp2 * satrec->x7thm1 * sin2u; 2187 | xnode = nodep + 1.5 * temp2 * cosip * sin2u; 2188 | xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; 2189 | mvt = rdotl - nm * temp1 * satrec->x1mth2 * sin2u / satrec->xke; 2190 | rvdot = rvdotl + nm * temp1 * (satrec->x1mth2 * cos2u + 2191 | 1.5 * satrec->con41) / satrec->xke; 2192 | 2193 | /* --------------------- orientation vectors ------------------- */ 2194 | sinsu = SIN(su); 2195 | cossu = COS(su); 2196 | snod = SIN(xnode); 2197 | cnod = COS(xnode); 2198 | sini = SIN(xinc); 2199 | cosi = COS(xinc); 2200 | xmx = -snod * cosi; 2201 | xmy = cnod * cosi; 2202 | ux = xmx * sinsu + cnod * cossu; 2203 | uy = xmy * sinsu + snod * cossu; 2204 | uz = sini * sinsu; 2205 | vx = xmx * cossu - cnod * sinsu; 2206 | vy = xmy * cossu - snod * sinsu; 2207 | vz = sini * cossu; 2208 | 2209 | /* --------- position and velocity (in km and km/sec) ---------- */ 2210 | r[0] = (mrt * ux) * satrec->radiusearthkm; 2211 | r[1] = (mrt * uy) * satrec->radiusearthkm; 2212 | r[2] = (mrt * uz) * satrec->radiusearthkm; 2213 | v[0] = (mvt * ux + rvdot * vx) * vkmpersec; 2214 | v[1] = (mvt * uy + rvdot * vy) * vkmpersec; 2215 | v[2] = (mvt * uz + rvdot * vz) * vkmpersec; 2216 | } // if pl > 0 2217 | 2218 | // sgp4fix for decaying satellites 2219 | if (mrt < 1.0) 2220 | { 2221 | // printf("# decay condition %11.6f \n",mrt); 2222 | satrec->error = 6; 2223 | //return false; 2224 | } 2225 | 2226 | //#include "debug7.cpp" 2227 | //return true; 2228 | } // end sgp4 2229 | 2230 | 2231 | 2232 | 2233 | #if CSGP4_INIT 2234 | /*----------------------------------------------------------------------------- 2235 | * 2236 | * procedure sgp4init 2237 | * 2238 | * this procedure initializes variables for sgp4. 2239 | * 2240 | * author : david vallado 719-573-2600 28 jun 2005 2241 | * translated to c: charles lohr 2024-04-28 2242 | * 2243 | * inputs : 2244 | * opsmode - mode of operation afspc or improved 'a', 'i' 2245 | * whichconst - which set of constants to use 72, 84 2246 | * satn - satellite number 2247 | * bstar - sgp4 type drag coefficient kg/m2er 2248 | * ecco - eccentricity 2249 | * epoch - epoch time in days from jan 0, 1950. 0 hr 2250 | * argpo - argument of perigee (output if ds) 2251 | * inclo - inclination 2252 | * mo - mean anomaly (output if ds) 2253 | * no - mean motion 2254 | * nodeo - right ascension of ascending node 2255 | * 2256 | * outputs : 2257 | * satrec - common values for subsequent calls 2258 | * return code - non-zero on error. 2259 | * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er 2260 | * 2 - mean motion less than 0.0 2261 | * 3 - pert elements, ecc < 0.0 or ecc > 1.0 2262 | * 4 - semi-latus rectum < 0.0 2263 | * 5 - epoch elements are sub-orbital 2264 | * 6 - satellite has decayed 2265 | * 2266 | * locals : 2267 | * cnodm , snodm , cosim , sinim , cosomm , sinomm 2268 | * cc1sq , cc2 , cc3 2269 | * coef , coef1 2270 | * cosio4 - 2271 | * day - 2272 | * dndt - 2273 | * em - eccentricity 2274 | * emsq - eccentricity squared 2275 | * eeta - 2276 | * etasq - 2277 | * gam - 2278 | * argpm - argument of perigee 2279 | * nodem - 2280 | * inclm - inclination 2281 | * mm - mean anomaly 2282 | * nm - mean motion 2283 | * perige - perigee 2284 | * pinvsq - 2285 | * psisq - 2286 | * qzms24 - 2287 | * rtemsq - 2288 | * s1, s2, s3, s4, s5, s6, s7 - 2289 | * sfour - 2290 | * ss1, ss2, ss3, ss4, ss5, ss6, ss7 - 2291 | * sz1, sz2, sz3 2292 | * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - 2293 | * tc - 2294 | * temp - 2295 | * temp1, temp2, temp3 - 2296 | * tsi - 2297 | * xpidot - 2298 | * xhdot1 - 2299 | * z1, z2, z3 - 2300 | * z11, z12, z13, z21, z22, z23, z31, z32, z33 - 2301 | * 2302 | * coupling : 2303 | * getgravconst- 2304 | * initl - 2305 | * dscom - 2306 | * dpper - 2307 | * dsinit - 2308 | * sgp4 - 2309 | * 2310 | * references : 2311 | * hoots, roehrich, norad spacetrack report #3 1980 2312 | * hoots, norad spacetrack report #6 1986 2313 | * hoots, schumacher and glover 2004 2314 | * vallado, crawford, hujsak, kelso 2006 2315 | ----------------------------------------------------------------------------*/ 2316 | 2317 | CSGP4_DECORATOR void sgp4init 2318 | ( 2319 | enum gravconsttype whichconst, char opsmode/*, char * satn*/, SGPF epoch, 2320 | SGPF xbstar, SGPF xndot, SGPF xnddot, SGPF xecco, SGPF xargpo, 2321 | SGPF xinclo, SGPF xmo, SGPF xno_kozai, 2322 | SGPF xnodeo, SGPF initial_time, SGPF * initial_r, SGPF * initial_v, struct elsetrec * satrec 2323 | ) 2324 | { 2325 | /* --------------------- local variables ------------------------ */ 2326 | SGPF ao, ainv, con42, cosio, sinio, cosio2, eccsq, 2327 | omeosq, posq, rp, rteosq, 2328 | cnodm, snodm, cosim, sinim, cosomm, sinomm, cc1sq, 2329 | cc2, cc3, coef, coef1, cosio4, day, dndt, 2330 | em, emsq, eeta, etasq, gam, argpm, nodem, 2331 | inclm, mm, nm, perige, pinvsq, psisq, qzms24, 2332 | rtemsq, s1, s2, s3, s4, s5, s6, 2333 | s7, sfour, ss1, ss2, ss3, ss4, ss5, 2334 | ss6, ss7, sz1, sz2, sz3, sz11, sz12, 2335 | sz13, sz21, sz22, sz23, sz31, sz32, sz33, 2336 | tc, temp, temp1, temp2, temp3, tsi, xpidot, 2337 | xhdot1, z1, z2, z3, z11, z12, z13, 2338 | z21, z22, z23, z31, z32, z33, 2339 | qzms2t, ss, x2o3, 2340 | delmotemp, qzms2ttemp, qzms24temp; 2341 | 2342 | /* ------------------------ initialization --------------------- */ 2343 | // sgp4fix divisor for divide by zero check on inclination 2344 | // the old check used 1.0 + cos(Math.PI-1.0e-9), but then compared it to 2345 | // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 2346 | const SGPF temp4 = 1.5e-12; 2347 | 2348 | /* ----------- set all near earth variables to zero ------------ */ 2349 | satrec->isimp = 0; satrec->method = 'n'; satrec->aycof = 0.0; 2350 | satrec->con41 = 0.0; satrec->cc1 = 0.0; satrec->cc4 = 0.0; 2351 | satrec->cc5 = 0.0; satrec->d2 = 0.0; satrec->d3 = 0.0; 2352 | satrec->d4 = 0.0; satrec->delmo = 0.0; satrec->eta = 0.0; 2353 | satrec->argpdot = 0.0; satrec->omgcof = 0.0; satrec->sinmao = 0.0; 2354 | satrec->t = 0.0; satrec->t2cof = 0.0; satrec->t3cof = 0.0; 2355 | satrec->t4cof = 0.0; satrec->t5cof = 0.0; satrec->x1mth2 = 0.0; 2356 | satrec->x7thm1 = 0.0; satrec->mdot = 0.0; satrec->nodedot = 0.0; 2357 | satrec->xlcof = 0.0; satrec->xmcof = 0.0; satrec->nodecf = 0.0; 2358 | 2359 | /* ----------- set all deep space variables to zero ------------ */ 2360 | satrec->irez = 0; satrec->d2201 = 0.0; satrec->d2211 = 0.0; 2361 | satrec->d3210 = 0.0; satrec->d3222 = 0.0; satrec->d4410 = 0.0; 2362 | satrec->d4422 = 0.0; satrec->d5220 = 0.0; satrec->d5232 = 0.0; 2363 | satrec->d5421 = 0.0; satrec->d5433 = 0.0; satrec->dedt = 0.0; 2364 | satrec->del1 = 0.0; satrec->del2 = 0.0; satrec->del3 = 0.0; 2365 | satrec->didt = 0.0; satrec->dmdt = 0.0; satrec->dnodt = 0.0; 2366 | satrec->domdt = 0.0; satrec->e3 = 0.0; satrec->ee2 = 0.0; 2367 | satrec->peo = 0.0; satrec->pgho = 0.0; satrec->pho = 0.0; 2368 | satrec->pinco = 0.0; satrec->plo = 0.0; satrec->se2 = 0.0; 2369 | satrec->se3 = 0.0; satrec->sgh2 = 0.0; satrec->sgh3 = 0.0; 2370 | satrec->sgh4 = 0.0; satrec->sh2 = 0.0; satrec->sh3 = 0.0; 2371 | satrec->si2 = 0.0; satrec->si3 = 0.0; satrec->sl2 = 0.0; 2372 | satrec->sl3 = 0.0; satrec->sl4 = 0.0; satrec->gsto = 0.0; 2373 | satrec->xfact = 0.0; satrec->xgh2 = 0.0; satrec->xgh3 = 0.0; 2374 | satrec->xgh4 = 0.0; satrec->xh2 = 0.0; satrec->xh3 = 0.0; 2375 | satrec->xi2 = 0.0; satrec->xi3 = 0.0; satrec->xl2 = 0.0; 2376 | satrec->xl3 = 0.0; satrec->xl4 = 0.0; satrec->xlamo = 0.0; 2377 | satrec->zmol = 0.0; satrec->zmos = 0.0; satrec->atime = 0.0; 2378 | satrec->xli = 0.0; satrec->xni = 0.0; 2379 | 2380 | /* ------------------------ earth constants ----------------------- */ 2381 | // sgp4fix identify constants and allow aernate values 2382 | // this is now the only call for the constants 2383 | getgravconst(whichconst, &satrec->tumin, &satrec->mu, &satrec->radiusearthkm, &satrec->xke, 2384 | &satrec->j2, &satrec->j3, &satrec->j4, &satrec->j3oj2); 2385 | //------------------------------------------------------------------------- 2386 | satrec->error = 0; 2387 | satrec->operationmode = opsmode; 2388 | //strcpy( satrec->satnum, satn ); 2389 | 2390 | // sgp4fix - note the following variables are also passed directly via satrec-> 2391 | // it is possible to streamline the sgp4init call by deleting the 'x' 2392 | // variables, but the user would need to set the satrec* values first. we 2393 | // include the additional assignments in case twoline2rv is not used. 2394 | satrec->bstar = xbstar; 2395 | // sgp4fix allow additional parameters in the struct 2396 | satrec->ndot = xndot; 2397 | satrec->nddot = xnddot; 2398 | satrec->ecco = xecco; 2399 | satrec->argpo = xargpo; 2400 | satrec->inclo = xinclo; 2401 | satrec->mo = xmo; 2402 | // sgp4fix rename variables to clarify which mean motion is intended 2403 | satrec->no_kozai = xno_kozai; 2404 | satrec->nodeo = xnodeo; 2405 | 2406 | // single averaged mean elements 2407 | satrec->am = satrec->em = satrec->im = satrec->Om = satrec->mm = satrec->nm = 0.0; 2408 | 2409 | /* ------------------------ earth constants ----------------------- */ 2410 | // sgp4fix identify constants and allow alternate values no longer needed 2411 | // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 2412 | ss = 78.0 / satrec->radiusearthkm + 1.0; 2413 | // sgp4fix use multiply for speed instead of Math.POW 2414 | qzms2ttemp = (120.0 - 78.0) / satrec->radiusearthkm; 2415 | qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp; 2416 | x2o3 = 2.0 / 3.0; 2417 | 2418 | satrec->init = 'y'; 2419 | satrec->t = 0.0; 2420 | 2421 | // sgp4fix remove satn as it is not needed in initl 2422 | initl 2423 | (satrec->xke, satrec->j2, satrec->ecco, epoch, satrec->inclo, satrec->no_kozai, &satrec->method, 2424 | &ainv, &ao, &satrec->con41, &con42, &cosio, &cosio2, &eccsq, &omeosq, 2425 | &posq, &rp, &rteosq, &sinio, &satrec->gsto, satrec->operationmode, &satrec->no_unkozai); 2426 | satrec->a = POW(satrec->no_unkozai * satrec->tumin, (-2.0 / 3.0)); 2427 | satrec->alta = satrec->a * (1.0 + satrec->ecco) - 1.0; 2428 | satrec->altp = satrec->a * (1.0 - satrec->ecco) - 1.0; 2429 | satrec->error = 0; 2430 | 2431 | // sgp4fix remove this check as it is unnecessary 2432 | // the mrt check in sgp4 handles decaying satellite cases even if the starting 2433 | // condition is below the surface of te earth 2434 | // if (rp < 1.0) 2435 | // { 2436 | // printf("# *** satn%d epoch elts sub-orbital ***\n", satn); 2437 | // satrec->error = 5; 2438 | // } 2439 | 2440 | if ((omeosq >= 0.0) || (satrec->no_unkozai >= 0.0)) 2441 | { 2442 | satrec->isimp = 0; 2443 | if (rp < (220.0 / satrec->radiusearthkm + 1.0)) 2444 | satrec->isimp = 1; 2445 | sfour = ss; 2446 | qzms24 = qzms2t; 2447 | perige = (rp - 1.0) * satrec->radiusearthkm; 2448 | 2449 | /* - for perigees below 156 km, s and qoms2t are altered - */ 2450 | if (perige < 156.0) 2451 | { 2452 | sfour = perige - 78.0; 2453 | if (perige < 98.0) 2454 | sfour = 20.0; 2455 | // sgp4fix use multiply for speed instead of Math.POW 2456 | qzms24temp = (120.0 - sfour) / satrec->radiusearthkm; 2457 | qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp; 2458 | sfour = sfour / satrec->radiusearthkm + 1.0; 2459 | } 2460 | pinvsq = 1.0 / posq; 2461 | 2462 | tsi = 1.0 / (ao - sfour); 2463 | satrec->eta = ao * satrec->ecco * tsi; 2464 | etasq = satrec->eta * satrec->eta; 2465 | eeta = satrec->ecco * satrec->eta; 2466 | psisq = FABS(1.0 - etasq); 2467 | coef = qzms24 * POW(tsi, 4.0); 2468 | coef1 = coef / POW(psisq, 3.5); 2469 | cc2 = coef1 * satrec->no_unkozai * (ao * (1.0 + 1.5 * etasq + eeta * 2470 | (4.0 + etasq)) + 0.375 * satrec->j2 * tsi / psisq * satrec->con41 * 2471 | (8.0 + 3.0 * etasq * (8.0 + etasq))); 2472 | satrec->cc1 = satrec->bstar * cc2; 2473 | cc3 = 0.0; 2474 | if (satrec->ecco > 1.0e-4) 2475 | cc3 = -2.0 * coef * tsi * satrec->j3oj2 * satrec->no_unkozai * sinio / satrec->ecco; 2476 | satrec->x1mth2 = 1.0 - cosio2; 2477 | satrec->cc4 = 2.0 * satrec->no_unkozai * coef1 * ao * omeosq * 2478 | (satrec->eta * (2.0 + 0.5 * etasq) + satrec->ecco * 2479 | (0.5 + 2.0 * etasq) - satrec->j2 * tsi / (ao * psisq) * 2480 | (-3.0 * satrec->con41 * (1.0 - 2.0 * eeta + etasq * 2481 | (1.5 - 0.5 * eeta)) + 0.75 * satrec->x1mth2 * 2482 | (2.0 * etasq - eeta * (1.0 + etasq)) * COS(2.0 * satrec->argpo))); 2483 | satrec->cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * 2484 | (etasq + eeta) + eeta * etasq); 2485 | cosio4 = cosio2 * cosio2; 2486 | temp1 = 1.5 * satrec->j2 * pinvsq * satrec->no_unkozai; 2487 | temp2 = 0.5 * temp1 * satrec->j2 * pinvsq; 2488 | temp3 = -0.46875 * satrec->j4 * pinvsq * pinvsq * satrec->no_unkozai; 2489 | satrec->mdot = satrec->no_unkozai + 0.5 * temp1 * rteosq * satrec->con41 + 0.0625 * 2490 | temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); 2491 | satrec->argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * 2492 | (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + 2493 | temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); 2494 | xhdot1 = -temp1 * cosio; 2495 | satrec->nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + 2496 | 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; 2497 | xpidot = satrec->argpdot + satrec->nodedot; 2498 | satrec->omgcof = satrec->bstar * cc3 * COS(satrec->argpo); 2499 | satrec->xmcof = 0.0; 2500 | if (satrec->ecco > 1.0e-4) 2501 | satrec->xmcof = -x2o3 * coef * satrec->bstar / eeta; 2502 | satrec->nodecf = 3.5 * omeosq * xhdot1 * satrec->cc1; 2503 | satrec->t2cof = 1.5 * satrec->cc1; 2504 | // sgp4fix for divide by zero with xinco = 180 deg 2505 | if (FABS(cosio + 1.0) > 1.5e-12) 2506 | satrec->xlcof = -0.25 * satrec->j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); 2507 | else 2508 | satrec->xlcof = -0.25 * satrec->j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; 2509 | satrec->aycof = -0.5 * satrec->j3oj2 * sinio; 2510 | // sgp4fix use multiply for speed instead of Math.POW 2511 | delmotemp = 1.0 + satrec->eta * COS(satrec->mo); 2512 | satrec->delmo = delmotemp * delmotemp * delmotemp; 2513 | satrec->sinmao = SIN(satrec->mo); 2514 | satrec->x7thm1 = 7.0 * cosio2 - 1.0; 2515 | 2516 | /* --------------- deep space initialization ------------- */ 2517 | if ((2 * SGPPI / satrec->no_unkozai) >= 225.0) 2518 | { 2519 | satrec->method = 'd'; 2520 | satrec->isimp = 1; 2521 | tc = 0.0; 2522 | inclm = satrec->inclo; 2523 | 2524 | dscom 2525 | ( 2526 | epoch, satrec->ecco, satrec->argpo, tc, satrec->inclo, satrec->nodeo, 2527 | satrec->no_unkozai, &snodm, &cnodm, &sinim, &cosim, &sinomm, &cosomm, 2528 | &day, &satrec->e3, &satrec->ee2, &em, &emsq, &gam, 2529 | &satrec->peo, &satrec->pgho, &satrec->pho, &satrec->pinco, 2530 | &satrec->plo, &rtemsq, &satrec->se2, &satrec->se3, 2531 | &satrec->sgh2, &satrec->sgh3, &satrec->sgh4, 2532 | &satrec->sh2, & satrec->sh3, &satrec->si2, &satrec->si3, 2533 | &satrec->sl2, &satrec->sl3, &satrec->sl4, &s1, &s2, &s3, &s4, &s5, 2534 | &s6, &s7, &ss1, &ss2, &ss3, &ss4, &ss5, &ss6, &ss7, &sz1, &sz2, &sz3, 2535 | &sz11, &sz12, &sz13, &sz21, &sz22, &sz23, &sz31, &sz32, &sz33, 2536 | &satrec->xgh2, &satrec->xgh3, &satrec->xgh4, &satrec->xh2, 2537 | &satrec->xh3, &satrec->xi2, &satrec->xi3, &satrec->xl2, 2538 | &satrec->xl3, &satrec->xl4, &nm, &z1, &z2, &z3, &z11, 2539 | &z12, &z13, &z21, &z22, &z23, &z31, &z32, &z33, 2540 | &satrec->zmol, &satrec->zmos 2541 | ); 2542 | dpper 2543 | ( 2544 | satrec->e3, satrec->ee2, satrec->peo, satrec->pgho, 2545 | satrec->pho, satrec->pinco, satrec->plo, satrec->se2, 2546 | satrec->se3, satrec->sgh2, satrec->sgh3, satrec->sgh4, 2547 | satrec->sh2, satrec->sh3, satrec->si2, satrec->si3, 2548 | satrec->sl2, satrec->sl3, satrec->sl4, satrec->t, 2549 | satrec->xgh2, satrec->xgh3, satrec->xgh4, satrec->xh2, 2550 | satrec->xh3, satrec->xi2, satrec->xi3, satrec->xl2, 2551 | satrec->xl3, satrec->xl4, satrec->zmol, satrec->zmos, inclm, satrec->init, 2552 | &satrec->ecco, &satrec->inclo, &satrec->nodeo, &satrec->argpo, &satrec->mo, 2553 | satrec->operationmode 2554 | ); 2555 | 2556 | argpm = 0.0; 2557 | nodem = 0.0; 2558 | mm = 0.0; 2559 | 2560 | dsinit 2561 | ( 2562 | satrec->xke, 2563 | cosim, emsq, satrec->argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, 2564 | ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec->t, tc, 2565 | satrec->gsto, satrec->mo, satrec->mdot, satrec->no_unkozai, satrec->nodeo, 2566 | satrec->nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, 2567 | satrec->ecco, eccsq, &em, &argpm, &inclm, &mm, &nm, &nodem, 2568 | &satrec->irez, &satrec->atime, 2569 | &satrec->d2201, &satrec->d2211, &satrec->d3210, &satrec->d3222, 2570 | &satrec->d4410, &satrec->d4422, &satrec->d5220, &satrec->d5232, 2571 | &satrec->d5421, &satrec->d5433, &satrec->dedt, &satrec->didt, 2572 | &satrec->dmdt, &dndt, &satrec->dnodt, &satrec->domdt, 2573 | &satrec->del1, &satrec->del2, &satrec->del3, &satrec->xfact, 2574 | &satrec->xlamo, &satrec->xli, &satrec->xni 2575 | ); 2576 | } 2577 | 2578 | /* ----------- set variables if not deep space ----------- */ 2579 | if (satrec->isimp != 1) 2580 | { 2581 | cc1sq = satrec->cc1 * satrec->cc1; 2582 | satrec->d2 = 4.0 * ao * tsi * cc1sq; 2583 | temp = satrec->d2 * tsi * satrec->cc1 / 3.0; 2584 | satrec->d3 = (17.0 * ao + sfour) * temp; 2585 | satrec->d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * 2586 | satrec->cc1; 2587 | satrec->t3cof = satrec->d2 + 2.0 * cc1sq; 2588 | satrec->t4cof = 0.25 * (3.0 * satrec->d3 + satrec->cc1 * 2589 | (12.0 * satrec->d2 + 10.0 * cc1sq)); 2590 | satrec->t5cof = 0.2 * (3.0 * satrec->d4 + 2591 | 12.0 * satrec->cc1 * satrec->d3 + 2592 | 6.0 * satrec->d2 * satrec->d2 + 2593 | 15.0 * cc1sq * (2.0 * satrec->d2 + cc1sq)); 2594 | } 2595 | } // if omeosq = 0 ... 2596 | 2597 | /* finally propagate to zero epoch to initialize all others. */ 2598 | // sgp4fix take out check to let satellites process until they are actually below earth surface 2599 | // if(satrec->error == 0) 2600 | SGPF r[3]; 2601 | SGPF v[3]; 2602 | if( !initial_r ) initial_r = r; 2603 | if( !initial_v ) initial_v = v; 2604 | sgp4(satrec, initial_time, initial_r, initial_v); 2605 | 2606 | satrec->init = 'n'; 2607 | 2608 | //#include "debug6.cpp" 2609 | //sgp4fix return boolean. satrec->error contains any error codes 2610 | //return true; 2611 | } // end sgp4init 2612 | 2613 | #endif // CSGP4_INIT 2614 | 2615 | // Utility functions from "MathTimeLib.cs" 2616 | 2617 | 2618 | 2619 | /* ----------------------------------------------------------------------------- 2620 | * 2621 | * procedure days2mdhms 2622 | * 2623 | * this procedure converts the day of the year, days, to the equivalent month 2624 | * day, hour, minute and second. 2625 | * 2626 | * algorithm : set up array for the number of days per month 2627 | * find leap year - use 1900 because 2000 is a leap year 2628 | * loop through a temp value while the value is < the days 2629 | * perform int conversions to the correct day and month 2630 | * convert remainder into h m s using type conversions 2631 | * 2632 | * author : david vallado davallado@gmail.com 1 mar 2001 2633 | * translated to c: charles lohr 2024-04-28 2634 | * 2635 | * inputs description range / units 2636 | * year - year 1900 .. 2100 2637 | * days - julian day of the year 0.0 .. 366.0 2638 | * 2639 | * outputs : 2640 | * mon - month 1 .. 12 2641 | * day - day 1 .. 28,29,30,31 2642 | * hr - hour 0 .. 23 2643 | * minute - minute 0 .. 59 2644 | * second - second 0.0 .. 59.999 2645 | * 2646 | * locals : 2647 | * dayofyr - day of year 2648 | * temp - temporary extended values 2649 | * inttemp - temporary int value 2650 | * i - index 2651 | * lmonth[12] - int array containing the number of days per month 2652 | * 2653 | * coupling : 2654 | * none. 2655 | * --------------------------------------------------------------------------- */ 2656 | 2657 | CSGP4_DECORATOR void days2mdhms 2658 | ( 2659 | int year, SGPF days, 2660 | int * mon, int * day, int * hr, int * minute, SGPF CSGP4_OUT second 2661 | ) 2662 | { 2663 | int i, inttemp, dayofyr; 2664 | SGPF temp; 2665 | static const int lmonthNoLeap[] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; 2666 | static const int lmonthLeap[] = { 0, 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; 2667 | const int * lmonth; 2668 | dayofyr = (int)FLOOR(days); 2669 | /* ----------------- find month and day of month ---------------- */ 2670 | if ((year % 4) == 0) 2671 | lmonth = lmonthLeap; 2672 | else 2673 | lmonth = lmonthNoLeap; 2674 | 2675 | i = 1; 2676 | inttemp = 0; 2677 | while ((dayofyr > inttemp + lmonth[i]) && (i < 12)) 2678 | { 2679 | inttemp = inttemp + lmonth[i]; 2680 | i = i + 1; 2681 | } 2682 | *mon = i; 2683 | *day = dayofyr - inttemp; 2684 | /* ----------------- find hours minutes and seconds ------------- */ 2685 | temp = (days - dayofyr) * 24.0; 2686 | *hr = (int)(FLOOR(temp)); 2687 | temp = (temp - *hr) * 60.0; 2688 | *minute = (int)(FLOOR(temp)); 2689 | *second= (temp - *minute) * 60.0; 2690 | } // days2mdhms 2691 | 2692 | 2693 | /* ----------------------------------------------------------------------------- 2694 | * 2695 | * procedure jday 2696 | * 2697 | * this procedure finds the julian date given the year, month, day, and time. 2698 | * the julian date is defined by each elapsed day since noon, jan 1, 4713 bc. 2699 | * two values are passed back for improved accuracy 2700 | * 2701 | * algorithm : calculate the answer in one step for efficiency 2702 | * 2703 | * author : david vallado 719-573-2600 1 mar 2001 2704 | * translated to c: charles lohr 2024-04-28 2705 | * 2706 | * inputs description range / units 2707 | * year - year 1900 .. 2100 2708 | * mon - month 1 .. 12 2709 | * day - day 1 .. 28,29,30,31 2710 | * hr - universal time hour 0 .. 23 2711 | * min - universal time min 0 .. 59 2712 | * sec - universal time sec 0.0 .. 59.999 2713 | * 2714 | * outputs : 2715 | * jd - julian date (days only) days from 4713 bc 2716 | * jdFrac - julian date (fraction of a day) days from 0 hr of the day 2717 | * 2718 | * locals : 2719 | * none. 2720 | * 2721 | * coupling : 2722 | * none. 2723 | * 2724 | * references : 2725 | * vallado 2007, 189, alg 14, ex 3-14 2726 | * --------------------------------------------------------------------------- */ 2727 | 2728 | CSGP4_DECORATOR void jday 2729 | ( 2730 | int year, int mon, int day, int hr, int minute, SGPF sec, 2731 | SGPF CSGP4_OUT jd, SGPF CSGP4_OUT jdFrac 2732 | ) 2733 | { 2734 | *jd = 367.0 * year - 2735 | FLOOR((7 * (year + FLOOR((mon + 9) / 12.0))) * 0.25) + 2736 | FLOOR(275 * mon / 9.0) + 2737 | day + 1721013.5; // use - 678987.0 to go to mjd directly 2738 | *jdFrac = (sec + minute * 60.0 + hr * 3600.0) / 86400.0; 2739 | 2740 | // check that the day and fractional day are correct 2741 | if (FABS(*jdFrac) > 1.0) 2742 | { 2743 | SGPF dtt = FLOOR(*jdFrac); 2744 | *jd = *jd + dtt; 2745 | *jdFrac = *jdFrac - dtt; 2746 | } 2747 | 2748 | // - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5; 2749 | } // jday 2750 | 2751 | 2752 | /* ----------------------------------------------------------------------------- 2753 | * 2754 | * procedure invjday 2755 | * 2756 | * this procedure finds the year, month, day, hour, minute and second 2757 | * given the julian date. tu can be ut1, tdt, tdb, etc. jd is input 2758 | * with two arguments for additional accuracy 2759 | * 2760 | * algorithm : set up starting values 2761 | * find leap year - use 1900 because 2000 is a leap year 2762 | * find the elapsed days through the year in a loop 2763 | * call routine to find each individual value 2764 | * 2765 | * author : david vallado davallado@gmail.com 1 mar 2001 2766 | * translated to c: charles lohr 2024-04-28 2767 | * 2768 | * inputs description range / units 2769 | * jd - julian date (days only) days from 4713 bc 2770 | * jdFrac - julian date (fraction of a day) days from 0 hr of the day 2771 | * 2772 | * outputs : 2773 | * year - year 1900 .. 2100 2774 | * mon - month 1 .. 12 2775 | * day - day 1 .. 28,29,30,31 2776 | * hr - hour 0 .. 23 2777 | * minute - minute 0 .. 59 2778 | * second - second 0.0 .. 59.999 2779 | * 2780 | * locals : 2781 | * days - day of year plus fractional 2782 | * portion of a day days 2783 | * tu - julian centuries from 0 h 2784 | * jan 0, 1900 2785 | * temp - temporary SGPF values 2786 | * leapyrs - number of leap years from 1900 2787 | * 2788 | * coupling : 2789 | * days2mdhms - finds month, day, hour, minute and second given days and year 2790 | * 2791 | * references : 2792 | * vallado 2013, 202, alg 22, ex 3-13 2793 | * --------------------------------------------------------------------------- */ 2794 | 2795 | CSGP4_DECORATOR void invjday 2796 | ( 2797 | SGPF jd, SGPF jdFrac, 2798 | int * year, int * mon, int * day, 2799 | int * hr, int * minute, SGPF CSGP4_OUT second 2800 | ) 2801 | { 2802 | int leapyrs; 2803 | SGPF dt, days, tu, temp; 2804 | 2805 | // check jdfrac for multiple days 2806 | if (FABS(jdFrac) >= 1.0) 2807 | { 2808 | jd = jd + FLOOR(jdFrac); 2809 | jdFrac = jdFrac - FLOOR(jdFrac); 2810 | } 2811 | 2812 | // check for fraction of a day included in the jd 2813 | dt = jd - FLOOR(jd) - 0.5; 2814 | if (FABS(dt) > 0.00000001) 2815 | { 2816 | jd = jd - dt; 2817 | jdFrac = jdFrac + dt; 2818 | } 2819 | 2820 | /* --------------- find year and days of the year --------------- */ 2821 | temp = jd - 2415019.5; 2822 | tu = temp / 365.25; 2823 | *year = 1900 + (int)(FLOOR(tu)); 2824 | leapyrs = (int)(FLOOR((*year - 1901) * 0.25)); 2825 | 2826 | days = FLOOR(temp - ((*year - 1900) * 365.0 + leapyrs)); 2827 | 2828 | /* ------------ check for case of beginning of a year ----------- */ 2829 | if (days + jdFrac < 1.0) 2830 | { 2831 | *year = *year - 1; 2832 | leapyrs = (int)(FLOOR((*year - 1901) * 0.25)); 2833 | days = FLOOR(temp - ((*year - 1900) * 365.0 + leapyrs)); 2834 | } 2835 | 2836 | /* ----------------- find remaining data ----------------------- */ 2837 | // now add the daily time in to preserve accuracy 2838 | days2mdhms(*year, days + jdFrac, mon, day, hr, minute, second); 2839 | } // invjday 2840 | 2841 | 2842 | 2843 | 2844 | 2845 | /* 2846 | CSGP4_DECORATOR void Dumpelsetrec( struct elsetrec * obj ) 2847 | { 2848 | printf( "obj.a = %18.15f\n", obj->a ); 2849 | printf( "obj.alta = %18.15f\n", obj->alta ); 2850 | printf( "obj.altp = %18.15f\n", obj->altp ); 2851 | printf( "obj.am = %18.15f <<<<<<\n", obj->am ); 2852 | printf( "obj.argpdot = %18.15f\n", obj->argpdot ); 2853 | printf( "obj.argpo = %18.15f\n", obj->argpo ); 2854 | printf( "obj.bstar = %18.15f\n", obj->bstar ); 2855 | printf( "obj.ecco = %18.15f\n", obj->ecco ); 2856 | printf( "obj.em = %18.15f <<<<<<\n", obj->em ); 2857 | printf( "obj.error = %d\n", obj->error ); 2858 | printf( "obj.gsto = %18.15f (!!!!!)\n", obj->gsto ); 2859 | printf( "obj.im = %18.15f\n", obj->im ); 2860 | printf( "obj.inclo = %18.15f\n", obj->inclo ); 2861 | printf( "obj.j2 = %18.15f\n", obj->j2 ); 2862 | printf( "obj.j3 = %18.15f\n", obj->j3 ); 2863 | printf( "obj.j3oj2 = %18.15f\n", obj->j3oj2 ); 2864 | printf( "obj.j4 = %18.15f\n", obj->j4 ); 2865 | printf( "obj.mdot = %18.15f\n", obj->mdot ); 2866 | printf( "obj.method = %d\n", obj->method ); 2867 | printf( "obj.mm = %18.15f <<<<<<<\n", obj->mm ); 2868 | printf( "obj.mo = %18.15f\n", obj->mo ); 2869 | printf( "obj.mu = %18.15f\n", obj->mu ); 2870 | printf( "obj.nddot = %18.15f\n", obj->nddot ); 2871 | printf( "obj.ndot = %18.15f\n", obj->ndot ); 2872 | printf( "obj.nm = %18.15f <<<<<<<\n", obj->nm ); 2873 | printf( "obj.no_kozai = %18.15f\n", obj->no_kozai ); 2874 | printf( "obj.nodedot = %18.15f\n", obj->nodedot ); 2875 | printf( "obj.nodeo = %18.15f\n", obj->nodeo ); 2876 | printf( "obj.om = %18.15f\n", obj->om ); 2877 | printf( "obj.radiusearthkm = %18.15f\n", obj->radiusearthkm ); 2878 | printf( "obj.t = %18.15f\n", obj->t ); 2879 | printf( "obj.tumin = %18.15f\n", obj->tumin ); 2880 | printf( "obj.xke = %18.15f\n", obj->xke ); 2881 | } 2882 | */ 2883 | 2884 | 2885 | #endif 2886 | 2887 | -------------------------------------------------------------------------------- /csgp4_simple.h: -------------------------------------------------------------------------------- 1 | // A shader-compilable, C header transliterated from David Vallado's SGP4Lib.cs, version: "SGP4 Version 2020-03-12"; 2 | // https://celestrak.org/software/vallado-sw.php 3 | // I could not find a license, but assume whatever license the original code is under. 4 | // I (Charles Lohr) just ported the code back to C from C#. This code is much less featureful than the original code 5 | // AND ABSOLUTELY DOES NOT MAINTAIN THE SAME LEVEL OF ACCURACY. 6 | // 7 | // USE AT YOUR OWN RISK 8 | // 9 | // This file gets rid of basically all optional features of this system, and only does supports 'a' computation mode. 10 | // 11 | // Please note: There are still branches based off static initialization, please be sensitive to organize your 12 | // satellites in such a way you don't accidentally parallelize multiple satellites together. 13 | 14 | #ifndef _CSGP4_SIMPLE_H 15 | #define _CSGP4_SIMPLE_H 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #ifndef CSGP4_OUT 25 | #define CSGP4_OUT * restrict 26 | #endif 27 | 28 | #ifndef CSGP4_DEREF 29 | #define CSGP4_DEREF(x) (*x) 30 | #endif 31 | 32 | #ifndef CSGP4_REF 33 | #define CSGP4_REF(x) (&x) 34 | #endif 35 | 36 | #define SGPPI 3.1415926535897932384626433 37 | 38 | #define SGP4_FROM_EPOCH_DAYS 2440587.5 39 | 40 | // Default to double calculations. 41 | #ifndef CSGP4_USE_FLOAT 42 | #define CSGP4_USE_FLOAT 0 43 | #endif 44 | 45 | #ifndef CSGP4_INIT 46 | #define CSGP4_INIT 1 47 | #endif 48 | 49 | 50 | #ifndef CSGP4_DECORATOR 51 | #define CSGP4_DECORATOR static inline 52 | #endif 53 | 54 | 55 | 56 | 57 | #if CSGP4_USE_FLOAT 58 | // Use single precision. 59 | #define SGPF float 60 | #define FLOOR floorf 61 | #define FABS fabsf 62 | #define COS cosf 63 | #define SIN sinf 64 | #define ATAN2 atan2f 65 | #define SQRT sqrtf 66 | #define POW powf 67 | #define FMOD fmodf 68 | #else 69 | // Use double precision. 70 | #define SGPF double 71 | #define FLOOR floor 72 | #define FABS fabs 73 | #define COS cos 74 | #define SIN sin 75 | #define ATAN2 atan2 76 | #define SQRT sqrt 77 | #define POW pow 78 | #define FMOD fmod 79 | #endif 80 | 81 | static const SGPF radiusearthkm = 6378.135; // km 82 | 83 | #define BOOL int 84 | 85 | CSGP4_DECORATOR int sgp4init_simple 86 | ( 87 | /*enum gravconsttype whichconst,*/ SGPF epoch, 88 | SGPF xbstar, SGPF xndot, SGPF xnddot, SGPF xecco, SGPF xargpo, 89 | SGPF xinclo, SGPF xmo, SGPF xno_kozai, 90 | SGPF xnodeo, SGPF initial_time, SGPF * r, SGPF * v, 91 | SGPF * a_alta_altp 92 | ) 93 | { 94 | //struct elsetrec_simple gsr = { 0 }; 95 | int error; 96 | bool method; // true for 'd' false for 'n' (Deep / Near Eart) 97 | 98 | /* Near Earth */ 99 | bool isimp; 100 | 101 | SGPF aycof, con41, cc1, cc4, cc5, d2, d3, d4, 102 | delmo, eta, argpdot, omgcof, sinmao, t, t2cof, t3cof, 103 | t4cof, t5cof, x1mth2, x7thm1, mdot, nodedot, xlcof, xmcof, nodecf; 104 | 105 | 106 | SGPF bstar, inclo, nodeo, ecco, argpo, mo; 107 | 108 | /* Deep Space */ 109 | int irez; 110 | SGPF d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, 111 | d5421, d5433, dedt, del1, del2, del3, didt, dmdt, 112 | dnodt, domdt, e3, ee2, peo, pgho, pho, pinco, 113 | plo, se2, se3, sgh2, sgh3, sgh4, sh2, sh3, 114 | si2, si3, sl2, sl3, sl4, gsto, xfact, xgh2, 115 | xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, 116 | xl4, xlamo, zmol, zmos, atime, xli, xni; 117 | // sgp4fix add unkozai'd variable 118 | SGPF no_unkozai; 119 | // sgp4fix add singly averaged variables 120 | SGPF am, em, im, Om, mm, nm; 121 | // sgp4fix add constant parameters to eliminate mutliple calls during execution 122 | SGPF tumin, mu, xke, j2, j3, j4, j3oj2; 123 | 124 | 125 | SGPF a /* Not actually used in algo but fun to look at */; 126 | SGPF no_kozai; 127 | // SGPF ndot /* Not actually used in algo*/; 128 | // SGPF nddot /* Not actually used in algo*/; 129 | SGPF alta /* Not used in algo, but cool anyway to look at */; 130 | SGPF altp /* Not used in algo, but cool anyway to look at */; 131 | 132 | /* --------------------- local variables ------------------------ */ 133 | SGPF ao, con42, cosio, sinio, cosio2, eccsq, 134 | omeosq, posq, rp, rteosq, 135 | cnodm, snodm, cosim, sinim, cosomm, sinomm, cc1sq, 136 | cc2, cc3, coef, coef1, cosio4, day, dndt, 137 | emsq, eeta, etasq, gam, argpm, nodem, 138 | inclm, perige, pinvsq, psisq, qzms24, 139 | rtemsq, s1, s2, s3, s4, s5, s6, 140 | s7, sfour, ss1, ss2, ss3, ss4, ss5, 141 | ss6, ss7, sz1, sz2, sz3, sz11, sz12, 142 | sz13, sz21, sz22, sz23, sz31, sz32, sz33, 143 | tc, temp, temp1, temp2, temp3, tsi, xpidot, 144 | xhdot1, z1, z2, z3, z11, z12, z13, 145 | z21, z22, z23, z31, z32, z33, 146 | qzms2t, ss, 147 | delmotemp, qzms2ttemp, qzms24temp; 148 | 149 | /* ------------------------ initialization --------------------- */ 150 | // sgp4fix divisor for divide by zero check on inclination 151 | // the old check used 1.0 + cos(Math.PI-1.0e-9), but then compared it to 152 | // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 153 | const SGPF temp4 = 1.5e-12; 154 | const SGPF twopi = 2.0 * SGPPI; 155 | const SGPF x2o3 = 2.0 / 3.0; 156 | 157 | #if 0 158 | /* ----------- set all near earth variables to zero ------------ */ 159 | isimp = false; aycof = 0.0; 160 | con41 = 0.0; cc1 = 0.0; cc4 = 0.0; 161 | cc5 = 0.0; d2 = 0.0; d3 = 0.0; 162 | d4 = 0.0; delmo = 0.0; eta = 0.0; 163 | argpdot = 0.0; omgcof = 0.0; siao = 0.0; 164 | t = 0.0; t2cof = 0.0; t3cof = 0.0; 165 | t4cof = 0.0; t5cof = 0.0; x1mth2 = 0.0; 166 | x7thm1 = 0.0; mdot = 0.0; nodedot = 0.0; 167 | xlcof = 0.0; xmcof = 0.0; nodecf = 0.0; 168 | 169 | /* ----------- set all deep space variables to zero ------------ */ 170 | irez = 0; d2201 = 0.0; d2211 = 0.0; 171 | d3210 = 0.0; d3222 = 0.0; d4410 = 0.0; 172 | d4422 = 0.0; d5220 = 0.0; d5232 = 0.0; 173 | d5421 = 0.0; d5433 = 0.0; dedt = 0.0; 174 | del1 = 0.0; del2 = 0.0; del3 = 0.0; 175 | didt = 0.0; dmdt = 0.0; dnodt = 0.0; 176 | domdt = 0.0; e3 = 0.0; ee2 = 0.0; 177 | peo = 0.0; pgho = 0.0; pho = 0.0; 178 | pinco = 0.0; plo = 0.0; se2 = 0.0; 179 | se3 = 0.0; sgh2 = 0.0; sgh3 = 0.0; 180 | sgh4 = 0.0; sh2 = 0.0; sh3 = 0.0; 181 | si2 = 0.0; si3 = 0.0; sl2 = 0.0; 182 | sl3 = 0.0; sl4 = 0.0; gsto = 0.0; 183 | xfact = 0.0; xgh2 = 0.0; xgh3 = 0.0; 184 | xgh4 = 0.0; xh2 = 0.0; xh3 = 0.0; 185 | xi2 = 0.0; xi3 = 0.0; xl2 = 0.0; 186 | xl3 = 0.0; xl4 = 0.0; xlamo = 0.0; 187 | zmol = 0.0; zmos = 0.0; atime = 0.0; 188 | xli = 0.0; xni = 0.0; 189 | #endif 190 | 191 | /* ------------------------ earth constants ----------------------- */ 192 | // sgp4fix identify constants and allow aernate values 193 | // this is now the only call for the constants 194 | // getgravconst_simple(/*whichconst,*/ CSGP4_REF(tumin), CSGP4_REF(mu), CSGP4_REF(radiusearthkm), CSGP4_REF(xke), 195 | // CSGP4_REF(j2), CSGP4_REF(j3), CSGP4_REF(j4), CSGP4_REF(j3oj2)); 196 | //Hard-coded WGS72 197 | (mu) = 398600.8; // in km3 / s2 198 | (xke) = 60.0 / SQRT((radiusearthkm) * (radiusearthkm) * (radiusearthkm) / (mu)); 199 | (tumin) = 1.0 / (xke); 200 | (j2) = 0.001082616; 201 | (j3) = -0.00000253881; 202 | (j4) = -0.00000165597; 203 | (j3oj2) = (j3) / (j2); 204 | 205 | 206 | //------------------------------------------------------------------------- 207 | error = 0; 208 | //operationmode = opsmode; 209 | //strcpy( satnum, satn ); 210 | 211 | // sgp4fix - note the following variables are also passed directly via 212 | // it is possible to streamline the sgp4init call by deleting the 'x' 213 | // variables, but the user would need to set the satrec* values first. we 214 | // include the additional assignments in case twoline2rv is not used. 215 | bstar = xbstar; 216 | // sgp4fix allow additional parameters in the struct 217 | // ndot = xndot; 218 | // nddot = xnddot; 219 | ecco = xecco; 220 | argpo = xargpo; 221 | inclo = xinclo; 222 | mo = xmo; 223 | // sgp4fix rename variables to clarify which mean motion is intended 224 | no_kozai = xno_kozai; 225 | nodeo = xnodeo; 226 | 227 | // single averaged mean elements 228 | am = em = im = Om = mm = nm = 0.0; 229 | 230 | /* ------------------------ earth constants ----------------------- */ 231 | // sgp4fix identify constants and allow alternate values no longer needed 232 | // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 233 | ss = 78.0 / radiusearthkm + 1.0; 234 | // sgp4fix use multiply for speed instead of Math.POW 235 | qzms2ttemp = (120.0 - 78.0) / radiusearthkm; 236 | qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp; 237 | 238 | //init = 'y'; 239 | t = 0.0; 240 | 241 | // sgp4fix remove satn as it is not needed in initl 242 | //initl_simple 243 | // (xke, j2, ecco, epoch, inclo, no_kozai, CSGP4_REF(method), 244 | // CSGP4_REF(ainv), CSGP4_REF(ao), CSGP4_REF(con41), CSGP4_REF(con42), CSGP4_REF(cosio), CSGP4_REF(cosio2), CSGP4_REF(eccsq), CSGP4_REF(omeosq), 245 | // CSGP4_REF(posq), CSGP4_REF(rp), CSGP4_REF(rteosq), CSGP4_REF(sinio), CSGP4_REF(gsto), CSGP4_REF(no_unkozai) ); 246 | //initl_simple 247 | 248 | 249 | /* --------------------- local variables ------------------------ */ 250 | SGPF ak, d1, del, adel, po; 251 | 252 | // sgp4fix use old way of finding gst 253 | SGPF ds70; 254 | SGPF ts70, tfrac, c1, thgr70, fk5r, c1p2p; 255 | 256 | 257 | /* ------------- calculate auxillary epoch quantities ---------- */ 258 | (eccsq) = ecco * ecco; 259 | (omeosq) = 1.0 - (eccsq); 260 | (rteosq) = SQRT((omeosq)); 261 | (cosio) = COS(inclo); 262 | (cosio2) = (cosio) * (cosio); 263 | 264 | /* ------------------ un-kozai the mean motion ----------------- */ 265 | ak = POW(xke / no_kozai, x2o3); 266 | d1 = 0.75 * j2 * (3.0 * (cosio2) - 1.0) / ((rteosq) * (omeosq)); 267 | del = d1 / (ak * ak); 268 | adel = ak * (1.0 - del * del - del * 269 | (1.0 / 3.0 + 134.0 * del * del / 81.0)); 270 | del = d1 / (adel * adel); 271 | (no_unkozai) = no_kozai / (1.0 + del); 272 | 273 | (ao) = POW(xke / ((no_unkozai)), x2o3); 274 | (sinio) = SIN(inclo); 275 | po = (ao) * (omeosq); 276 | (con42) = 1.0 - 5.0 * (cosio2); 277 | (con41) = -(con42) - (cosio2) - (cosio2); 278 | //(ainv) = 1.0 / (ao); 279 | (posq) = po * po; 280 | (rp) = (ao) * (1.0 - ecco); 281 | (method) = false; 282 | //*method = 'n'; 283 | 284 | // sgp4fix modern approach to finding sidereal time 285 | // ALWAYS USE A OPS MODE in this form. 286 | //if (opsmode == 'a') 287 | { 288 | // sgp4fix use old way of finding gst 289 | // count integer number of days from 0 jan 1970 290 | ts70 = epoch - 7305.0; 291 | ds70 = FLOOR(ts70 + 1.0e-8); 292 | tfrac = ts70 - ds70; 293 | // find greenwich location at epoch 294 | c1 = 1.72027916940703639e-2; 295 | thgr70 = 1.7321343856509374; 296 | fk5r = 5.07551419432269442e-15; 297 | c1p2p = c1 + twopi; 298 | (gsto) = FMOD(thgr70 + c1 * ds70 + c1p2p * tfrac + ts70 * ts70 * fk5r, twopi); 299 | if ((gsto) < 0.0) 300 | (gsto) = (gsto) + twopi; 301 | } 302 | 303 | // end initl_simple 304 | 305 | 306 | 307 | a = POW(no_unkozai * tumin, (-2.0 / 3.0)); 308 | alta = a * (1.0 + ecco) - 1.0; 309 | altp = a * (1.0 - ecco) - 1.0; 310 | error = 0; 311 | 312 | if( a_alta_altp ) 313 | { 314 | a_alta_altp[0] = a; 315 | a_alta_altp[1] = alta; 316 | a_alta_altp[2] = altp; 317 | } 318 | 319 | // sgp4fix remove this check as it is unnecessary 320 | // the mrt check in sgp4 handles decaying satellite cases even if the starting 321 | // condition is below the surface of te earth 322 | // if (rp < 1.0) 323 | // { 324 | // printf("# *** satn%d epoch elts sub-orbital ***\n", satn); 325 | // error = 5; 326 | // } 327 | 328 | if ((omeosq >= 0.0) || (no_unkozai >= 0.0)) 329 | { 330 | isimp = false; 331 | if (rp < (220.0 / radiusearthkm + 1.0)) 332 | isimp = true; 333 | sfour = ss; 334 | qzms24 = qzms2t; 335 | perige = (rp - 1.0) * radiusearthkm; 336 | 337 | /* - for perigees below 156 km, s and qoms2t are altered - */ 338 | if (perige < 156.0) 339 | { 340 | sfour = perige - 78.0; 341 | if (perige < 98.0) 342 | sfour = 20.0; 343 | // sgp4fix use multiply for speed instead of Math.POW 344 | qzms24temp = (120.0 - sfour) / radiusearthkm; 345 | qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp; 346 | sfour = sfour / radiusearthkm + 1.0; 347 | } 348 | pinvsq = 1.0 / posq; 349 | 350 | tsi = 1.0 / (ao - sfour); 351 | eta = ao * ecco * tsi; 352 | etasq = eta * eta; 353 | eeta = ecco * eta; 354 | psisq = FABS(1.0 - etasq); 355 | coef = qzms24 * POW(tsi, 4.0); 356 | coef1 = coef / POW(psisq, 3.5); 357 | cc2 = coef1 * no_unkozai * (ao * (1.0 + 1.5 * etasq + eeta * 358 | (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * con41 * 359 | (8.0 + 3.0 * etasq * (8.0 + etasq))); 360 | cc1 = bstar * cc2; 361 | cc3 = 0.0; 362 | if (ecco > 1.0e-4) 363 | cc3 = -2.0 * coef * tsi * j3oj2 * no_unkozai * sinio / ecco; 364 | x1mth2 = 1.0 - cosio2; 365 | cc4 = 2.0 * no_unkozai * coef1 * ao * omeosq * 366 | (eta * (2.0 + 0.5 * etasq) + ecco * 367 | (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * 368 | (-3.0 * con41 * (1.0 - 2.0 * eeta + etasq * 369 | (1.5 - 0.5 * eeta)) + 0.75 * x1mth2 * 370 | (2.0 * etasq - eeta * (1.0 + etasq)) * COS(2.0 * argpo))); 371 | cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * 372 | (etasq + eeta) + eeta * etasq); 373 | cosio4 = cosio2 * cosio2; 374 | temp1 = 1.5 * j2 * pinvsq * no_unkozai; 375 | temp2 = 0.5 * temp1 * j2 * pinvsq; 376 | temp3 = -0.46875 * j4 * pinvsq * pinvsq * no_unkozai; 377 | mdot = no_unkozai + 0.5 * temp1 * rteosq * con41 + 0.0625 * 378 | temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); 379 | argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * 380 | (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + 381 | temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); 382 | xhdot1 = -temp1 * cosio; 383 | nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + 384 | 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; 385 | xpidot = argpdot + nodedot; 386 | omgcof = bstar * cc3 * COS(argpo); 387 | xmcof = 0.0; 388 | if (ecco > 1.0e-4) 389 | xmcof = -x2o3 * coef * bstar / eeta; 390 | nodecf = 3.5 * omeosq * xhdot1 * cc1; 391 | t2cof = 1.5 * cc1; 392 | // sgp4fix for divide by zero with xinco = 180 deg 393 | if (FABS(cosio + 1.0) > 1.5e-12) 394 | xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); 395 | else 396 | xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; 397 | aycof = -0.5 * j3oj2 * sinio; 398 | // sgp4fix use multiply for speed instead of Math.POW 399 | delmotemp = 1.0 + eta * COS(mo); 400 | delmo = delmotemp * delmotemp * delmotemp; 401 | sinmao = SIN(mo); 402 | x7thm1 = 7.0 * cosio2 - 1.0; 403 | 404 | /* --------------- deep space initialization ------------- */ 405 | if ((2 * SGPPI / no_unkozai) >= 225.0) 406 | { 407 | method = true; 408 | isimp = true; 409 | tc = 0.0; 410 | inclm = inclo; 411 | 412 | //dscom_simple begin. 413 | 414 | SGPF nodep = nodeo; 415 | SGPF argpp = argpo; 416 | SGPF inclp = inclo; 417 | /* -------------------------- constants ------------------------- */ 418 | const SGPF zes = 0.01675; 419 | const SGPF zel = 0.05490; 420 | const SGPF c1ss = 2.9864797e-6; 421 | const SGPF c1l = 4.7968065e-7; 422 | const SGPF zsinis = 0.39785416; 423 | const SGPF zcosis = 0.91744867; 424 | const SGPF zcosgs = 0.1945905; 425 | const SGPF zsings = -0.98088458; 426 | 427 | /* --------------------- local variables ------------------------ */ 428 | int lsflg; 429 | SGPF a1, a2, a3, a4, a5, a6, a7, 430 | a8, a9, a10, betasq, cc, ctem, stem, 431 | x1, x2, x3, x4, x5, x6, x7, 432 | x8, xnodce, xnoi, zcosg, zcosgl, zcosh, zcoshl, 433 | zcosi, zcosil, zsing, zsingl, zsinh, zsinhl, zsini, 434 | zsinil, zx, zy; 435 | 436 | // sgp4fix - initialize the parameters for c# 437 | (ss1) = 0.0; 438 | (ss2) = 0.0; 439 | (ss3) = 0.0; 440 | (ss4) = 0.0; 441 | (ss5) = 0.0; 442 | (ss6) = 0.0; 443 | (ss7) = 0.0; 444 | (s1) = 0.0; 445 | (s2) = 0.0; 446 | (s3) = 0.0; 447 | (s4) = 0.0; 448 | (s5) = 0.0; 449 | (s6) = 0.0; 450 | (s7) = 0.0; 451 | (sz11) = 0.0; 452 | (sz12) = 0.0; 453 | (sz13) = 0.0; 454 | (sz1) = 0.0; 455 | (sz2) = 0.0; 456 | (sz3) = 0.0; 457 | (sz21) = 0.0; 458 | (sz22) = 0.0; 459 | (sz23) = 0.0; 460 | (sz31) = 0.0; 461 | (sz32) = 0.0; 462 | (sz33) = 0.0; 463 | (z13) = 0.0; 464 | (z21) = 0.0; 465 | (z1) = 0.0; 466 | (z2) = 0.0; 467 | (z3) = 0.0; 468 | (z11) = 0.0; 469 | (z12) = 0.0; 470 | (z31) = 0.0; 471 | (z21) = 0.0; 472 | (z22) = 0.0; 473 | (z23) = 0.0; 474 | (z32) = 0.0; 475 | (z33) = 0.0; 476 | 477 | (nm) = no_unkozai; 478 | (em) = ecco; 479 | (snodm) = SIN(nodep); 480 | (cnodm) = COS(nodep); 481 | (sinomm) = SIN(argpp); 482 | (cosomm) = COS(argpp); 483 | (sinim) = SIN(inclp); 484 | (cosim) = COS(inclp); 485 | (emsq) = (em) * (em); 486 | betasq = 1.0 - (emsq); 487 | (rtemsq) = SQRT(betasq); 488 | 489 | /* ----------------- initialize lunar solar terms --------------- */ 490 | (peo) = 0.0; 491 | (pinco) = 0.0; 492 | (plo) = 0.0; 493 | (pgho) = 0.0; 494 | (pho) = 0.0; 495 | (day) = epoch + 18261.5 + tc / 1440.0; 496 | xnodce = FMOD(4.5236020 - 9.2422029e-4 * (day), twopi); 497 | stem = SIN(xnodce); 498 | ctem = COS(xnodce); 499 | zcosil = 0.91375164 - 0.03568096 * ctem; 500 | zsinil = SQRT(1.0 - zcosil * zcosil); 501 | zsinhl = 0.089683511 * stem / zsinil; 502 | zcoshl = SQRT(1.0 - zsinhl * zsinhl); 503 | (gam) = 5.8351514 + 0.0019443680 * (day); 504 | zx = 0.39785416 * stem / zsinil; 505 | zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; 506 | zx = ATAN2(zx, zy); 507 | zx = (gam) + zx - xnodce; 508 | zcosgl = COS(zx); 509 | zsingl = SIN(zx); 510 | 511 | /* ------------------------- do solar terms --------------------- */ 512 | zcosg = zcosgs; 513 | zsing = zsings; 514 | zcosi = zcosis; 515 | zsini = zsinis; 516 | zcosh = (cnodm); 517 | zsinh = (snodm); 518 | cc = c1ss; 519 | xnoi = 1.0 / nm; 520 | 521 | for (lsflg = 1; lsflg <= 2; lsflg++) 522 | { 523 | a1 = zcosg * zcosh + zsing * zcosi * zsinh; 524 | a3 = -zsing * zcosh + zcosg * zcosi * zsinh; 525 | a7 = -zcosg * zsinh + zsing * zcosi * zcosh; 526 | a8 = zsing * zsini; 527 | a9 = zsing * zsinh + zcosg * zcosi * zcosh; 528 | a10 = zcosg * zsini; 529 | a2 = (cosim) * a7 + (sinim) * a8; 530 | a4 = (cosim) * a9 + (sinim) * a10; 531 | a5 = -(sinim) * a7 + (cosim) * a8; 532 | a6 = -(sinim) * a9 + (cosim) * a10; 533 | 534 | x1 = a1 * (cosomm) + a2 * (sinomm); 535 | x2 = a3 * (cosomm) + a4 * (sinomm); 536 | x3 = -a1 * (sinomm) + a2 * (cosomm); 537 | x4 = -a3 * (sinomm) + a4 * (cosomm); 538 | x5 = a5 * (sinomm); 539 | x6 = a6 * (sinomm); 540 | x7 = a5 * (cosomm); 541 | x8 = a6 * (cosomm); 542 | 543 | (z31) = 12.0 * x1 * x1 - 3.0 * x3 * x3; 544 | (z32) = 24.0 * x1 * x2 - 6.0 * x3 * x4; 545 | (z33) = 12.0 * x2 * x2 - 3.0 * x4 * x4; 546 | (z1) = 3.0 * (a1 * a1 + a2 * a2) + z31 * (emsq); 547 | (z2) = 6.0 * (a1 * a3 + a2 * a4) + z32 * (emsq); 548 | (z3) = 3.0 * (a3 * a3 + a4 * a4) + z33 * (emsq); 549 | (z11) = -6.0 * a1 * a5 + (emsq) * (-24.0 * x1 * x7 - 6.0 * x3 * x5); 550 | (z12) = -6.0 * (a1 * a6 + a3 * a5) + (emsq) * 551 | (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); 552 | (z13) = -6.0 * a3 * a6 + (emsq) * (-24.0 * x2 * x8 - 6.0 * x4 * x6); 553 | (z21) = 6.0 * a2 * a5 + (emsq) * (24.0 * x1 * x5 - 6.0 * x3 * x7); 554 | (z22) = 6.0 * (a4 * a5 + a2 * a6) + (emsq) * 555 | (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); 556 | (z23) = 6.0 * a4 * a6 + (emsq) * (24.0 * x2 * x6 - 6.0 * x4 * x8); 557 | (z1) = (z1) + (z1) + betasq * (z31); 558 | (z2) = (z2) + (z2) + betasq * (z32); 559 | (z3) = (z3) + (z3) + betasq * (z33); 560 | (s3) = cc * xnoi; 561 | (s2) = -0.5 * (s3) / (rtemsq); 562 | (s4) = (s3) * (rtemsq); 563 | (s1) = -15.0 * (em) * (s4); 564 | (s5) = x1 * x3 + x2 * x4; 565 | (s6) = x2 * x3 + x1 * x4; 566 | (s7) = x2 * x4 - x1 * x3; 567 | 568 | /* ----------------------- do lunar terms ------------------- */ 569 | if (lsflg == 1) 570 | { 571 | (ss1) = (s1); 572 | (ss2) = (s2); 573 | (ss3) = (s3); 574 | (ss4) = (s4); 575 | (ss5) = (s5); 576 | (ss6) = (s6); 577 | (ss7) = (s7); 578 | (sz1) = (z1); 579 | (sz2) = (z2); 580 | (sz3) = (z3); 581 | (sz11) = (z11); 582 | (sz12) = (z12); 583 | (sz13) = (z13); 584 | (sz21) = (z21); 585 | (sz22) = (z22); 586 | (sz23) = (z23); 587 | (sz31) = (z31); 588 | (sz32) = (z32); 589 | (sz33) = (z33); 590 | zcosg = zcosgl; 591 | zsing = zsingl; 592 | zcosi = zcosil; 593 | zsini = zsinil; 594 | zcosh = zcoshl * (cnodm) + zsinhl * (snodm); 595 | zsinh = (snodm) * zcoshl - (cnodm) * zsinhl; 596 | cc = c1l; 597 | } 598 | } 599 | 600 | (zmol) = FMOD( (4.7199672 + 0.22997150 * (day) - (gam)), twopi ); 601 | (zmos) = FMOD( (6.2565837 + 0.017201977 * (day)), twopi ); 602 | 603 | /* ------------------------ do solar terms ---------------------- */ 604 | (se2) = 2.0 * (ss1) * (ss6); 605 | (se3) = 2.0 * (ss1) * (ss7); 606 | (si2) = 2.0 * (ss2) * (sz12); 607 | (si3) = 2.0 * (ss2) * ((sz13) - (sz11)); 608 | (sl2) = -2.0 * (ss3) * (sz2); 609 | (sl3) = -2.0 * (ss3) * ((sz3) - (sz1)); 610 | (sl4) = -2.0 * (ss3) * (-21.0 - 9.0 * (emsq)) * zes; 611 | (sgh2) = 2.0 * (ss4) * (sz32); 612 | (sgh3) = 2.0 * (ss4) * ((sz33) - (sz31)); 613 | (sgh4) = -18.0 * (ss4) * zes; 614 | (sh2) = -2.0 * (ss2) * (sz22); 615 | (sh3) = -2.0 * (ss2) * ((sz23) - (sz21)); 616 | 617 | /* ------------------------ do lunar terms ---------------------- */ 618 | (ee2) = 2.0 * (s1) * (s6); 619 | (e3) = 2.0 * (s1) * (s7); 620 | (xi2) = 2.0 * (s2) * (z12); 621 | (xi3) = 2.0 * (s2) * ((z13) - (z11)); 622 | (xl2) = -2.0 * (s3) * (z2); 623 | (xl3) = -2.0 * (s3) * ((z3) - (z1)); 624 | (xl4) = -2.0 * (s3) * (-21.0 - 9.0 * (emsq)) * zel; 625 | (xgh2) = 2.0 * (s4) * (z32); 626 | (xgh3) = 2.0 * (s4) * ((z33) - (z31)); 627 | (xgh4) = -18.0 * (s4) * zel; 628 | (xh2) = -2.0 * (s2) * (z22); 629 | (xh3) = -2.0 * (s2) * ((z23) - (z21)); 630 | 631 | 632 | //dscom_simple end. 633 | 634 | // Original code ran dpper_simple( ... true ... ) here. But we don't actually need to. 635 | 636 | argpm = 0.0; 637 | nodem = 0.0; 638 | mm = 0.0; 639 | 640 | /* 641 | dsinit_simple 642 | ( 643 | xke, 644 | cosim, emsq, argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, 645 | ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, t, tc, 646 | gsto, mo, mdot, no_unkozai, nodeo, 647 | nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, 648 | ecco, eccsq, CSGP4_REF(em), CSGP4_REF(argpm), CSGP4_REF(inclm), CSGP4_REF(mm), CSGP4_REF(nm), CSGP4_REF(nodem), 649 | CSGP4_REF(irez), CSGP4_REF(atime), 650 | CSGP4_REF(d2201), CSGP4_REF(d2211), CSGP4_REF(d3210), CSGP4_REF(d3222), 651 | CSGP4_REF(d4410), CSGP4_REF(d4422), CSGP4_REF(d5220), CSGP4_REF(d5232), 652 | CSGP4_REF(d5421), CSGP4_REF(d5433), CSGP4_REF(dedt), CSGP4_REF(didt), 653 | CSGP4_REF(dmdt), CSGP4_REF(dndt), CSGP4_REF(dnodt), CSGP4_REF(domdt), 654 | CSGP4_REF(del1), CSGP4_REF(del2), CSGP4_REF(del3), CSGP4_REF(xfact), 655 | CSGP4_REF(xlamo), CSGP4_REF(xli), CSGP4_REF(xni) 656 | ); 657 | */ 658 | 659 | //dsinit_simple 660 | 661 | /* --------------------- local variables ------------------------ */ 662 | const SGPF twopi = 2.0 * SGPPI; 663 | 664 | SGPF ainv2, aonv = 0.0, cosisq, eoc, f220, f221, f311, 665 | f321, f322, f330, f441, f442, f522, f523, 666 | f542, f543, g200, g201, g211, g300, g310, 667 | g322, g410, g422, g520, g521, g532, g533, 668 | ses, sgs, sghl, sghs, shs, shll, sis, 669 | sini2, sls, temp, temp1, theta, xno2, q22, 670 | q31, q33, root22, root44, root54, rptim, root32, 671 | root52, x2o3, znl, emo, zns, emsqo; 672 | 673 | q22 = 1.7891679e-6; 674 | q31 = 2.1460748e-6; 675 | q33 = 2.2123015e-7; 676 | root22 = 1.7891679e-6; 677 | root44 = 7.3636953e-9; 678 | root54 = 2.1765803e-9; 679 | rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 680 | root32 = 3.7393792e-7; 681 | root52 = 1.1428639e-7; 682 | x2o3 = 2.0 / 3.0; 683 | znl = 1.5835218e-4; 684 | zns = 1.19459e-5; 685 | 686 | // sgp4fix identify constants and allow alternate values 687 | // just xke is used here so pass it in rather than have multiple calls 688 | // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 689 | 690 | /* -------------------- deep space initialization ------------ */ 691 | (irez) = 0; 692 | if (((nm) < 0.0052359877) && ((nm) > 0.0034906585)) 693 | (irez) = 1; 694 | if (((nm) >= 8.26e-3) && ((nm) <= 9.24e-3) && ((em) >= 0.5)) 695 | (irez) = 2; 696 | 697 | /* ------------------------ do solar terms ------------------- */ 698 | ses = ss1 * zns * ss5; 699 | sis = ss2 * zns * (sz11 + sz13); 700 | sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); 701 | sghs = ss4 * zns * (sz31 + sz33 - 6.0); 702 | shs = -zns * ss2 * (sz21 + sz23); 703 | // sgp4fix for 180 deg incl 704 | if (((inclm) < 5.2359877e-2) || ((inclm) > SGPPI - 5.2359877e-2)) 705 | shs = 0.0; 706 | if (sinim != 0.0) 707 | shs = shs / sinim; 708 | sgs = sghs - cosim * shs; 709 | 710 | /* ------------------------- do lunar terms ------------------ */ 711 | (dedt) = ses + s1 * znl * s5; 712 | (didt) = sis + s2 * znl * (z11 + z13); 713 | (dmdt) = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); 714 | sghl = s4 * znl * (z31 + z33 - 6.0); 715 | shll = -znl * s2 * (z21 + z23); 716 | // sgp4fix for 180 deg incl 717 | if (((inclm) < 5.2359877e-2) || ((inclm) > SGPPI - 5.2359877e-2)) 718 | shll = 0.0; 719 | (domdt) = sgs + sghl; 720 | (dnodt) = shs; 721 | if (sinim != 0.0) 722 | { 723 | (domdt) = (domdt) - cosim / sinim * shll; 724 | (dnodt) = (dnodt) + shll / sinim; 725 | } 726 | 727 | /* ----------- calculate deep space resonance effects -------- */ 728 | (dndt) = 0.0; 729 | theta = FMOD((gsto + tc * rptim), twopi ); 730 | (em) = (em) + (dedt) * t; 731 | (inclm) = (inclm) + (didt) * t; 732 | (argpm) = (argpm) + (domdt) * t; 733 | (nodem) = (nodem) + (dnodt) * t; 734 | (mm) = (mm) + (dmdt) * t; 735 | // sgp4fix for negative inclinations 736 | // the following if statement should be commented out 737 | //if (inclm < 0.0) 738 | // { 739 | // inclm = -inclm; 740 | // argpm = argpm - Math.PI; 741 | // nodem = nodem + Math.PI; 742 | // } 743 | 744 | /* -------------- initialize the resonance terms ------------- */ 745 | if ((irez) != 0) 746 | { 747 | aonv = POW((nm) / xke, x2o3); 748 | 749 | /* ---------- geopotential resonance for 12 hour orbits ------ */ 750 | if ((irez) == 2) 751 | { 752 | cosisq = cosim * cosim; 753 | emo = (em); 754 | (em) = ecco; 755 | emsqo = emsq; 756 | emsq = eccsq; 757 | eoc = (em) * emsq; 758 | g201 = -0.306 - ((em) - 0.64) * 0.440; 759 | 760 | if ((em) <= 0.65) 761 | { 762 | g211 = 3.616 - 13.2470 * (em) + 16.2900 * emsq; 763 | g310 = -19.302 + 117.3900 * (em) - 228.4190 * emsq + 156.5910 * eoc; 764 | g322 = -18.9068 + 109.7927 * (em) - 214.6334 * emsq + 146.5816 * eoc; 765 | g410 = -41.122 + 242.6940 * (em) - 471.0940 * emsq + 313.9530 * eoc; 766 | g422 = -146.407 + 841.8800 * (em) - 1629.014 * emsq + 1083.4350 * eoc; 767 | g520 = -532.114 + 3017.977 * (em) - 5740.032 * emsq + 3708.2760 * eoc; 768 | } 769 | else 770 | { 771 | g211 = -72.099 + 331.819 * (em) - 508.738 * emsq + 266.724 * eoc; 772 | g310 = -346.844 + 1582.851 * (em) - 2415.925 * emsq + 1246.113 * eoc; 773 | g322 = -342.585 + 1554.908 * (em) - 2366.899 * emsq + 1215.972 * eoc; 774 | g410 = -1052.797 + 4758.686 * (em) - 7193.992 * emsq + 3651.957 * eoc; 775 | g422 = -3581.690 + 16178.110 * (em) - 24462.770 * emsq + 12422.520 * eoc; 776 | if (em > 0.715) 777 | g520 = -5149.66 + 29936.92 * (em) - 54087.36 * emsq + 31324.56 * eoc; 778 | else 779 | g520 = 1464.74 - 4664.75 * (em) + 3763.64 * emsq; 780 | } 781 | if (em < 0.7) 782 | { 783 | g533 = -919.22770 + 4988.6100 * (em) - 9064.7700 * emsq + 5542.21 * eoc; 784 | g521 = -822.71072 + 4568.6173 * (em) - 8491.4146 * emsq + 5337.524 * eoc; 785 | g532 = -853.66600 + 4690.2500 * (em) - 8624.7700 * emsq + 5341.4 * eoc; 786 | } 787 | else 788 | { 789 | g533 = -37995.780 + 161616.52 * (em) - 229838.20 * emsq + 109377.94 * eoc; 790 | g521 = -51752.104 + 218913.95 * (em) - 309468.16 * emsq + 146349.42 * eoc; 791 | g532 = -40023.880 + 170470.89 * (em) - 242699.48 * emsq + 115605.82 * eoc; 792 | } 793 | 794 | sini2 = sinim * sinim; 795 | f220 = 0.75 * (1.0 + 2.0 * cosim + cosisq); 796 | f221 = 1.5 * sini2; 797 | f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); 798 | f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); 799 | f441 = 35.0 * sini2 * f220; 800 | f442 = 39.3750 * sini2 * sini2; 801 | f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim - 5.0 * cosisq) + 802 | 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq)); 803 | f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + 804 | 10.0 * cosisq) + 6.56250012 * (1.0 + 2.0 * cosim - 3.0 * cosisq)); 805 | f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim + cosisq * 806 | (-12.0 + 8.0 * cosim + 10.0 * cosisq)); 807 | f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim + cosisq * 808 | (12.0 + 8.0 * cosim - 10.0 * cosisq)); 809 | xno2 = (nm) * (nm); 810 | ainv2 = aonv * aonv; 811 | temp1 = 3.0 * xno2 * ainv2; 812 | temp = temp1 * root22; 813 | (d2201) = temp * f220 * g201; 814 | (d2211) = temp * f221 * g211; 815 | temp1 = temp1 * aonv; 816 | temp = temp1 * root32; 817 | (d3210) = temp * f321 * g310; 818 | (d3222) = temp * f322 * g322; 819 | temp1 = temp1 * aonv; 820 | temp = 2.0 * temp1 * root44; 821 | (d4410) = temp * f441 * g410; 822 | (d4422) = temp * f442 * g422; 823 | temp1 = temp1 * aonv; 824 | temp = temp1 * root52; 825 | (d5220) = temp * f522 * g520; 826 | (d5232) = temp * f523 * g532; 827 | temp = 2.0 * temp1 * root54; 828 | (d5421) = temp * f542 * g521; 829 | (d5433) = temp * f543 * g533; 830 | (xlamo) = FMOD( (mo + nodeo + nodeo - theta - theta), twopi ); 831 | (xfact) = mdot + (dmdt) + 2.0 * (nodedot + (dnodt) - rptim) - no_unkozai; 832 | (em) = emo; 833 | emsq = emsqo; 834 | } 835 | 836 | /* ---------------- synchronous resonance terms -------------- */ 837 | if ((irez) == 1) 838 | { 839 | g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); 840 | g310 = 1.0 + 2.0 * emsq; 841 | g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); 842 | f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); 843 | f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); 844 | f330 = 1.0 + cosim; 845 | f330 = 1.875 * f330 * f330 * f330; 846 | (del1) = 3.0 * (nm) * (nm) * aonv * aonv; 847 | (del2) = 2.0 * (del1) * f220 * g200 * q22; 848 | (del3) = 3.0 * (del1) * f330 * g300 * q33 * aonv; 849 | (del1) = (del1) * f311 * g310 * q31 * aonv; 850 | (xlamo) = FMOD(mo + nodeo + argpo - theta, twopi ); 851 | (xfact) = mdot + xpidot - rptim + (dmdt) + (domdt) + (dnodt) - no_unkozai; 852 | } 853 | 854 | /* ------------ for sgp4, initialize the integrator ---------- */ 855 | (xli) = (xlamo); 856 | (xni) = no_unkozai; 857 | (atime) = 0.0; 858 | (nm) = no_unkozai + (dndt); 859 | } 860 | 861 | //end dsinit_simple 862 | } 863 | 864 | /* ----------- set variables if not deep space ----------- */ 865 | if (isimp != true) 866 | { 867 | cc1sq = cc1 * cc1; 868 | d2 = 4.0 * ao * tsi * cc1sq; 869 | temp = d2 * tsi * cc1 / 3.0; 870 | d3 = (17.0 * ao + sfour) * temp; 871 | d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * 872 | cc1; 873 | t3cof = d2 + 2.0 * cc1sq; 874 | t4cof = 0.25 * (3.0 * d3 + cc1 * 875 | (12.0 * d2 + 10.0 * cc1sq)); 876 | t5cof = 0.2 * (3.0 * d4 + 877 | 12.0 * cc1 * d3 + 878 | 6.0 * d2 * d2 + 879 | 15.0 * cc1sq * (2.0 * d2 + cc1sq)); 880 | } 881 | } // if omeosq = 0 ... 882 | 883 | 884 | /* finally propagate to zero epoch to initialize all others. */ 885 | // sgp4fix take out check to let satellites process until they are actually below earth surface 886 | // if(error == 0) 887 | SGPF r_default[3]; 888 | SGPF v_default[3]; 889 | if( !r ) r = r_default; 890 | if( !v ) v = v_default; 891 | 892 | //sgp4_simple(satrec, initial_time, initial_r, initial_v); 893 | // SGP4_SIMPLE 894 | 895 | SGPF tsince = initial_time; 896 | 897 | SGPF axnl, aynl, betal, cnod, 898 | cos2u, coseo1, cosi, cosip, cosisq, cossu, cosu, 899 | delm, delomg, ecose, el2, eo1, 900 | ep, esine, argpdf, pl, mrt = 0.0, 901 | mvt, rdotl, rl, rvdot, rvdotl, 902 | sin2u, sineo1, sini, sinip, sinsu, sinu, 903 | snod, su, t2, t3, t4, tem5, 904 | tempa, tempe, templ, u, ux, 905 | uy, uz, vx, vy, vz, argpp, 906 | xinc, xincp, xl, xlm, mp, 907 | xmdf, xmx, xmy, nodedf, xnode, nodep, 908 | //twopi, //, j2, j3, tumin, j4, xke, j3oj2, radiusearthkm, 909 | vkmpersec, delmtemp; // mu, 910 | int ktr; 911 | 912 | // assign initial values 913 | r[0] = 0.0; 914 | r[1] = 0.0; 915 | r[2] = 0.0; 916 | v[0] = 0.0; 917 | v[1] = 0.0; 918 | v[2] = 0.0; 919 | 920 | /* ------------------ set mathematical constants --------------- */ 921 | // sgp4fix divisor for divide by zero check on inclination 922 | // the old check used 1.0 + cos(Math.PI-1.0e-9), but then compared it to 923 | // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 924 | //temp4 = 1.5e-12; 925 | //twopi = 2.0 * SGPPI; 926 | //x2o3 = 2.0 / 3.0; 927 | // sgp4fix identify constants and allow alternate values 928 | // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 929 | vkmpersec = radiusearthkm * xke / 60.0; 930 | 931 | /* --------------------- clear sgp4 error flag ----------------- */ 932 | t = tsince; 933 | error = 0; 934 | 935 | /* ------- update for secular gravity and atmospheric drag ----- */ 936 | xmdf = mo + mdot * t; 937 | argpdf = argpo + argpdot * t; 938 | nodedf = nodeo + nodedot * t; 939 | argpm = argpdf; 940 | mm = xmdf; 941 | t2 = t * t; 942 | nodem = nodedf + nodecf * t2; 943 | tempa = 1.0 - cc1 * t; 944 | tempe = bstar * cc4 * t; 945 | templ = t2cof * t2; 946 | 947 | if (isimp != true) 948 | { 949 | delomg = omgcof * t; 950 | // sgp4fix use mutliply for speed instead of Math.POW 951 | delmtemp = 1.0 + eta * COS(xmdf); 952 | delm = xmcof * 953 | (delmtemp * delmtemp * delmtemp - 954 | delmo); 955 | temp = delomg + delm; 956 | mm = xmdf + temp; 957 | argpm = argpdf - temp; 958 | t3 = t2 * t; 959 | t4 = t3 * t; 960 | tempa = tempa - d2 * t2 - d3 * t3 - 961 | d4 * t4; 962 | tempe = tempe + bstar * cc5 * (SIN(mm) - 963 | sinmao); 964 | templ = templ + t3cof * t3 + t4 * (t4cof + 965 | t * t5cof); 966 | } 967 | 968 | nm = no_unkozai; 969 | em = ecco; 970 | inclm = inclo; 971 | 972 | if (method) 973 | { 974 | tc = t; 975 | /* 976 | dspace_simple 977 | ( 978 | irez, 979 | d2201, d2211, d3210, 980 | d3222, d4410, d4422, 981 | d5220, d5232, d5421, 982 | d5433, dedt, del1, 983 | del2, del3, didt, 984 | dmdt, dnodt, domdt, 985 | argpo, argpdot, t, tc, 986 | gsto, xfact, xlamo, 987 | no_unkozai, CSGP4_REF(atime), 988 | CSGP4_REF(em), CSGP4_REF(argpm), CSGP4_REF(inclm), CSGP4_REF(xli), CSGP4_REF(mm), CSGP4_REF(xni), 989 | CSGP4_REF(nodem), CSGP4_REF(dndt), CSGP4_REF(nm) 990 | ); 991 | */ 992 | const SGPF twopi = 2.0 * SGPPI; 993 | //int iretn; //, iret; 994 | SGPF delt, ft, theta, x2li, x2omi, xl, xldot, xnddt, xndt, xomi, g22, g32, 995 | g44, g52, g54, fasx2, fasx4, fasx6, rptim, step2, stepn, stepp; 996 | 997 | fasx2 = 0.13130908; 998 | fasx4 = 2.8843198; 999 | fasx6 = 0.37448087; 1000 | g22 = 5.7686396; 1001 | g32 = 0.95240898; 1002 | g44 = 1.8014998; 1003 | g52 = 1.0508330; 1004 | g54 = 4.4108898; 1005 | rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 1006 | stepp = 720.0; 1007 | stepn = -720.0; 1008 | step2 = 259200.0; 1009 | 1010 | /* ----------- calculate deep space resonance effects ----------- */ 1011 | dndt = 0.0; 1012 | theta = FMOD( (gsto + tc * rptim), twopi ); 1013 | em = em + dedt * t; 1014 | 1015 | inclm = inclm + didt * t; 1016 | argpm = argpm + domdt * t; 1017 | nodem = nodem + dnodt * t; 1018 | mm = mm + dmdt * t; 1019 | 1020 | // sgp4fix for negative inclinations 1021 | // the following if statement should be commented out 1022 | // if (inclm < 0.0) 1023 | // { 1024 | // inclm = -inclm; 1025 | // argpm = argpm - Math.PI; 1026 | // nodem = nodem + Math.PI; 1027 | // } 1028 | 1029 | /* - update resonances : numerical (euler-maclaurin) integration - */ 1030 | /* ------------------------- epoch restart ---------------------- */ 1031 | // sgp4fix for propagator problems 1032 | // the following integration works for negative time steps and periods 1033 | // the specific changes are unknown because the original code was so convoluted 1034 | // sgp4fix set initial values for c# 1035 | xndt = 0.0; 1036 | xnddt = 0.0; 1037 | xldot = 0.0; 1038 | 1039 | // sgp4fix take out atime = 0.0 and fix for faster operation 1040 | ft = 0.0; 1041 | if (irez != 0) 1042 | { 1043 | // sgp4fix streamline check 1044 | if ((atime == 0.0) || (t * atime <= 0.0) || (FABS(t) < FABS(atime))) 1045 | { 1046 | atime = 0.0; 1047 | xni = no_unkozai; 1048 | xli = xlamo; 1049 | } 1050 | SGPF xli_cache = xli; 1051 | // sgp4fix move check outside loop 1052 | if (t > 0.0) 1053 | delt = stepp; 1054 | else 1055 | delt = stepn; 1056 | 1057 | //iretn = 381; // added for do loop 1058 | //iret = 0; // added for loop 1059 | //while (iretn == 381) 1060 | //bool iretn = true; 1061 | //do 1062 | // printf( "ITER: %f / %f %f\n", t-atime, stepp, delt ); 1063 | int maxiter = (t-atime)/delt + 2; //+1 just in case. +1 is required. 1064 | if( irez != 2 ) 1065 | { 1066 | for( int i = 0; i < maxiter; i++ ) 1067 | { 1068 | /* ------------------- dot terms calculated ------------- */ 1069 | /* ----------- near - synchronous resonance terms ------- */ 1070 | xndt = del1 * SIN(xli_cache - fasx2) + del2 * SIN(2.0 * (xli_cache - fasx4)) + 1071 | del3 * SIN(3.0 * (xli_cache - fasx6)); 1072 | xldot = xni + xfact; 1073 | xnddt = del1 * COS(xli_cache - fasx2) + 1074 | 2.0 * del2 * COS(2.0 * (xli_cache - fasx4)) + 1075 | 3.0 * del3 * COS(3.0 * (xli_cache - fasx6)); 1076 | xnddt = xnddt * xldot; 1077 | 1078 | /* ----------------------- integrator ------------------- */ 1079 | // sgp4fix move end checks to end of routine 1080 | if (FABS(t - atime) >= stepp) 1081 | { 1082 | //iret = 0; 1083 | //iretn = true; 1084 | xli_cache = xli_cache + xldot * delt + xndt * step2; 1085 | xni = xni + xndt * delt + xnddt * step2; 1086 | atime = atime + delt; 1087 | } 1088 | else // exit here 1089 | { 1090 | ft = t - atime; 1091 | //iretn = false; 1092 | break; 1093 | } 1094 | } 1095 | } 1096 | else 1097 | { 1098 | for( int i = 0; i < maxiter; i++ ) 1099 | { 1100 | /* --------- near - half-day resonance terms -------- */ 1101 | xomi = argpo + argpdot * atime; 1102 | x2omi = xomi + xomi; 1103 | x2li = xli_cache + xli_cache; 1104 | xndt = d2201 * SIN(x2omi + xli_cache - g22) + d2211 * SIN(xli_cache - g22) + 1105 | d3210 * SIN(xomi + xli_cache - g32) + d3222 * SIN(-xomi + xli_cache - g32) + 1106 | d4410 * SIN(x2omi + x2li - g44) + d4422 * SIN(x2li - g44) + 1107 | d5220 * SIN(xomi + xli_cache- g52) + d5232 * SIN(-xomi + xli - g52) + 1108 | d5421 * SIN(xomi + x2li - g54) + d5433 * SIN(-xomi + x2li - g54); 1109 | xldot = xni + xfact; 1110 | xnddt = d2201 * COS(x2omi + xli_cache - g22) + d2211 * COS(xli_cache - g22) + 1111 | d3210 * COS(xomi + xli_cache - g32) + d3222 * COS(-xomi + xli_cache - g32) + 1112 | d5220 * COS(xomi + xli_cache - g52) + d5232 * COS(-xomi + xli_cache - g52) + 1113 | 2.0 * (d4410 * COS(x2omi + x2li - g44) + 1114 | d4422 * COS(x2li - g44) + d5421 * COS(xomi + x2li - g54) + 1115 | d5433 * COS(-xomi + x2li - g54)); 1116 | xnddt = xnddt * xldot; 1117 | /* ----------------------- integrator ------------------- */ 1118 | // sgp4fix move end checks to end of routine 1119 | if (FABS(t - atime) >= stepp) 1120 | { 1121 | //iret = 0; 1122 | //iretn = true; 1123 | xli_cache = xli_cache + xldot * delt + xndt * step2; 1124 | xni = xni + xndt * delt + xnddt * step2; 1125 | atime = atime + delt; 1126 | } 1127 | else // exit here 1128 | { 1129 | ft = t - atime; 1130 | //iretn = false; 1131 | break; 1132 | } 1133 | } 1134 | } 1135 | nm = xni + xndt * ft + xnddt * ft * ft * 0.5; 1136 | xl = xli_cache + xldot * ft + xndt * ft * ft * 0.5; 1137 | xli = xli_cache; 1138 | if (irez != 1) 1139 | { 1140 | mm = xl - 2.0 * nodem + 2.0 * theta; 1141 | dndt = nm - no_unkozai; 1142 | } 1143 | else 1144 | { 1145 | mm = xl - nodem - argpm + theta; 1146 | dndt = nm - no_unkozai; 1147 | } 1148 | nm = no_unkozai + dndt; 1149 | } 1150 | 1151 | 1152 | } // if method = true 1153 | 1154 | if (nm <= 0.0) 1155 | { 1156 | // printf("# error nm %f\n", nm); 1157 | error = 2; 1158 | // sgp4fix add return 1159 | //return false; 1160 | } 1161 | 1162 | am = POW((xke / nm), x2o3) * tempa * tempa; 1163 | nm = xke / POW(am, 1.5); 1164 | em = em - tempe; 1165 | 1166 | // fix tolerance for error recognition 1167 | // sgp4fix am is fixed from the previous nm check 1168 | if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/ ) 1169 | { 1170 | // printf("# error em %f\n", em); 1171 | error = 1; 1172 | // sgp4fix to return if there is an error in eccentricity 1173 | //return false; 1174 | } 1175 | // sgp4fix fix tolerance to avoid a divide by zero 1176 | if (em < 1.0e-6) 1177 | em = 1.0e-6; 1178 | mm = mm + no_unkozai * templ; 1179 | xlm = mm + argpm + nodem; 1180 | emsq = em * em; 1181 | temp = 1.0 - emsq; 1182 | 1183 | nodem = FMOD((nodem), twopi ); 1184 | argpm = FMOD((argpm), twopi ); 1185 | xlm = FMOD((xlm), twopi ); 1186 | mm = FMOD((xlm - argpm - nodem), twopi); 1187 | 1188 | // sgp4fix recover singly averaged mean elements 1189 | am = am; 1190 | em = em; 1191 | im = inclm; 1192 | Om = nodem; 1193 | // om = argpm; 1194 | mm = mm; 1195 | nm = nm; 1196 | 1197 | /* ----------------- compute extra mean quantities ------------- */ 1198 | sinim = SIN(inclm); 1199 | cosim = COS(inclm); 1200 | 1201 | /* -------------------- add lunar-solar periodics -------------- */ 1202 | ep = em; 1203 | xincp = inclm; 1204 | argpp = argpm; 1205 | nodep = nodem; 1206 | mp = mm; 1207 | sinip = sinim; 1208 | cosip = cosim; 1209 | if (method) 1210 | { 1211 | /* 1212 | dpper_simple 1213 | ( 1214 | e3, ee2, peo, 1215 | pgho, pho, pinco, 1216 | plo, se2, se3, 1217 | sgh2, sgh3, sgh4, 1218 | sh2, sh3, si2, 1219 | si3, sl2, sl3, 1220 | sl4, t, xgh2, 1221 | xgh3, xgh4, xh2, 1222 | xh3, xi2, xi3, 1223 | xl2, xl3, xl4, 1224 | zmol, zmos, inclo, 1225 | false, CSGP4_REF(ep), CSGP4_REF(xincp), CSGP4_REF(nodep), CSGP4_REF(argpp), CSGP4_REF(mp) 1226 | ); 1227 | */ 1228 | 1229 | //dpper_simple 1230 | /* --------------------- local variables ------------------------ */ 1231 | const SGPF twopi = 2.0 * SGPPI; 1232 | SGPF alfdp, betdp, cosip, cosop, dalf, dbet, dls, 1233 | f2, f3, pe, pgh, ph, pinc, pl, 1234 | sel, ses, sghl, sghs, shll, shs, sil, 1235 | sinip, sinop, sinzf, sis, sll, sls, xls, 1236 | xnoh, zf, zm, zel, zes, znl, zns; 1237 | 1238 | /* ---------------------- constants ----------------------------- */ 1239 | zns = 1.19459e-5; 1240 | zes = 0.01675; 1241 | znl = 1.5835218e-4; 1242 | zel = 0.05490; 1243 | 1244 | /* --------------- calculate time varying periodics ----------- */ 1245 | zm = zmos + zns * t; 1246 | // be sure that the initial call has time set to zero 1247 | //if (init) 1248 | // zm = zmos; 1249 | zf = zm + 2.0 * zes * SIN(zm); 1250 | sinzf = SIN(zf); 1251 | f2 = 0.5 * sinzf * sinzf - 0.25; 1252 | f3 = -0.5 * sinzf * COS(zf); 1253 | ses = se2 * f2 + se3 * f3; 1254 | sis = si2 * f2 + si3 * f3; 1255 | sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; 1256 | sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; 1257 | shs = sh2 * f2 + sh3 * f3; 1258 | zm = zmol + znl * t; 1259 | //if (init) 1260 | // zm = zmol; 1261 | zf = zm + 2.0 * zel * SIN(zm); 1262 | sinzf = SIN(zf); 1263 | f2 = 0.5 * sinzf * sinzf - 0.25; 1264 | f3 = -0.5 * sinzf * COS(zf); 1265 | sel = ee2 * f2 + e3 * f3; 1266 | sil = xi2 * f2 + xi3 * f3; 1267 | sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; 1268 | sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; 1269 | shll = xh2 * f2 + xh3 * f3; 1270 | pe = ses + sel; 1271 | pinc = sis + sil; 1272 | pl = sls + sll; 1273 | pgh = sghs + sghl; 1274 | ph = shs + shll; 1275 | 1276 | // not init 1277 | { 1278 | pe = pe - peo; 1279 | pinc = pinc - pinco; 1280 | pl = pl - plo; 1281 | pgh = pgh - pgho; 1282 | ph = ph - pho; 1283 | (xincp) = (xincp) + pinc; 1284 | (ep) = (ep) + pe; 1285 | sinip = SIN((xincp)); 1286 | cosip = COS((xincp)); 1287 | 1288 | /* ----------------- apply periodics directly ------------ */ 1289 | // sgp4fix for lyddane choice 1290 | // strn3 used original inclination - this is technically feasible 1291 | // gsfc used perturbed inclination - also technically feasible 1292 | // probably best to readjust the 0.2 limit value and limit discontinuity 1293 | // 0.2 rad = 11.45916 deg 1294 | // use next line for original strn3 approach and original inclination 1295 | // if (inclo >= 0.2) 1296 | // use next line for gsfc version and perturbed inclination 1297 | if ((xincp) >= 0.2) 1298 | { 1299 | ph = ph / sinip; 1300 | pgh = pgh - cosip * ph; 1301 | (argpp) = (argpp) + pgh; 1302 | (nodep) = (nodep) + ph; 1303 | (mp) = (mp) + pl; 1304 | } 1305 | else 1306 | { 1307 | /* ---- apply periodics with lyddane modification ---- */ 1308 | sinop = SIN((nodep)); 1309 | cosop = COS((nodep)); 1310 | alfdp = sinip * sinop; 1311 | betdp = sinip * cosop; 1312 | dalf = ph * cosop + pinc * cosip * sinop; 1313 | dbet = -ph * sinop + pinc * cosip * cosop; 1314 | alfdp = alfdp + dalf; 1315 | betdp = betdp + dbet; 1316 | (nodep) = FMOD( (nodep), twopi ); 1317 | // sgp4fix for afspc written intrinsic functions 1318 | // nodep used without a trigonometric function ahead 1319 | if (((nodep) < 0.0)) // && (opsmode == 'a')) 1320 | (nodep) = (nodep) + twopi; 1321 | xls = (mp) + (argpp) + cosip * (nodep); 1322 | dls = pl + pgh - pinc * (nodep) * sinip; 1323 | xls = xls + dls; 1324 | xnoh = (nodep); 1325 | (nodep) = ATAN2(alfdp, betdp); 1326 | // sgp4fix for afspc written intrinsic functions 1327 | // nodep used without a trigonometric function ahead 1328 | if (((nodep) < 0.0)) // && (opsmode == 'a')) 1329 | (nodep) = (nodep) + twopi; 1330 | if (FABS(xnoh - (nodep)) > SGPPI) 1331 | { 1332 | if ((nodep) < xnoh) 1333 | (nodep) = (nodep) + twopi; 1334 | else 1335 | (nodep) = (nodep) - twopi; 1336 | } 1337 | (mp) = (mp) + pl; 1338 | (argpp) = xls - (mp) - cosip * (nodep); 1339 | } 1340 | } // if init == 'n' 1341 | //end dpper_simple 1342 | 1343 | 1344 | if (xincp < 0.0) 1345 | { 1346 | xincp = -xincp; 1347 | nodep = nodep + SGPPI; 1348 | argpp = argpp - SGPPI; 1349 | } 1350 | if ((ep < 0.0) || (ep > 1.0)) 1351 | { 1352 | // printf("# error ep %f\n", ep); 1353 | error = 3; 1354 | // sgp4fix add return 1355 | //return false; 1356 | } 1357 | 1358 | } // if method = true 1359 | 1360 | /* -------------------- long period periodics ------------------ */ 1361 | if (method) 1362 | { 1363 | sinip = SIN(xincp); 1364 | cosip = COS(xincp); 1365 | aycof = -0.5 * j3oj2 * sinip; 1366 | // sgp4fix for divide by zero for xincp = 180 deg 1367 | if (FABS(cosip + 1.0) > 1.5e-12) 1368 | xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); 1369 | else 1370 | xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; 1371 | } 1372 | axnl = ep * COS(argpp); 1373 | temp = 1.0 / (am * (1.0 - ep * ep)); 1374 | aynl = ep * SIN(argpp) + temp * aycof; 1375 | xl = mp + argpp + nodep + temp * xlcof * axnl; 1376 | 1377 | /* --------------------- solve kepler's equation --------------- */ 1378 | u = FMOD( (xl - nodep), twopi ); 1379 | eo1 = u; 1380 | tem5 = 9999.9; 1381 | ktr = 1; 1382 | // sgp4fix for c# intiialize 1383 | coseo1 = 0.0; 1384 | sineo1 = 0.0; 1385 | // sgp4fix for kepler iteration 1386 | // the following iteration needs better limits on corrections 1387 | while ((FABS(tem5) >= 1.0e-12) && (ktr <= 10)) 1388 | { 1389 | sineo1 = SIN(eo1); 1390 | coseo1 = COS(eo1); 1391 | tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; 1392 | tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; 1393 | if (FABS(tem5) >= 0.95) 1394 | { 1395 | tem5 = tem5 > 0.0 ? 0.95 : -0.95; 1396 | } 1397 | eo1 = eo1 + tem5; 1398 | ktr = ktr + 1; 1399 | } 1400 | 1401 | /* ------------- short period preliminary quantities ----------- */ 1402 | ecose = axnl * coseo1 + aynl * sineo1; 1403 | esine = axnl * sineo1 - aynl * coseo1; 1404 | el2 = axnl * axnl + aynl * aynl; 1405 | pl = am * (1.0 - el2); 1406 | if (pl < 0.0) 1407 | { 1408 | // printf("# error pl %f\n", pl); 1409 | error = 4; 1410 | // sgp4fix add return 1411 | //return false; 1412 | } 1413 | else 1414 | { 1415 | rl = am * (1.0 - ecose); 1416 | rdotl = SQRT(am) * esine / rl; 1417 | rvdotl = SQRT(pl) / rl; 1418 | betal = SQRT(1.0 - el2); 1419 | temp = esine / (1.0 + betal); 1420 | sinu = am / rl * (sineo1 - aynl - axnl * temp); 1421 | cosu = am / rl * (coseo1 - axnl + aynl * temp); 1422 | su = ATAN2(sinu, cosu); 1423 | sin2u = (cosu + cosu) * sinu; 1424 | cos2u = 1.0 - 2.0 * sinu * sinu; 1425 | temp = 1.0 / pl; 1426 | temp1 = 0.5 * j2 * temp; 1427 | temp2 = temp1 * temp; 1428 | 1429 | /* -------------- update for short period periodics ------------ */ 1430 | if (method ) // == 'd') 1431 | { 1432 | cosisq = cosip * cosip; 1433 | con41 = 3.0 * cosisq - 1.0; 1434 | x1mth2 = 1.0 - cosisq; 1435 | x7thm1 = 7.0 * cosisq - 1.0; 1436 | } 1437 | mrt = rl * (1.0 - 1.5 * temp2 * betal * con41) + 1438 | 0.5 * temp1 * x1mth2 * cos2u; 1439 | su = su - 0.25 * temp2 * x7thm1 * sin2u; 1440 | xnode = nodep + 1.5 * temp2 * cosip * sin2u; 1441 | xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; 1442 | mvt = rdotl - nm * temp1 * x1mth2 * sin2u / xke; 1443 | rvdot = rvdotl + nm * temp1 * (x1mth2 * cos2u + 1444 | 1.5 * con41) / xke; 1445 | 1446 | /* --------------------- orientation vectors ------------------- */ 1447 | sinsu = SIN(su); 1448 | cossu = COS(su); 1449 | snod = SIN(xnode); 1450 | cnod = COS(xnode); 1451 | sini = SIN(xinc); 1452 | cosi = COS(xinc); 1453 | xmx = -snod * cosi; 1454 | xmy = cnod * cosi; 1455 | ux = xmx * sinsu + cnod * cossu; 1456 | uy = xmy * sinsu + snod * cossu; 1457 | uz = sini * sinsu; 1458 | vx = xmx * cossu - cnod * sinsu; 1459 | vy = xmy * cossu - snod * sinsu; 1460 | vz = sini * cossu; 1461 | 1462 | /* --------- position and velocity (in km and km/sec) ---------- */ 1463 | r[0] = (mrt * ux) * radiusearthkm; 1464 | r[1] = (mrt * uy) * radiusearthkm; 1465 | r[2] = (mrt * uz) * radiusearthkm; 1466 | v[0] = (mvt * ux + rvdot * vx) * vkmpersec; 1467 | v[1] = (mvt * uy + rvdot * vy) * vkmpersec; 1468 | v[2] = (mvt * uz + rvdot * vz) * vkmpersec; 1469 | } // if pl > 0 1470 | 1471 | // sgp4fix for decaying satellites 1472 | if (mrt < 1.0) 1473 | { 1474 | // printf("# decay condition %11.6f \n",mrt); 1475 | error = 6; 1476 | //return false; 1477 | } 1478 | 1479 | return error; 1480 | //#include "debug7.cpp" 1481 | //return true; 1482 | 1483 | //END SGP4_SIMPLE 1484 | 1485 | 1486 | //init = 'n'; 1487 | 1488 | //#include "debug6.cpp" 1489 | //sgp4fix return boolean. error contains any error codes 1490 | //return true; 1491 | } // end sgp4init 1492 | 1493 | #endif 1494 | 1495 | -------------------------------------------------------------------------------- /os_generic.h: -------------------------------------------------------------------------------- 1 | #ifndef _OS_GENERIC_H 2 | #define _OS_GENERIC_H 3 | /* 4 | "osgeneric" Generic, platform independent tool for threads and time. 5 | Geared around Windows and Linux. Designed for operation on MSVC, 6 | TCC, GCC and clang. Others may work. 7 | 8 | It offers the following operations: 9 | 10 | Delay functions: 11 | void OGSleep( int is ); 12 | void OGUSleep( int ius ); 13 | 14 | Getting current time (may be time from program start, boot, or epoc) 15 | double OGGetAbsoluteTime(); 16 | double OGGetFileTime( const char * file ); 17 | 18 | Thread functions 19 | og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter ); 20 | void * OGJoinThread( og_thread_t ot ); 21 | void OGCancelThread( og_thread_t ot ); 22 | 23 | Mutex functions, used for protecting data structures. 24 | (recursive on platforms where available.) 25 | og_mutex_t OGCreateMutex(); 26 | void OGLockMutex( og_mutex_t om ); 27 | void OGUnlockMutex( og_mutex_t om ); 28 | void OGDeleteMutex( og_mutex_t om ); 29 | 30 | Always a semaphore (not recursive) 31 | og_sema_t OGCreateSema(); //Create a semaphore, comes locked initially. 32 | NOTE: For platform compatibility, max count is 32767 33 | void OGLockSema( og_sema_t os ); 34 | int OGGetSema( og_sema_t os ); //if <0 there was a failure. 35 | void OGUnlockSema( og_sema_t os ); 36 | void OGDeleteSema( og_sema_t os ); 37 | 38 | TLS (Thread-Local Storage) 39 | og_tls_t OGCreateTLS(); 40 | void OGDeleteTLS( og_tls_t tls ); 41 | void OGSetTLS( og_tls_t tls, void * data ); 42 | void * OGGetTLS( og_tls_t tls ); 43 | 44 | You can permute the operations of this file by the following means: 45 | OSG_NO_IMPLEMENTATION 46 | OSG_PREFIX 47 | OSG_NOSTATIC 48 | 49 | The default behavior is to do static inline. 50 | 51 | Copyright (c) 2011-2012,2013,2016,2018,2019,2020 <>< Charles Lohr 52 | 53 | This file may be licensed under the MIT/x11 license, NewBSD or CC0 licenses 54 | 55 | Permission is hereby granted, free of charge, to any person obtaining a 56 | copy of this software and associated documentation files (the "Software"), 57 | to deal in the Software without restriction, including without limitation 58 | the rights to use, copy, modify, merge, publish, distribute, sublicense, 59 | and/or sell copies of the Software, and to permit persons to whom the 60 | Software is furnished to do so, subject to the following conditions: 61 | 62 | The above copyright notice and this permission notice shall be included in 63 | all copies or substantial portions of this file. 64 | 65 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 66 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 67 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 68 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 69 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 70 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 71 | IN THE SOFTWARE. 72 | 73 | Date Stamp: 2019-09-05 CNL: Allow for noninstantiation and added TLS. 74 | Date Stamp: 2018-03-25 CNL: Switched to header-only format. 75 | */ 76 | 77 | 78 | #if defined( OSG_NOSTATIC ) && OSG_NOSTATIC != 0 79 | #ifndef OSG_PREFIX 80 | #define OSG_PREFIX 81 | #endif 82 | #ifndef OSG_NO_IMPLEMENTATION 83 | #define OSG_NO_IMPLEMENTATION 84 | #endif 85 | #endif 86 | 87 | #ifndef OSG_PREFIX 88 | #ifdef __wasm__ 89 | #define OSG_PREFIX 90 | #else 91 | #define OSG_PREFIX static inline 92 | #endif 93 | #endif 94 | 95 | //In case you want to hook the closure of a thread, i.e. if your system has thread-local storage. 96 | #ifndef OSG_TERM_THREAD_CODE 97 | #define OSG_TERM_THREAD_CODE 98 | #endif 99 | 100 | typedef void* og_thread_t; 101 | typedef void* og_mutex_t; 102 | typedef void* og_sema_t; 103 | typedef void* og_tls_t; 104 | 105 | #ifdef __cplusplus 106 | extern "C" { 107 | #endif 108 | 109 | OSG_PREFIX void OGSleep( int is ); 110 | OSG_PREFIX void OGUSleep( int ius ); 111 | OSG_PREFIX double OGGetAbsoluteTime(); 112 | OSG_PREFIX double OGGetFileTime( const char * file ); 113 | OSG_PREFIX og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter ); 114 | OSG_PREFIX void * OGJoinThread( og_thread_t ot ); 115 | OSG_PREFIX void OGCancelThread( og_thread_t ot ); 116 | OSG_PREFIX og_mutex_t OGCreateMutex(); 117 | OSG_PREFIX void OGLockMutex( og_mutex_t om ); 118 | OSG_PREFIX void OGUnlockMutex( og_mutex_t om ); 119 | OSG_PREFIX void OGDeleteMutex( og_mutex_t om ); 120 | OSG_PREFIX og_sema_t OGCreateSema(); 121 | OSG_PREFIX int OGGetSema( og_sema_t os ); 122 | OSG_PREFIX void OGLockSema( og_sema_t os ); 123 | OSG_PREFIX void OGUnlockSema( og_sema_t os ); 124 | OSG_PREFIX void OGDeleteSema( og_sema_t os ); 125 | OSG_PREFIX og_tls_t OGCreateTLS(); 126 | OSG_PREFIX void OGDeleteTLS( og_tls_t key ); 127 | OSG_PREFIX void * OGGetTLS( og_tls_t key ); 128 | OSG_PREFIX void OGSetTLS( og_tls_t key, void * data ); 129 | 130 | #ifdef __cplusplus 131 | }; 132 | #endif 133 | 134 | #ifndef OSG_NO_IMPLEMENTATION 135 | 136 | #if defined( WIN32 ) || defined (WINDOWS) || defined( _WIN32) 137 | #define USE_WINDOWS 138 | #endif 139 | 140 | 141 | #ifdef __cplusplus 142 | extern "C" { 143 | #endif 144 | 145 | 146 | #ifdef USE_WINDOWS 147 | 148 | #include 149 | #include 150 | 151 | OSG_PREFIX void OGSleep( int is ) 152 | { 153 | Sleep( is*1000 ); 154 | } 155 | 156 | OSG_PREFIX void OGUSleep( int ius ) 157 | { 158 | Sleep( ius/1000 ); 159 | } 160 | 161 | OSG_PREFIX double OGGetAbsoluteTime() 162 | { 163 | static LARGE_INTEGER lpf; 164 | LARGE_INTEGER li; 165 | 166 | if( !lpf.QuadPart ) 167 | { 168 | QueryPerformanceFrequency( &lpf ); 169 | } 170 | 171 | QueryPerformanceCounter( &li ); 172 | return (double)li.QuadPart / (double)lpf.QuadPart; 173 | } 174 | 175 | 176 | OSG_PREFIX double OGGetFileTime( const char * file ) 177 | { 178 | FILETIME ft; 179 | 180 | HANDLE h = CreateFile(file, GENERIC_READ, FILE_SHARE_READ, NULL, OPEN_EXISTING, 0, NULL); 181 | 182 | if( h==INVALID_HANDLE_VALUE ) 183 | return -1; 184 | 185 | GetFileTime( h, 0, 0, &ft ); 186 | 187 | CloseHandle( h ); 188 | 189 | return ft.dwHighDateTime + ft.dwLowDateTime; 190 | } 191 | 192 | 193 | OSG_PREFIX og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter ) 194 | { 195 | return (og_thread_t)CreateThread( 0, 0, (LPTHREAD_START_ROUTINE)routine, parameter, 0, 0 ); 196 | } 197 | 198 | OSG_PREFIX void * OGJoinThread( og_thread_t ot ) 199 | { 200 | WaitForSingleObject( ot, INFINITE ); 201 | OSG_TERM_THREAD_CODE 202 | CloseHandle( ot ); 203 | return 0; 204 | } 205 | 206 | OSG_PREFIX void OGCancelThread( og_thread_t ot ) 207 | { 208 | OSG_TERM_THREAD_CODE 209 | TerminateThread( ot, 0); 210 | CloseHandle( ot ); 211 | } 212 | 213 | OSG_PREFIX og_mutex_t OGCreateMutex() 214 | { 215 | return CreateMutex( 0, 0, 0 ); 216 | } 217 | 218 | OSG_PREFIX void OGLockMutex( og_mutex_t om ) 219 | { 220 | WaitForSingleObject(om, INFINITE); 221 | } 222 | 223 | OSG_PREFIX void OGUnlockMutex( og_mutex_t om ) 224 | { 225 | ReleaseMutex(om); 226 | } 227 | 228 | OSG_PREFIX void OGDeleteMutex( og_mutex_t om ) 229 | { 230 | CloseHandle( om ); 231 | } 232 | 233 | 234 | 235 | OSG_PREFIX og_sema_t OGCreateSema() 236 | { 237 | HANDLE sem = CreateSemaphore( 0, 0, 32767, 0 ); 238 | return (og_sema_t)sem; 239 | } 240 | 241 | OSG_PREFIX int OGGetSema( og_sema_t os ) 242 | { 243 | typedef LONG NTSTATUS; 244 | HANDLE sem = (HANDLE)os; 245 | typedef NTSTATUS (NTAPI *_NtQuerySemaphore)( 246 | HANDLE SemaphoreHandle, 247 | DWORD SemaphoreInformationClass, /* Would be SEMAPHORE_INFORMATION_CLASS */ 248 | PVOID SemaphoreInformation, /* but this is to much to dump here */ 249 | ULONG SemaphoreInformationLength, 250 | PULONG ReturnLength OPTIONAL 251 | ); 252 | 253 | typedef struct _SEMAPHORE_BASIC_INFORMATION { 254 | ULONG CurrentCount; 255 | ULONG MaximumCount; 256 | } SEMAPHORE_BASIC_INFORMATION; 257 | 258 | 259 | static _NtQuerySemaphore NtQuerySemaphore; 260 | SEMAPHORE_BASIC_INFORMATION BasicInfo; 261 | NTSTATUS Status; 262 | 263 | if( !NtQuerySemaphore ) 264 | { 265 | NtQuerySemaphore = (_NtQuerySemaphore)GetProcAddress (GetModuleHandle ("ntdll.dll"), "NtQuerySemaphore"); 266 | if( !NtQuerySemaphore ) 267 | { 268 | return -1; 269 | } 270 | } 271 | 272 | 273 | Status = NtQuerySemaphore (sem, 0 /*SemaphoreBasicInformation*/, 274 | &BasicInfo, sizeof (SEMAPHORE_BASIC_INFORMATION), NULL); 275 | 276 | if (Status == ERROR_SUCCESS) 277 | { 278 | return BasicInfo.CurrentCount; 279 | } 280 | 281 | return -2; 282 | } 283 | 284 | OSG_PREFIX void OGLockSema( og_sema_t os ) 285 | { 286 | WaitForSingleObject( (HANDLE)os, INFINITE ); 287 | } 288 | 289 | OSG_PREFIX void OGUnlockSema( og_sema_t os ) 290 | { 291 | ReleaseSemaphore( (HANDLE)os, 1, 0 ); 292 | } 293 | 294 | OSG_PREFIX void OGDeleteSema( og_sema_t os ) 295 | { 296 | CloseHandle( os ); 297 | } 298 | 299 | OSG_PREFIX og_tls_t OGCreateTLS() 300 | { 301 | return (og_tls_t)(intptr_t)TlsAlloc(); 302 | } 303 | 304 | OSG_PREFIX void OGDeleteTLS( og_tls_t key ) 305 | { 306 | TlsFree( (DWORD)(intptr_t)key ); 307 | } 308 | 309 | OSG_PREFIX void * OGGetTLS( og_tls_t key ) 310 | { 311 | return TlsGetValue( (DWORD)(intptr_t)key ); 312 | } 313 | 314 | OSG_PREFIX void OGSetTLS( og_tls_t key, void * data ) 315 | { 316 | TlsSetValue( (DWORD)(intptr_t)key, data ); 317 | } 318 | 319 | #elif defined( __wasm__ ) 320 | 321 | //We don't actually have any function defintions here. 322 | //The outside system will handle it. 323 | 324 | #else 325 | 326 | #ifndef _GNU_SOURCE 327 | #define _GNU_SOURCE 328 | #endif 329 | 330 | #include 331 | #include 332 | #include 333 | #include 334 | #include 335 | #include 336 | 337 | OSG_PREFIX void OGSleep( int is ) 338 | { 339 | sleep( is ); 340 | } 341 | 342 | OSG_PREFIX void OGUSleep( int ius ) 343 | { 344 | usleep( ius ); 345 | } 346 | 347 | OSG_PREFIX double OGGetAbsoluteTime() 348 | { 349 | struct timeval tv; 350 | gettimeofday( &tv, 0 ); 351 | return ((double)tv.tv_usec)/1000000. + (tv.tv_sec); 352 | } 353 | 354 | OSG_PREFIX double OGGetFileTime( const char * file ) 355 | { 356 | struct stat buff; 357 | 358 | int r = stat( file, &buff ); 359 | 360 | if( r < 0 ) 361 | { 362 | return -1; 363 | } 364 | 365 | return buff.st_mtime; 366 | } 367 | 368 | 369 | 370 | OSG_PREFIX og_thread_t OGCreateThread( void * (routine)( void * ), void * parameter ) 371 | { 372 | pthread_t * ret = (pthread_t *)malloc( sizeof( pthread_t ) ); 373 | if( !ret ) return 0; 374 | int r = pthread_create( ret, 0, routine, parameter ); 375 | if( r ) 376 | { 377 | free( ret ); 378 | return 0; 379 | } 380 | return (og_thread_t)ret; 381 | } 382 | 383 | OSG_PREFIX void * OGJoinThread( og_thread_t ot ) 384 | { 385 | void * retval; 386 | if( !ot ) 387 | { 388 | return 0; 389 | } 390 | pthread_join( *(pthread_t*)ot, &retval ); 391 | OSG_TERM_THREAD_CODE 392 | free( ot ); 393 | return retval; 394 | } 395 | 396 | OSG_PREFIX void OGCancelThread( og_thread_t ot ) 397 | { 398 | if( !ot ) 399 | { 400 | return; 401 | } 402 | #ifdef ANDROID 403 | pthread_kill( *(pthread_t*)ot, SIGTERM ); 404 | #else 405 | pthread_cancel( *(pthread_t*)ot ); 406 | #endif 407 | OSG_TERM_THREAD_CODE 408 | free( ot ); 409 | } 410 | 411 | OSG_PREFIX og_mutex_t OGCreateMutex() 412 | { 413 | pthread_mutexattr_t mta; 414 | og_mutex_t r = malloc( sizeof( pthread_mutex_t ) ); 415 | if( !r ) return 0; 416 | 417 | pthread_mutexattr_init(&mta); 418 | pthread_mutexattr_settype(&mta, PTHREAD_MUTEX_RECURSIVE); 419 | 420 | pthread_mutex_init( (pthread_mutex_t *)r, &mta ); 421 | 422 | return r; 423 | } 424 | 425 | OSG_PREFIX void OGLockMutex( og_mutex_t om ) 426 | { 427 | if( !om ) 428 | { 429 | return; 430 | } 431 | pthread_mutex_lock( (pthread_mutex_t*)om ); 432 | } 433 | 434 | OSG_PREFIX void OGUnlockMutex( og_mutex_t om ) 435 | { 436 | if( !om ) 437 | { 438 | return; 439 | } 440 | pthread_mutex_unlock( (pthread_mutex_t*)om ); 441 | } 442 | 443 | OSG_PREFIX void OGDeleteMutex( og_mutex_t om ) 444 | { 445 | if( !om ) 446 | { 447 | return; 448 | } 449 | 450 | pthread_mutex_destroy( (pthread_mutex_t*)om ); 451 | free( om ); 452 | } 453 | 454 | 455 | 456 | 457 | OSG_PREFIX og_sema_t OGCreateSema() 458 | { 459 | sem_t * sem = (sem_t *)malloc( sizeof( sem_t ) ); 460 | if( !sem ) return 0; 461 | sem_init( sem, 0, 0 ); 462 | return (og_sema_t)sem; 463 | } 464 | 465 | OSG_PREFIX int OGGetSema( og_sema_t os ) 466 | { 467 | int valp; 468 | sem_getvalue( (sem_t*)os, &valp ); 469 | return valp; 470 | } 471 | 472 | 473 | OSG_PREFIX void OGLockSema( og_sema_t os ) 474 | { 475 | sem_wait( (sem_t*)os ); 476 | } 477 | 478 | OSG_PREFIX void OGUnlockSema( og_sema_t os ) 479 | { 480 | sem_post( (sem_t*)os ); 481 | } 482 | 483 | OSG_PREFIX void OGDeleteSema( og_sema_t os ) 484 | { 485 | sem_destroy( (sem_t*)os ); 486 | free(os); 487 | } 488 | 489 | OSG_PREFIX og_tls_t OGCreateTLS() 490 | { 491 | pthread_key_t ret = 0; 492 | pthread_key_create(&ret, 0); 493 | return (og_tls_t)(intptr_t)ret; 494 | } 495 | 496 | OSG_PREFIX void OGDeleteTLS( og_tls_t key ) 497 | { 498 | pthread_key_delete( (pthread_key_t)(intptr_t)key ); 499 | } 500 | 501 | OSG_PREFIX void * OGGetTLS( og_tls_t key ) 502 | { 503 | return pthread_getspecific( (pthread_key_t)(intptr_t)key ); 504 | } 505 | 506 | OSG_PREFIX void OGSetTLS( og_tls_t key, void * data ) 507 | { 508 | pthread_setspecific( (pthread_key_t)(intptr_t)key, data ); 509 | } 510 | 511 | #endif 512 | 513 | #ifdef __cplusplus 514 | }; 515 | #endif 516 | 517 | #endif //OSG_NO_IMPLEMENTATION 518 | 519 | #endif //_OS_GENERIC_H 520 | 521 | 522 | -------------------------------------------------------------------------------- /spacestations.txt: -------------------------------------------------------------------------------- 1 | ISS (ZARYA) 2 | 1 25544U 98067A 24118.69784154 .00029521 00000+0 51116-3 0 9995 3 | 2 25544 51.6397 205.7509 0003603 115.2045 341.2688 15.50662375450710 4 | CSS (TIANHE) 5 | 1 48274U 21035A 24118.44050188 .00054863 00000+0 59183-3 0 9996 6 | 2 48274 41.4693 33.0831 0004332 276.0244 84.0102 15.62776353171154 7 | ISS (NAUKA) 8 | 1 49044U 21066A 24118.48725720 .00026346 00000+0 45746-3 0 9996 9 | 2 49044 51.6370 206.7977 0003687 115.0868 245.0504 15.50646640156623 10 | FREGAT DEB 11 | 1 49271U 11037PF 24118.30337627 .00023882 00000+0 58842-1 0 9992 12 | 2 49271 51.6570 133.6816 0957886 13.8031 348.7151 12.13798424129019 13 | CSS (WENTIAN) 14 | 1 53239U 22085A 24118.44050188 .00054863 00000+0 59183-3 0 9999 15 | 2 53239 41.4693 33.0831 0004332 276.0244 84.0102 15.62776353100232 16 | CSS (MENGTIAN) 17 | 1 54216U 22143A 24118.44050188 .00054863 00000+0 59183-3 0 9990 18 | 2 54216 41.4693 33.0831 0004332 276.0244 84.0102 15.62776353 84712 19 | ISS DEB [SPX-28 IPA FSE] 20 | 1 57212U 98067VP 24118.53352302 .00321704 51531-4 83372-3 0 9998 21 | 2 57212 51.6219 173.1418 0006705 6.6182 353.4913 15.94523914 48079 22 | SHENZHOU-17 (SZ-17) 23 | 1 58146U 23164A 24118.44050188 .00054863 00000+0 59183-3 0 9990 24 | 2 58146 41.4693 33.0831 0004332 276.0244 84.0102 15.62776353 28503 25 | ISS DEB 26 | 1 58229U 98067WC 24118.48772975 .00194554 00000+0 11931-2 0 9995 27 | 2 58229 51.6294 193.2879 0006383 355.4605 4.6336 15.76141218 27649 28 | PROGRESS-MS 25 29 | 1 58460U 23184A 24118.48725720 .00026346 00000+0 45746-3 0 9991 30 | 2 58460 51.6370 206.7977 0003687 115.0868 245.0504 15.50646640 22920 31 | BEAK 32 | 1 58612U 98067WD 24118.47884925 .00195596 00000+0 13546-2 0 9990 33 | 2 58612 51.6318 196.9984 0005518 0.6010 359.4994 15.73339441 20544 34 | TIANZHOU-7 35 | 1 58811U 24013A 24118.44050188 .00054863 00000+0 59183-3 0 9993 36 | 2 58811 41.4693 33.0831 0004332 276.0244 84.0102 15.62776353170803 37 | CYGNUS NG-20 38 | 1 58898U 24021A 24118.48725720 .00026346 00000+0 45746-3 0 9997 39 | 2 58898 51.6370 206.7977 0003687 115.0868 245.0504 15.50646640 13475 40 | PROGRESS-MS 26 41 | 1 58961U 24029A 24118.48725720 .00026346 00000+0 45746-3 0 9996 42 | 2 58961 51.6370 206.7977 0003687 115.0868 245.0504 15.50646640450224 43 | CREW DRAGON 8 44 | 1 59097U 24042A 24118.48725720 .00026346 00000+0 45746-3 0 9992 45 | 2 59097 51.6370 206.7977 0003687 115.0868 245.0504 15.50646640450225 46 | DRAGON CRS-30 47 | 1 59287U 24054A 24118.48725720 .00026346 00000+0 45746-3 0 9996 48 | 2 59287 51.6370 206.7977 0003687 115.0868 245.0504 15.50646640450530 49 | SOYUZ-MS 25 50 | 1 59294U 24055A 24118.48725720 .00026346 00000+0 45746-3 0 9995 51 | 2 59294 51.6370 206.7977 0003687 115.0868 245.0504 15.50646640450538 52 | MICROORBITER-1 53 | 1 59483U 98067WF 24118.53167835 .00102883 00000+0 15298-2 0 9991 54 | 2 59483 51.6358 206.3444 0002491 77.5632 282.5637 15.54333343 2579 55 | CURTIS 56 | 1 59507U 98067WG 24118.53348947 .00078633 00000+0 12002-2 0 9998 57 | 2 59507 51.6352 206.3565 0001449 95.2299 264.8857 15.53739606 2581 58 | KASHIWA 59 | 1 59508U 98067WH 24118.59544911 .00113235 00000+0 16685-2 0 9993 60 | 2 59508 51.6354 206.0202 0002396 76.8225 283.3033 15.54534924 2595 61 | 1998-067WJ 62 | 1 59559U 98067WJ 24118.54358973 .00076906 00000+0 12093-2 0 9997 63 | 2 59559 51.6367 206.4250 0004268 39.1757 320.9541 15.52919902 1451 64 | 1998-067WK 65 | 1 59560U 98067WK 24118.54136723 .00126062 00000+0 19030-2 0 9993 66 | 2 59560 51.6362 206.4104 0006432 49.8498 310.3055 15.53797558 1388 67 | 1998-067WL 68 | 1 59561U 98067WL 24118.54704160 .00041801 00000+0 68793-3 0 9998 69 | 2 59561 51.6374 206.4470 0002554 71.5589 288.5678 15.51922376 1420 70 | 1998-067WM 71 | 1 59562U 98067WM 24118.54466293 .00080657 00000+0 12740-2 0 9993 72 | 2 59562 51.6367 206.4313 0002559 92.8411 267.2872 15.52779284 1434 73 | 1998-067WN 74 | 1 59563U 98067WN 24118.54566586 .00075882 00000+0 12120-2 0 9996 75 | 2 59563 51.6371 206.4390 0002964 100.1163 260.0161 15.52496084 1427 76 | SHENZHOU-18 (SZ-18) 77 | 1 59591U 24078A 24117.16233996 .00050859 00000+0 55240-3 0 9999 78 | 2 59591 41.4690 40.8827 0004294 269.1016 90.9332 15.62627011170957 79 | CZ-2F R/B 80 | 1 59592U 24078B 24118.77731622 .01097656 76004-5 71799-3 0 9994 81 | 2 59592 41.4685 30.2051 0094454 142.3853 273.8599 16.06676466 363 82 | CZ-2F DEB 83 | 1 59593U 24078C 24117.16437429 .06643541 69424-5 71055-2 0 9992 84 | 2 59593 41.6118 40.5394 0251395 137.5527 224.6445 15.69435837 103 85 | CZ-2F DEB 86 | 1 59594U 24078D 24117.48013544 .08483376 68819-5 73850-2 0 9998 87 | 2 59594 41.5696 38.6918 0223885 143.4243 218.4447 15.77673603 127 88 | CZ-2F DEB 89 | 1 59595U 24078E 24117.85336774 .13837883 68135-5 88539-2 0 9996 90 | 2 59595 41.2954 36.8973 0161439 124.3929 237.3200 15.94157667 211 91 | 1998-067WP 92 | 1 59596U 98067WP 24118.54263951 .00091150 00000+0 14127-2 0 9997 93 | 2 59596 51.6367 206.4189 0004442 37.8849 322.2453 15.53243176 1466 94 | 1998-067WQ 95 | 1 59597U 98067WQ 24118.54261632 .00092663 00000+0 14349-2 0 9998 96 | 2 59597 51.6373 206.4189 0004469 37.7426 322.3877 15.53259561 1436 97 | 98 | -------------------------------------------------------------------------------- /trackonly.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define CSGP4_USE_FLOAT 0 4 | #define CSGP4_INIT 0 5 | 6 | #include "csgp4.h" 7 | 8 | int main( int argc, char ** argv ) 9 | { 10 | double ro[3]; 11 | double vo[3]; 12 | struct elsetrec iss; 13 | double startmfe = (1714240800 - /* iss.epoch */ 1000000)/60.0; 14 | sgp4 (&iss, startmfe, ro, vo); 15 | return 0; 16 | } 17 | --------------------------------------------------------------------------------