├── .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 |
--------------------------------------------------------------------------------