1 /* this file contains routines to support Earth satellites.
2 *
3 * Orbit propagation is based on the NORAD SGP4/SDP4 code, as converted from
4 * the original FORTRAN to C by Magnus Backstrom. The paper "Spacetrack
5 * Report Number 3: Models for Propagation of NORAD Element Sets" describes
6 * the calculations.
7 * See http://www.celestrak.com/NORAD/documentation/spacetrk.pdf.
8 *
9 * A few topocentric routines are also used from the 'orbit' program which is
10 * Copyright (c) 1986,1987,1988,1989,1990 Robert W. Berger N3EMO, who has
11 * granted permission for it to be licensed under the same terms as those
12 * of the PyEphem package in which this source file is included.
13 */
14
15 /* define this to use orbit's propagator
16 #define USE_ORBIT_PROPAGATOR
17 */
18
19 /* define this to print some stuff
20 #define ESAT_TRACE
21 */
22
23 #include <stdio.h>
24 #include <math.h>
25 #include <string.h>
26 #include <stdlib.h>
27
28 #include "astro.h"
29 #include "preferences.h"
30
31 #include "vector.h"
32 #include "sattypes.h"
33 #include "satlib.h"
34
35 #if defined(_MSC_VER) && (_MSC_VER < 1800)
36 #define isnan(x) _isnan(x)
37 #endif
38
39 #define ESAT_MAG 2 /* fake satellite magnitude */
40
41 typedef double MAT3x3[3][3];
42
43 static int crazyOp (Now *np, Obj *op);
44 static void esat_prop (Now *np, Obj *op, double *SatX, double *SatY, double
45 *SatZ, double *SatVX, double *SatVY, double *SatVZ);
46 static void GetSatelliteParams (Obj *op);
47 static void GetSiteParams (Now *np);
48 static double Kepler (double MeanAnomaly, double Eccentricity);
49 static void GetSubSatPoint (double SatX, double SatY, double SatZ,
50 double T, double *Latitude, double *Longitude, double *Height);
51 static void GetSatPosition (double EpochTime, double EpochRAAN,
52 double EpochArgPerigee, double SemiMajorAxis, double Inclination,
53 double Eccentricity, double RAANPrecession, double PerigeePrecession,
54 double T, double TrueAnomaly, double *X, double *Y, double *Z,
55 double *Radius, double *VX, double *VY, double *VZ);
56 static void GetSitPosition (double SiteLat, double SiteLong,
57 double SiteElevation, double CrntTime, double *SiteX, double *SiteY,
58 double *SiteZ, double *SiteVX, double *SiteVY, MAT3x3 SiteMatrix);
59 static void GetRange (double SiteX, double SiteY, double SiteZ,
60 double SiteVX, double SiteVY, double SatX, double SatY, double SatZ,
61 double SatVX, double SatVY, double SatVZ, double *Range,
62 double *RangeRate);
63 static void GetTopocentric (double SatX, double SatY, double SatZ,
64 double SiteX, double SiteY, double SiteZ, MAT3x3 SiteMatrix, double *X,
65 double *Y, double *Z);
66 static void GetBearings (double SatX, double SatY, double SatZ,
67 double SiteX, double SiteY, double SiteZ, MAT3x3 SiteMatrix,
68 double *Azimuth, double *Elevation);
69 static int Eclipsed (double SatX, double SatY, double SatZ,
70 double SatRadius, double CrntTime);
71 static void InitOrbitRoutines (double EpochDay, int AtEod);
72
73 #ifdef USE_ORBIT_PROPAGATOR
74 static void GetPrecession (double SemiMajorAxis, double Eccentricity,
75 double Inclination, double *RAANPrecession, double *PerigeePrecession);
76 #endif /* USE_ORBIT_PROPAGATOR */
77
78 /* stuff from orbit */
79 /* char VersionStr[] = "N3EMO Orbit Simulator v3.9"; */
80
81 #ifdef PI2
82 #undef PI2
83 #endif
84
85 #define PI2 (PI*2)
86
87 #define MinutesPerDay (24*60.0)
88 #define SecondsPerDay (60*MinutesPerDay)
89 #define HalfSecond (0.5/SecondsPerDay)
90 #define EarthRadius 6378.16 /* Kilometers */
91 #define C 2.997925e5 /* Kilometers/Second */
92 #define RadiansPerDegree (PI/180)
93 #define ABS(x) ((x) < 0 ? (-(x)) : (x))
94 #define SQR(x) ((x)*(x))
95
96 #define EarthFlat (1/298.25) /* Earth Flattening Coeff. */
97 #define SiderealSolar 1.0027379093
98 #define SidRate (PI2*SiderealSolar/SecondsPerDay) /* radians/second */
99 #define GM 398600 /* Kilometers^3/seconds^2 */
100
101 #define Epsilon (RadiansPerDegree/3600) /* 1 arc second */
102 #define SunRadius 695000
103 #define SunSemiMajorAxis 149598845.0 /* Kilometers */
104
105 /* Keplerian Elements and misc. data for the satellite */
106 static double EpochDay; /* time of epoch */
107 static double EpochMeanAnomaly; /* Mean Anomaly at epoch */
108 static long EpochOrbitNum; /* Integer orbit # of epoch */
109 static double EpochRAAN; /* RAAN at epoch */
110 static double epochMeanMotion; /* Revolutions/day */
111 static double OrbitalDecay; /* Revolutions/day^2 */
112 static double EpochArgPerigee; /* argument of perigee at epoch */
113 static double Eccentricity;
114 static double Inclination;
115
116 /* Site Parameters */
117 static double SiteLat,SiteLong,SiteAltitude;
118
119
120 static double SidDay,SidReference; /* Date and sidereal time */
121
122 /* Keplerian elements for the sun */
123 static double SunEpochTime,SunInclination,SunRAAN,SunEccentricity,
124 SunArgPerigee,SunMeanAnomaly,SunMeanMotion;
125
126 /* values for shadow geometry */
127 static double SinPenumbra,CosPenumbra;
128
129
130 /* given a Now and an Obj with info about an earth satellite in the es_* fields
131 * fill in the s_* sky fields describing the satellite.
132 * as usual, we compute the geocentric ra/dec precessed to np->n_epoch and
133 * compute topocentric altitude accounting for refraction.
134 * return 0 if all ok, else -1.
135 */
136 int
obj_earthsat(Now * np,Obj * op)137 obj_earthsat (Now *np, Obj *op)
138 {
139 double Radius; /* From geocenter */
140 double SatX,SatY,SatZ; /* In Right Ascension based system */
141 double SatVX,SatVY,SatVZ; /* Kilometers/second */
142 double SiteX,SiteY,SiteZ;
143 double SiteVX,SiteVY;
144 double SiteMatrix[3][3];
145 double Height;
146 double SSPLat,SSPLong;
147 double Azimuth,Elevation,Range;
148 double RangeRate;
149 double dtmp;
150 double CrntTime;
151 double ra, dec;
152
153 #ifdef ESAT_TRACE
154 printf ("\n");
155 printf ("Name = %s\n", op->o_name);
156 printf ("current jd = %13.5f\n", mjd+MJD0);
157 printf ("current mjd = %g\n", mjd);
158 printf ("satellite jd = %13.5f\n", op->es_epoch+MJD0);
159 printf ("satellite mjd = %g\n", op->es_epoch);
160 #endif /* ESAT_TRACE */
161
162 /* xephem uses noon 12/31/1899 as 0; orbit uses midnight 1/1/1900.
163 * thus, xephem runs 12 hours, or 1/2 day, behind of what orbit wants.
164 */
165 CrntTime = mjd + 0.5;
166
167 /* extract the XEphem data forms into those used by orbit.
168 * (we still use some functions and names from orbit, thank you).
169 */
170 InitOrbitRoutines(CrntTime, 1);
171 GetSatelliteParams(op);
172 GetSiteParams(np);
173
174 /* propagate to np->n_mjd */
175 esat_prop (np, op, &SatX, &SatY, &SatZ, &SatVX, &SatVY, &SatVZ);
176 if (isnan(SatX))
177 return -1;
178 Radius = sqrt (SatX*SatX + SatY*SatY + SatZ*SatZ);
179
180 /* find geocentric EOD equatorial directly from xyz vector */
181 dtmp = atan2 (SatY, SatX);
182 range (&dtmp, 2*PI);
183 op->s_gaera = dtmp;
184 op->s_gaedec = atan2 (SatZ, sqrt(SatX*SatX + SatY*SatY));
185
186 /* find topocentric from site location */
187 GetSitPosition(SiteLat,SiteLong,SiteAltitude,CrntTime,
188 &SiteX,&SiteY,&SiteZ,&SiteVX,&SiteVY,SiteMatrix);
189 GetBearings(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,
190 &Azimuth,&Elevation);
191
192 op->s_az = Azimuth;
193 refract (pressure, temp, Elevation, &dtmp);
194 op->s_alt = dtmp;
195
196 /* Range: line-of-site distance to satellite, m
197 * RangeRate: m/s
198 */
199 GetRange(SiteX,SiteY,SiteZ,SiteVX,SiteVY,
200 SatX,SatY,SatZ,SatVX,SatVY,SatVZ,&Range,&RangeRate);
201
202 op->s_range = (float)(Range*1000); /* we want m */
203 op->s_rangev = (float)(RangeRate*1000); /* we want m/s */
204
205 /* SSPLat: sub-satellite latitude, rads
206 * SSPLong: sub-satellite longitude, >0 west, rads
207 * Height: height of satellite above ground, m
208 */
209 GetSubSatPoint(SatX,SatY,SatZ,CrntTime,
210 &SSPLat,&SSPLong,&Height);
211
212 op->s_elev = (float)(Height*1000); /* we want m */
213 op->s_sublat = (float)SSPLat;
214 op->s_sublng = (float)(-SSPLong); /* we want +E */
215
216 op->s_eclipsed = Eclipsed(SatX,SatY,SatZ,Radius,CrntTime);
217
218 #ifdef ESAT_TRACE
219 printf ("CrntTime = %g\n", CrntTime);
220 printf ("SatX = %g\n", SatX);
221 printf ("SatY = %g\n", SatY);
222 printf ("SatZ = %g\n", SatZ);
223 printf ("Radius = %g\n", Radius);
224 printf ("SatVX = %g\n", SatVX);
225 printf ("SatVY = %g\n", SatVY);
226 printf ("SatVZ = %g\n", SatVZ);
227 printf ("SiteX = %g\n", SiteX);
228 printf ("SiteY = %g\n", SiteY);
229 printf ("SiteZ = %g\n", SiteZ);
230 printf ("SiteVX = %g\n", SiteVX);
231 printf ("SiteVY = %g\n", SiteVY);
232 printf ("Height = %g\n", Height);
233 printf ("SSPLat = %g\n", SSPLat);
234 printf ("SSPLong = %g\n", SSPLong);
235 printf ("Azimuth = %g\n", Azimuth);
236 printf ("Elevation = %g\n", Elevation);
237 printf ("Range = %g\n", Range);
238 printf ("RangeRate = %g\n", RangeRate);
239 fflush (stdout);
240 #endif /* ESAT_TRACE */
241
242 /* find s_ra/dec, depending on current options. */
243 if (pref_get(PREF_EQUATORIAL) == PREF_TOPO) {
244 double ha, lst;
245 aa_hadec (lat, Elevation, (double)op->s_az, &ha, &dec);
246 now_lst (np, &lst);
247 ra = hrrad(lst) - ha;
248 range (&ra, 2*PI);
249 op->s_ha = ha;
250 } else {
251 ra = op->s_gaera;
252 dec = op->s_gaedec;
253 }
254 op->s_ra = ra;
255 op->s_dec = dec;
256 if (epoch != EOD && mjd != epoch)
257 precess (mjd, epoch, &ra, &dec);
258 op->s_astrora = ra;
259 op->s_astrodec = dec;
260
261 /* just make up a size and brightness */
262 set_smag (op, ESAT_MAG);
263 op->s_size = (float)0;
264
265 return (0);
266 }
267
268 /* find position and velocity vector for given Obj at the given time.
269 * set USE_ORBIT_PROPAGATOR depending on desired propagator to use.
270 */
271 static void
esat_prop(Now * np,Obj * op,double * SatX,double * SatY,double * SatZ,double * SatVX,double * SatVY,double * SatVZ)272 esat_prop (Now *np, Obj *op, double *SatX, double *SatY, double *SatZ,
273 double *SatVX, double *SatVY, double *SatVZ)
274 {
275 #ifdef USE_ORBIT_PROPAGATOR
276 double ReferenceOrbit; /* Floating point orbit # at epoch */
277 double CurrentOrbit;
278 long OrbitNum;
279 double RAANPrecession,PerigeePrecession;
280 double MeanAnomaly,TrueAnomaly;
281 double SemiMajorAxis;
282 double AverageMotion, /* Corrected for drag */
283 CurrentMotion;
284 double Radius;
285 double CrntTime;
286
287 if (crazyOp (np, op)) {
288 *SatX = *SatY = *SatZ = *SatVX = *SatVY = *SatVZ = 0;
289 return;
290 }
291
292 SemiMajorAxis = 331.25 * exp(2*log(MinutesPerDay/epochMeanMotion)/3);
293 GetPrecession(SemiMajorAxis,Eccentricity,Inclination,&RAANPrecession,
294 &PerigeePrecession);
295
296 ReferenceOrbit = EpochMeanAnomaly/PI2 + EpochOrbitNum;
297
298 CrntTime = mjd + 0.5;
299 AverageMotion = epochMeanMotion + (CrntTime-EpochDay)*OrbitalDecay/2;
300 CurrentMotion = epochMeanMotion + (CrntTime-EpochDay)*OrbitalDecay;
301
302 SemiMajorAxis = 331.25 * exp(2*log(MinutesPerDay/CurrentMotion)/3);
303
304 CurrentOrbit = ReferenceOrbit + (CrntTime-EpochDay)*AverageMotion;
305
306 OrbitNum = CurrentOrbit;
307
308 MeanAnomaly = (CurrentOrbit-OrbitNum)*PI2;
309
310 TrueAnomaly = Kepler(MeanAnomaly,Eccentricity);
311
312 GetSatPosition(EpochDay,EpochRAAN,EpochArgPerigee,SemiMajorAxis,
313 Inclination,Eccentricity,RAANPrecession,PerigeePrecession,
314 CrntTime,TrueAnomaly,SatX,SatY,SatZ,&Radius,SatVX,SatVY,SatVZ);
315
316 #ifdef ESAT_TRACE
317 printf ("O Radius = %g\n", Radius);
318 printf ("ReferenceOrbit = %g\n", ReferenceOrbit);
319 printf ("CurrentOrbit = %g\n", CurrentOrbit);
320 printf ("RAANPrecession = %g\n", RAANPrecession);
321 printf ("PerigeePrecession = %g\n", PerigeePrecession);
322 printf ("MeanAnomaly = %g\n", MeanAnomaly);
323 printf ("TrueAnomaly = %g\n", TrueAnomaly);
324 printf ("SemiMajorAxis = %g\n", SemiMajorAxis);
325 printf ("AverageMotion = %g\n", AverageMotion);
326 printf ("CurrentMotion = %g\n", CurrentMotion);
327 #endif /* ESAT_TRACE */
328
329 #else /* ! USE_ORBIT_PROPAGATOR */
330 #define MPD 1440.0 /* minutes per day */
331
332 SatElem se;
333 SatData sd;
334 Vec3 posvec, velvec;
335 double dy;
336 double dt;
337 int yr;
338
339 if (crazyOp (np, op)) {
340 *SatX = *SatY = *SatZ = *SatVX = *SatVY = *SatVZ = 0;
341 return;
342 }
343
344 /* init */
345 memset ((void *)&se, 0, sizeof(se));
346 memset ((void *)&sd, 0, sizeof(sd));
347 sd.elem = &se;
348
349 /* se_EPOCH is packed as yr*1000 + dy, where yr is years since 1900
350 * and dy is day of year, Jan 1 being 1
351 */
352 mjd_dayno (op->es_epoch, &yr, &dy);
353 yr -= 1900;
354 dy += 1;
355 se.se_EPOCH = yr*1000 + dy;
356
357 /* others carry over with some change in units */
358 se.se_XNO = op->es_n * (2*PI/MPD); /* revs/day to rads/min */
359 se.se_XINCL = (float)degrad(op->es_inc);
360 se.se_XNODEO = (float)degrad(op->es_raan);
361 se.se_EO = op->es_e;
362 se.se_OMEGAO = (float)degrad(op->es_ap);
363 se.se_XMO = (float)degrad(op->es_M);
364 se.se_BSTAR = op->es_drag;
365 se.se_XNDT20 = op->es_decay*(2*PI/MPD/MPD); /*rv/dy^^2 to rad/min^^2*/
366
367 se.se_id.orbit = op->es_orbit;
368
369 dt = (mjd-op->es_epoch)*MPD;
370
371 #ifdef ESAT_TRACE
372 printf ("se_EPOCH : %30.20f\n", se.se_EPOCH);
373 printf ("se_XNO : %30.20f\n", se.se_XNO);
374 printf ("se_XINCL : %30.20f\n", se.se_XINCL);
375 printf ("se_XNODEO : %30.20f\n", se.se_XNODEO);
376 printf ("se_EO : %30.20f\n", se.se_EO);
377 printf ("se_OMEGAO : %30.20f\n", se.se_OMEGAO);
378 printf ("se_XMO : %30.20f\n", se.se_XMO);
379 printf ("se_BSTAR : %30.20f\n", se.se_BSTAR);
380 printf ("se_XNDT20 : %30.20f\n", se.se_XNDT20);
381 printf ("se_orbit : %30d\n", se.se_id.orbit);
382 printf ("dt : %30.20f\n", dt);
383 #endif /* ESAT_TRACE */
384
385 /* compute the state vectors */
386 if (se.se_XNO >= (1.0/225.0))
387 sgp4(&sd, &posvec, &velvec, dt); /* NEO */
388 else
389 sdp4(&sd, &posvec, &velvec, dt); /* GEO */
390 if (sd.prop.sgp4)
391 free (sd.prop.sgp4); /* sd.prop.sdp4 is in same union */
392 if (sd.deep)
393 free (sd.deep);
394
395 /* earth radii to km */
396 *SatX = (ERAD/1000)*posvec.x;
397 *SatY = (ERAD/1000)*posvec.y;
398 *SatZ = (ERAD/1000)*posvec.z;
399 /* Minutes per day/Seconds by day = Minutes/Second = 1/60 */
400 *SatVX = (ERAD*velvec.x)/(1000*60);
401 *SatVY =(ERAD*velvec.y)/(1000*60);
402 *SatVZ = (ERAD*velvec.z)/(1000*60);
403
404 #endif
405 }
406
407 /* return 1 if op is crazy @ np */
408 static int
crazyOp(Now * np,Obj * op)409 crazyOp (Now *np, Obj *op)
410 {
411 /* toss if more than a year old */
412 return (fabs(op->es_epoch - mjd) > 365);
413 }
414
415 /* grab the xephem stuff from op and copy into orbit's globals.
416 */
417 static void
GetSatelliteParams(Obj * op)418 GetSatelliteParams(Obj *op)
419 {
420 /* the following are for the orbit functions */
421 /* xephem uses noon 12/31/1899 as 0; orbit uses midnight 1/1/1900 as 1.
422 * thus, xephem runs 12 hours, or 1/2 day, behind of what orbit wants.
423 */
424 EpochDay = op->es_epoch + 0.5;
425
426 /* xephem stores inc in degrees; orbit wants rads */
427 Inclination = degrad(op->es_inc);
428
429 /* xephem stores RAAN in degrees; orbit wants rads */
430 EpochRAAN = degrad(op->es_raan);
431
432 Eccentricity = op->es_e;
433
434 /* xephem stores arg of perigee in degrees; orbit wants rads */
435 EpochArgPerigee = degrad(op->es_ap);
436
437 /* xephem stores mean anomaly in degrees; orbit wants rads */
438 EpochMeanAnomaly = degrad (op->es_M);
439
440 epochMeanMotion = op->es_n;
441
442 OrbitalDecay = op->es_decay;
443
444 EpochOrbitNum = op->es_orbit;
445 }
446
447
448
449 static void
GetSiteParams(Now * np)450 GetSiteParams(Now *np)
451 {
452 SiteLat = lat;
453
454 /* xephem stores longitude as >0 east; orbit wants >0 west */
455 SiteLong = 2.0*PI - lng;
456
457 /* what orbit calls altitude xephem calls elevation and stores it from
458 * sea level in earth radii; orbit wants km
459 */
460 SiteAltitude = elev*ERAD/1000.0;
461
462 /* we don't implement a minimum horizon altitude cutoff
463 SiteMinElev = 0;
464 */
465
466 #ifdef ESAT_TRACE
467 printf ("SiteLat = %g\n", SiteLat);
468 printf ("SiteLong = %g\n", SiteLong);
469 printf ("SiteAltitude = %g\n", SiteAltitude);
470 fflush (stdout);
471 #endif
472 }
473
474
475 /* Solve Kepler's equation */
476 /* Inputs: */
477 /* MeanAnomaly Time Since last perigee, in radians. */
478 /* PI2 = one complete orbit. */
479 /* Eccentricity Eccentricity of orbit's ellipse. */
480 /* Output: */
481 /* TrueAnomaly Angle between perigee, geocenter, and */
482 /* current position. */
483
484 static
Kepler(double MeanAnomaly,double Eccentricity)485 double Kepler(double MeanAnomaly, double Eccentricity)
486 {
487 register double E; /* Eccentric Anomaly */
488 register double Error;
489 register double TrueAnomaly;
490
491 E = MeanAnomaly ;/*+ Eccentricity*sin(MeanAnomaly); -- Initial guess */
492 do
493 {
494 Error = (E - Eccentricity*sin(E) - MeanAnomaly)
495 / (1 - Eccentricity*cos(E));
496 E -= Error;
497 }
498 while (ABS(Error) >= Epsilon);
499
500 if (ABS(E-PI) < Epsilon)
501 TrueAnomaly = PI;
502 else
503 TrueAnomaly = 2*atan(sqrt((1+Eccentricity)/(1-Eccentricity))
504 *tan(E/2));
505 if (TrueAnomaly < 0)
506 TrueAnomaly += PI2;
507
508 return TrueAnomaly;
509 }
510
511 static void
GetSubSatPoint(double SatX,double SatY,double SatZ,double T,double * Latitude,double * Longitude,double * Height)512 GetSubSatPoint(double SatX, double SatY, double SatZ, double T,
513 double *Latitude, double *Longitude, double *Height)
514 {
515 double r;
516 /* ECD: long i; */
517
518 r = sqrt(SQR(SatX) + SQR(SatY) + SQR(SatZ));
519
520 *Longitude = PI2*((T-SidDay)*SiderealSolar + SidReference)
521 - atan2(SatY,SatX);
522
523 /* ECD:
524 * want Longitude in range -PI to PI , +W
525 */
526 range (Longitude, 2*PI);
527 if (*Longitude > PI)
528 *Longitude -= 2*PI;
529
530 *Latitude = atan(SatZ/sqrt(SQR(SatX) + SQR(SatY)));
531
532 #define SSPELLIPSE
533 #ifdef SSPELLIPSE
534 /* ECD */
535 *Height = r - EarthRadius*(sqrt(1-(2*EarthFlat-SQR(EarthFlat))*SQR(sin(*Latitude))));
536 #else
537 *Height = r - EarthRadius;
538 #endif
539 }
540
541
542 #ifdef USE_ORBIT_PROPAGATOR
543 static void
GetPrecession(double SemiMajorAxis,double Eccentricity,double Inclination,double * RAANPrecession,double * PerigeePrecession)544 GetPrecession(double SemiMajorAxis, double Eccentricity, double Inclination,
545 double *RAANPrecession, double *PerigeePrecession)
546 {
547 *RAANPrecession = 9.95*pow(EarthRadius/SemiMajorAxis,3.5) * cos(Inclination)
548 / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
549
550 *PerigeePrecession = 4.97*pow(EarthRadius/SemiMajorAxis,3.5)
551 * (5*SQR(cos(Inclination))-1)
552 / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
553 }
554 #endif /* USE_ORBIT_PROPAGATOR */
555
556 /* Compute the satellite postion and velocity in the RA based coordinate
557 * system.
558 * ECD: take care not to let Radius get below EarthRadius.
559 */
560
561 static void
GetSatPosition(double EpochTime,double EpochRAAN,double EpochArgPerigee,double SemiMajorAxis,double Inclination,double Eccentricity,double RAANPrecession,double PerigeePrecession,double T,double TrueAnomaly,double * X,double * Y,double * Z,double * Radius,double * VX,double * VY,double * VZ)562 GetSatPosition(double EpochTime, double EpochRAAN, double EpochArgPerigee,
563 double SemiMajorAxis, double Inclination, double Eccentricity,
564 double RAANPrecession, double PerigeePrecession, double T,
565 double TrueAnomaly, double *X, double *Y, double *Z, double *Radius,
566 double *VX, double *VY, double *VZ)
567
568 {
569 double RAAN,ArgPerigee;
570
571
572 double Xw,Yw,VXw,VYw; /* In orbital plane */
573 double Tmp;
574 double Px,Qx,Py,Qy,Pz,Qz; /* Escobal transformation 31 */
575 double CosArgPerigee,SinArgPerigee;
576 double CosRAAN,SinRAAN,CoSinclination,SinInclination;
577
578 *Radius = SemiMajorAxis*(1-SQR(Eccentricity))
579 / (1+Eccentricity*cos(TrueAnomaly));
580
581 if (*Radius <= EarthRadius)
582 *Radius = EarthRadius;
583
584
585 Xw = *Radius * cos(TrueAnomaly);
586 Yw = *Radius * sin(TrueAnomaly);
587
588 Tmp = sqrt(GM/(SemiMajorAxis*(1-SQR(Eccentricity))));
589
590 VXw = -Tmp*sin(TrueAnomaly);
591 VYw = Tmp*(cos(TrueAnomaly) + Eccentricity);
592
593 ArgPerigee = EpochArgPerigee + (T-EpochTime)*PerigeePrecession;
594 RAAN = EpochRAAN - (T-EpochTime)*RAANPrecession;
595
596 CosRAAN = cos(RAAN); SinRAAN = sin(RAAN);
597 CosArgPerigee = cos(ArgPerigee); SinArgPerigee = sin(ArgPerigee);
598 CoSinclination = cos(Inclination); SinInclination = sin(Inclination);
599
600 Px = CosArgPerigee*CosRAAN - SinArgPerigee*SinRAAN*CoSinclination;
601 Py = CosArgPerigee*SinRAAN + SinArgPerigee*CosRAAN*CoSinclination;
602 Pz = SinArgPerigee*SinInclination;
603 Qx = -SinArgPerigee*CosRAAN - CosArgPerigee*SinRAAN*CoSinclination;
604 Qy = -SinArgPerigee*SinRAAN + CosArgPerigee*CosRAAN*CoSinclination;
605 Qz = CosArgPerigee*SinInclination;
606
607 *X = Px*Xw + Qx*Yw; /* Escobal, transformation #31 */
608 *Y = Py*Xw + Qy*Yw;
609 *Z = Pz*Xw + Qz*Yw;
610
611 *VX = Px*VXw + Qx*VYw;
612 *VY = Py*VXw + Qy*VYw;
613 *VZ = Pz*VXw + Qz*VYw;
614 }
615
616 /* Compute the site postion and velocity in the RA based coordinate
617 system. SiteMatrix is set to a matrix which is used by GetTopoCentric
618 to convert geocentric coordinates to topocentric (observer-centered)
619 coordinates. */
620
621 static void
GetSitPosition(double SiteLat,double SiteLong,double SiteElevation,double CrntTime,double * SiteX,double * SiteY,double * SiteZ,double * SiteVX,double * SiteVY,MAT3x3 SiteMatrix)622 GetSitPosition(double SiteLat, double SiteLong, double SiteElevation,
623 double CrntTime, double *SiteX, double *SiteY, double *SiteZ, double *SiteVX,
624 double *SiteVY, MAT3x3 SiteMatrix)
625 {
626 static double G1,G2; /* Used to correct for flattening of the Earth */
627 static double CosLat,SinLat;
628 static double OldSiteLat = -100000; /* Used to avoid unneccesary recomputation */
629 static double OldSiteElevation = -100000;
630 double Lat;
631 double SiteRA; /* Right Ascension of site */
632 double CosRA,SinRA;
633
634 if ((SiteLat != OldSiteLat) || (SiteElevation != OldSiteElevation))
635 {
636 OldSiteLat = SiteLat;
637 OldSiteElevation = SiteElevation;
638 Lat = atan(1/(1-SQR(EarthFlat))*tan(SiteLat));
639
640 CosLat = cos(Lat);
641 SinLat = sin(Lat);
642
643 G1 = EarthRadius/(sqrt(1-(2*EarthFlat-SQR(EarthFlat))*SQR(SinLat)));
644 G2 = G1*SQR(1-EarthFlat);
645 G1 += SiteElevation;
646 G2 += SiteElevation;
647 }
648
649
650 SiteRA = PI2*((CrntTime-SidDay)*SiderealSolar + SidReference)
651 - SiteLong;
652 CosRA = cos(SiteRA);
653 SinRA = sin(SiteRA);
654
655
656 *SiteX = G1*CosLat*CosRA;
657 *SiteY = G1*CosLat*SinRA;
658 *SiteZ = G2*SinLat;
659 *SiteVX = -SidRate * *SiteY;
660 *SiteVY = SidRate * *SiteX;
661
662 SiteMatrix[0][0] = SinLat*CosRA;
663 SiteMatrix[0][1] = SinLat*SinRA;
664 SiteMatrix[0][2] = -CosLat;
665 SiteMatrix[1][0] = -SinRA;
666 SiteMatrix[1][1] = CosRA;
667 SiteMatrix[1][2] = 0.0;
668 SiteMatrix[2][0] = CosRA*CosLat;
669 SiteMatrix[2][1] = SinRA*CosLat;
670 SiteMatrix[2][2] = SinLat;
671 }
672
673 static void
GetRange(double SiteX,double SiteY,double SiteZ,double SiteVX,double SiteVY,double SatX,double SatY,double SatZ,double SatVX,double SatVY,double SatVZ,double * Range,double * RangeRate)674 GetRange(double SiteX, double SiteY, double SiteZ, double SiteVX,
675 double SiteVY, double SatX, double SatY, double SatZ, double SatVX,
676 double SatVY, double SatVZ, double *Range, double *RangeRate)
677 {
678 double DX,DY,DZ;
679
680 DX = SatX - SiteX; DY = SatY - SiteY; DZ = SatZ - SiteZ;
681
682 *Range = sqrt(SQR(DX)+SQR(DY)+SQR(DZ));
683
684 *RangeRate = ((SatVX-SiteVX)*DX + (SatVY-SiteVY)*DY + SatVZ*DZ)
685 / *Range;
686 }
687
688 /* Convert from geocentric RA based coordinates to topocentric
689 (observer centered) coordinates */
690
691 static void
GetTopocentric(double SatX,double SatY,double SatZ,double SiteX,double SiteY,double SiteZ,MAT3x3 SiteMatrix,double * X,double * Y,double * Z)692 GetTopocentric(double SatX, double SatY, double SatZ, double SiteX,
693 double SiteY, double SiteZ, MAT3x3 SiteMatrix, double *X, double *Y,
694 double *Z)
695 {
696 SatX -= SiteX;
697 SatY -= SiteY;
698 SatZ -= SiteZ;
699
700 *X = SiteMatrix[0][0]*SatX + SiteMatrix[0][1]*SatY
701 + SiteMatrix[0][2]*SatZ;
702 *Y = SiteMatrix[1][0]*SatX + SiteMatrix[1][1]*SatY
703 + SiteMatrix[1][2]*SatZ;
704 *Z = SiteMatrix[2][0]*SatX + SiteMatrix[2][1]*SatY
705 + SiteMatrix[2][2]*SatZ;
706 }
707
708 static void
GetBearings(double SatX,double SatY,double SatZ,double SiteX,double SiteY,double SiteZ,MAT3x3 SiteMatrix,double * Azimuth,double * Elevation)709 GetBearings(double SatX, double SatY, double SatZ, double SiteX,
710 double SiteY, double SiteZ, MAT3x3 SiteMatrix, double *Azimuth,
711 double *Elevation)
712 {
713 double x,y,z;
714
715 GetTopocentric(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,&x,&y,&z);
716
717 *Elevation = atan(z/sqrt(SQR(x) + SQR(y)));
718
719 *Azimuth = PI - atan2(y,x);
720
721 if (*Azimuth < 0)
722 *Azimuth += PI;
723 }
724
725 static int
Eclipsed(double SatX,double SatY,double SatZ,double SatRadius,double CrntTime)726 Eclipsed(double SatX, double SatY, double SatZ, double SatRadius,
727 double CrntTime)
728 {
729 double MeanAnomaly,TrueAnomaly;
730 double SunX,SunY,SunZ,SunRad;
731 double vx,vy,vz;
732 double CosTheta;
733
734 MeanAnomaly = SunMeanAnomaly+ (CrntTime-SunEpochTime)*SunMeanMotion*PI2;
735 TrueAnomaly = Kepler(MeanAnomaly,SunEccentricity);
736
737 GetSatPosition(SunEpochTime,SunRAAN,SunArgPerigee,SunSemiMajorAxis,
738 SunInclination,SunEccentricity,0.0,0.0,CrntTime,
739 TrueAnomaly,&SunX,&SunY,&SunZ,&SunRad,&vx,&vy,&vz);
740
741 CosTheta = (SunX*SatX + SunY*SatY + SunZ*SatZ)/(SunRad*SatRadius)
742 *CosPenumbra + (SatRadius/EarthRadius)*SinPenumbra;
743
744 if (CosTheta < 0)
745 if (CosTheta < -sqrt(SQR(SatRadius)-SQR(EarthRadius))/SatRadius
746 *CosPenumbra + (SatRadius/EarthRadius)*SinPenumbra)
747
748 return 1;
749 return 0;
750 }
751
752 /* Initialize the Sun's keplerian elements for a given epoch.
753 Formulas are from "Explanatory Supplement to the Astronomical Ephemeris".
754 Also init the sidereal reference */
755
756 static void
InitOrbitRoutines(double EpochDay,int AtEod)757 InitOrbitRoutines(double EpochDay, int AtEod)
758 {
759 double T,T2,T3,Omega;
760 int n;
761 double SunTrueAnomaly,SunDistance;
762
763 T = (floor(EpochDay)-0.5)/36525;
764 T2 = T*T;
765 T3 = T2*T;
766
767 SidDay = floor(EpochDay);
768
769 SidReference = (6.6460656 + 2400.051262*T + 0.00002581*T2)/24;
770 SidReference -= floor(SidReference);
771
772 /* Omega is used to correct for the nutation and the abberation */
773 Omega = AtEod ? (259.18 - 1934.142*T) * RadiansPerDegree : 0.0;
774 n = (int)(Omega / PI2);
775 Omega -= n*PI2;
776
777 SunEpochTime = EpochDay;
778 SunRAAN = 0;
779
780 SunInclination = (23.452294 - 0.0130125*T - 0.00000164*T2
781 + 0.000000503*T3 +0.00256*cos(Omega)) * RadiansPerDegree;
782 SunEccentricity = (0.01675104 - 0.00004180*T - 0.000000126*T2);
783 SunArgPerigee = (281.220833 + 1.719175*T + 0.0004527*T2
784 + 0.0000033*T3) * RadiansPerDegree;
785 SunMeanAnomaly = (358.475845 + 35999.04975*T - 0.00015*T2
786 - 0.00000333333*T3) * RadiansPerDegree;
787 n = (int)(SunMeanAnomaly / PI2);
788 SunMeanAnomaly -= n*PI2;
789
790 SunMeanMotion = 1/(365.24219879 - 0.00000614*T);
791
792 SunTrueAnomaly = Kepler(SunMeanAnomaly,SunEccentricity);
793 SunDistance = SunSemiMajorAxis*(1-SQR(SunEccentricity))
794 / (1+SunEccentricity*cos(SunTrueAnomaly));
795
796 SinPenumbra = (SunRadius-EarthRadius)/SunDistance;
797 CosPenumbra = sqrt(1-SQR(SinPenumbra));
798 }
799
800