source: Sophya/trunk/SophyaExt/XephemAstroLib/earthsat.c@ 1895

Last change on this file since 1895 was 1719, checked in by cmv, 24 years ago

Adapted to version 3.5 xephem cmv 22/10/2001
File size: 24.1 KB
Line 
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
11 *
12 */
13
14/* define this to use orbit's propagator
15#define USE_ORBIT_PROPAGATOR
16 */
17
18/* define this to print some stuff
19#define ESAT_TRACE
20 */
21
22#include <stdio.h>
23#include <math.h>
24#include <string.h>
25
26#if defined(__STDC__)
27#include <stdlib.h>
28#endif
29
30#include "P_.h"
31#include "astro.h"
32#include "circum.h"
33#include "preferences.h"
34
35#include "vector.h"
36#include "sattypes.h"
37#include "satlib.h"
38
39
40#define ESAT_MAG 2 /* fake satellite magnitude */
41
42typedef double MAT3x3[3][3];
43
44static void esat_prop P_((Now *np, Obj *op, double *SatX, double *SatY, double
45 *SatZ, double *SatVX, double *SatVY, double *SatVZ));
46static void GetSatelliteParams P_((Obj *op));
47static void GetSiteParams P_((Now *np));
48static double Kepler P_((double MeanAnomaly, double Eccentricity));
49static void GetSubSatPoint P_((double SatX, double SatY, double SatZ,
50 double T, double *Latitude, double *Longitude, double *Height));
51static void GetSatPosition P_((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));
56static void GetSitPosition P_((double SiteLat, double SiteLong,
57 double SiteElevation, double CrntTime, double *SiteX, double *SiteY,
58 double *SiteZ, double *SiteVX, double *SiteVY, MAT3x3 SiteMatrix));
59static void GetRange P_((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));
63static void GetTopocentric P_((double SatX, double SatY, double SatZ,
64 double SiteX, double SiteY, double SiteZ, MAT3x3 SiteMatrix, double *X,
65 double *Y, double *Z));
66static void GetBearings P_((double SatX, double SatY, double SatZ,
67 double SiteX, double SiteY, double SiteZ, MAT3x3 SiteMatrix,
68 double *Azimuth, double *Elevation));
69static int Eclipsed P_((double SatX, double SatY, double SatZ,
70 double SatRadius, double CrntTime));
71static void InitOrbitRoutines P_((double EpochDay, int AtEod));
72
73#ifdef USE_ORBIT_PROPAGATOR
74static void GetPrecession P_((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 */
106static double EpochDay; /* time of epoch */
107static double EpochMeanAnomaly; /* Mean Anomaly at epoch */
108static long EpochOrbitNum; /* Integer orbit # of epoch */
109static double EpochRAAN; /* RAAN at epoch */
110static double epochMeanMotion; /* Revolutions/day */
111static double OrbitalDecay; /* Revolutions/day^2 */
112static double EpochArgPerigee; /* argument of perigee at epoch */
113static double Eccentricity;
114static double Inclination;
115
116/* Site Parameters */
117static double SiteLat,SiteLong,SiteAltitude;
118
119
120static double SidDay,SidReference; /* Date and sidereal time */
121
122/* Keplerian elements for the sun */
123static double SunEpochTime,SunInclination,SunRAAN,SunEccentricity,
124 SunArgPerigee,SunMeanAnomaly,SunMeanMotion;
125
126/* values for shadow geometry */
127static 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 */
136int
137obj_earthsat (np, op)
138Now *np;
139Obj *op;
140{
141 double Radius; /* From geocenter */
142 double SatX,SatY,SatZ; /* In Right Ascension based system */
143 double SatVX,SatVY,SatVZ; /* Kilometers/second */
144 double SiteX,SiteY,SiteZ;
145 double SiteVX,SiteVY;
146 double SiteMatrix[3][3];
147 double Height;
148 double SSPLat,SSPLong;
149 double Azimuth,Elevation,Range;
150 double RangeRate;
151 double dtmp;
152 double CrntTime;
153 double ra, dec;
154
155#ifdef ESAT_TRACE
156 printf ("\n");
157 printf ("Name = %s\n", op->o_name);
158 printf ("current jd = %13.5f\n", mjd+MJD0);
159 printf ("current mjd = %g\n", mjd);
160 printf ("satellite jd = %13.5f\n", op->es_epoch+MJD0);
161 printf ("satellite mjd = %g\n", op->es_epoch);
162#endif /* ESAT_TRACE */
163
164 /* xephem uses noon 12/31/1899 as 0; orbit uses midnight 1/1/1900.
165 * thus, xephem runs 12 hours, or 1/2 day, behind of what orbit wants.
166 */
167 CrntTime = mjd + 0.5;
168
169 /* extract the XEphem data forms into those used by orbit.
170 * (we still use some functions and names from orbit, thank you).
171 */
172 InitOrbitRoutines(CrntTime, 1);
173 GetSatelliteParams(op);
174 GetSiteParams(np);
175
176 /* propagate to np->n_mjd */
177 esat_prop (np, op, &SatX, &SatY, &SatZ, &SatVX, &SatVY, &SatVZ);
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 = (float) dtmp;
184 op->s_gaedec = (float) 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, (double)op->s_alt, (double)op->s_az, &ha, &dec);
246 now_lst (np, &lst);
247 ra = hrrad(lst) - ha;
248 range (&ra, 2*PI);
249 } else {
250 ra = op->s_gaera;
251 dec = op->s_gaedec;
252 }
253 if (epoch != EOD)
254 precess (mjd, epoch, &ra, &dec);
255 op->s_ra = (float)ra;
256 op->s_dec = (float)dec;
257
258 /* just make up a size and brightness */
259 set_smag (op, ESAT_MAG);
260 op->s_size = (float)0;
261
262 return (0);
263}
264
265/* find position and velocity vector for given Obj at the given time.
266 * set USE_ORBIT_PROPAGATOR depending on desired propagator to use.
267 */
268static void
269esat_prop (np, op, SatX, SatY, SatZ, SatVX, SatVY, SatVZ)
270Now *np;
271Obj *op;
272double *SatX,*SatY,*SatZ;
273double *SatVX,*SatVY,*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 SemiMajorAxis = 331.25 * exp(2*log(MinutesPerDay/epochMeanMotion)/3);
288 GetPrecession(SemiMajorAxis,Eccentricity,Inclination,&RAANPrecession,
289 &PerigeePrecession);
290
291 ReferenceOrbit = EpochMeanAnomaly/PI2 + EpochOrbitNum;
292
293 CrntTime = mjd + 0.5;
294 AverageMotion = epochMeanMotion + (CrntTime-EpochDay)*OrbitalDecay/2;
295 CurrentMotion = epochMeanMotion + (CrntTime-EpochDay)*OrbitalDecay;
296
297 SemiMajorAxis = 331.25 * exp(2*log(MinutesPerDay/CurrentMotion)/3);
298
299 CurrentOrbit = ReferenceOrbit + (CrntTime-EpochDay)*AverageMotion;
300
301 OrbitNum = CurrentOrbit;
302
303 MeanAnomaly = (CurrentOrbit-OrbitNum)*PI2;
304
305 TrueAnomaly = Kepler(MeanAnomaly,Eccentricity);
306
307 GetSatPosition(EpochDay,EpochRAAN,EpochArgPerigee,SemiMajorAxis,
308 Inclination,Eccentricity,RAANPrecession,PerigeePrecession,
309 CrntTime,TrueAnomaly,SatX,SatY,SatZ,&Radius,SatVX,SatVY,SatVZ);
310
311#ifdef ESAT_TRACE
312 printf ("O Radius = %g\n", Radius);
313 printf ("ReferenceOrbit = %g\n", ReferenceOrbit);
314 printf ("CurrentOrbit = %g\n", CurrentOrbit);
315 printf ("RAANPrecession = %g\n", RAANPrecession);
316 printf ("PerigeePrecession = %g\n", PerigeePrecession);
317 printf ("MeanAnomaly = %g\n", MeanAnomaly);
318 printf ("TrueAnomaly = %g\n", TrueAnomaly);
319 printf ("SemiMajorAxis = %g\n", SemiMajorAxis);
320 printf ("AverageMotion = %g\n", AverageMotion);
321 printf ("CurrentMotion = %g\n", CurrentMotion);
322#endif /* ESAT_TRACE */
323
324#else /* ! USE_ORBIT_PROPAGATOR */
325#define MPD 1440.0 /* minutes per day */
326
327 SatElem se;
328 SatData sd;
329 Vec3 posvec, velvec;
330 double dy;
331 double dt;
332 int yr;
333
334 /* init */
335 memset ((void *)&se, 0, sizeof(se));
336 memset ((void *)&sd, 0, sizeof(sd));
337 sd.elem = &se;
338
339 /* se_EPOCH is packed as yr*1000 + dy, where yr is years since 1900
340 * and dy is day of year, Jan 1 being 1
341 */
342 mjd_dayno (op->es_epoch, &yr, &dy);
343 yr -= 1900;
344 dy += 1;
345 se.se_EPOCH = yr*1000 + dy;
346
347 /* others carry over with some change in units */
348 se.se_XNO = op->es_n * (2*PI/MPD); /* revs/day to rads/min */
349 se.se_XINCL = (float)degrad(op->es_inc);
350 se.se_XNODEO = (float)degrad(op->es_raan);
351 se.se_EO = op->es_e;
352 se.se_OMEGAO = (float)degrad(op->es_ap);
353 se.se_XMO = (float)degrad(op->es_M);
354 se.se_BSTAR = op->es_drag;
355 se.se_XNDT20 = op->es_decay*(2*PI/MPD/MPD); /*rv/dy^^2 to rad/min^^2*/
356
357 se.se_id.orbit = op->es_orbit;
358
359 dt = (mjd-op->es_epoch)*MPD;
360
361#ifdef ESAT_TRACE
362 printf ("se_EPOCH : %30.20f\n", se.se_EPOCH);
363 printf ("se_XNO : %30.20f\n", se.se_XNO);
364 printf ("se_XINCL : %30.20f\n", se.se_XINCL);
365 printf ("se_XNODEO : %30.20f\n", se.se_XNODEO);
366 printf ("se_EO : %30.20f\n", se.se_EO);
367 printf ("se_OMEGAO : %30.20f\n", se.se_OMEGAO);
368 printf ("se_XMO : %30.20f\n", se.se_XMO);
369 printf ("se_BSTAR : %30.20f\n", se.se_BSTAR);
370 printf ("se_XNDT20 : %30.20f\n", se.se_XNDT20);
371 printf ("se_orbit : %30d\n", se.se_id.orbit);
372 printf ("dt : %30.20f\n", dt);
373#endif /* ESAT_TRACE */
374
375 /* compute the state vectors */
376 if (se.se_XNO >= (1.0/225.0))
377 sgp4(&sd, &posvec, &velvec, dt); /* NEO */
378 else
379 sdp4(&sd, &posvec, &velvec, dt); /* GEO */
380 if (sd.prop.sgp4)
381 free (sd.prop.sgp4); /* sd.prop.sdp4 is in same union */
382 if (sd.deep)
383 free (sd.deep);
384
385 *SatX = ERAD*posvec.x/1000; /* earth radii to km */
386 *SatY = ERAD*posvec.y/1000;
387 *SatZ = ERAD*posvec.z/1000;
388 *SatVX = 100*velvec.x; /* ?? */
389 *SatVY = 100*velvec.y;
390 *SatVZ = 100*velvec.z;
391#endif
392}
393
394
395/* grab the xephem stuff from op and copy into orbit's globals.
396 */
397static void
398GetSatelliteParams(op)
399Obj *op;
400{
401 /* the following are for the orbit functions */
402 /* xephem uses noon 12/31/1899 as 0; orbit uses midnight 1/1/1900 as 1.
403 * thus, xephem runs 12 hours, or 1/2 day, behind of what orbit wants.
404 */
405 EpochDay = op->es_epoch + 0.5;
406
407 /* xephem stores inc in degrees; orbit wants rads */
408 Inclination = degrad(op->es_inc);
409
410 /* xephem stores RAAN in degrees; orbit wants rads */
411 EpochRAAN = degrad(op->es_raan);
412
413 Eccentricity = op->es_e;
414
415 /* xephem stores arg of perigee in degrees; orbit wants rads */
416 EpochArgPerigee = degrad(op->es_ap);
417
418 /* xephem stores mean anomaly in degrees; orbit wants rads */
419 EpochMeanAnomaly = degrad (op->es_M);
420
421 epochMeanMotion = op->es_n;
422
423 OrbitalDecay = op->es_decay;
424
425 EpochOrbitNum = op->es_orbit;
426}
427
428
429
430static void
431GetSiteParams(np)
432Now *np;
433{
434 SiteLat = lat;
435
436 /* xephem stores longitude as >0 east; orbit wants >0 west */
437 SiteLong = 2.0*PI - lng;
438
439 /* what orbit calls altitude xephem calls elevation and stores it from
440 * sea level in earth radii; orbit wants km
441 */
442 SiteAltitude = elev*ERAD/1000.0;
443
444 /* we don't implement a minimum horizon altitude cutoff
445 SiteMinElev = 0;
446 */
447
448#ifdef ESAT_TRACE
449 printf ("SiteLat = %g\n", SiteLat);
450 printf ("SiteLong = %g\n", SiteLong);
451 printf ("SiteAltitude = %g\n", SiteAltitude);
452 fflush (stdout);
453#endif
454}
455
456
457/* Solve Kepler's equation */
458/* Inputs: */
459/* MeanAnomaly Time Since last perigee, in radians. */
460/* PI2 = one complete orbit. */
461/* Eccentricity Eccentricity of orbit's ellipse. */
462/* Output: */
463/* TrueAnomaly Angle between perigee, geocenter, and */
464/* current position. */
465
466static
467double Kepler(MeanAnomaly,Eccentricity)
468register double MeanAnomaly,Eccentricity;
469
470{
471register double E; /* Eccentric Anomaly */
472register double Error;
473register double TrueAnomaly;
474
475 E = MeanAnomaly ;/*+ Eccentricity*sin(MeanAnomaly); -- Initial guess */
476 do
477 {
478 Error = (E - Eccentricity*sin(E) - MeanAnomaly)
479 / (1 - Eccentricity*cos(E));
480 E -= Error;
481 }
482 while (ABS(Error) >= Epsilon);
483
484 if (ABS(E-PI) < Epsilon)
485 TrueAnomaly = PI;
486 else
487 TrueAnomaly = 2*atan(sqrt((1+Eccentricity)/(1-Eccentricity))
488 *tan(E/2));
489 if (TrueAnomaly < 0)
490 TrueAnomaly += PI2;
491
492 return TrueAnomaly;
493}
494
495static void
496GetSubSatPoint(SatX,SatY,SatZ,T,Latitude,Longitude,Height)
497double SatX,SatY,SatZ,T;
498double *Latitude,*Longitude,*Height;
499{
500 double r;
501 /* ECD: long i; */
502
503 r = sqrt(SQR(SatX) + SQR(SatY) + SQR(SatZ));
504
505 *Longitude = PI2*((T-SidDay)*SiderealSolar + SidReference)
506 - atan2(SatY,SatX);
507
508 /* ECD:
509 * want Longitude in range -PI to PI , +W
510 */
511 range (Longitude, 2*PI);
512 if (*Longitude > PI)
513 *Longitude -= 2*PI;
514
515 *Latitude = atan(SatZ/sqrt(SQR(SatX) + SQR(SatY)));
516
517#if SSPELLIPSE
518#else
519 *Height = r - EarthRadius;
520#endif
521}
522
523
524#ifdef USE_ORBIT_PROPAGATOR
525static void
526GetPrecession(SemiMajorAxis,Eccentricity,Inclination,
527 RAANPrecession,PerigeePrecession)
528double SemiMajorAxis,Eccentricity,Inclination;
529double *RAANPrecession,*PerigeePrecession;
530{
531 *RAANPrecession = 9.95*pow(EarthRadius/SemiMajorAxis,3.5) * cos(Inclination)
532 / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
533
534 *PerigeePrecession = 4.97*pow(EarthRadius/SemiMajorAxis,3.5)
535 * (5*SQR(cos(Inclination))-1)
536 / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
537}
538#endif /* USE_ORBIT_PROPAGATOR */
539
540/* Compute the satellite postion and velocity in the RA based coordinate
541 * system.
542 * ECD: take care not to let Radius get below EarthRadius.
543 */
544
545static void
546GetSatPosition(EpochTime,EpochRAAN,EpochArgPerigee,SemiMajorAxis,
547 Inclination,Eccentricity,RAANPrecession,PerigeePrecession,
548 T,TrueAnomaly,X,Y,Z,Radius,VX,VY,VZ)
549
550double EpochTime,EpochRAAN,EpochArgPerigee;
551double SemiMajorAxis,Inclination,Eccentricity;
552double RAANPrecession,PerigeePrecession,T, TrueAnomaly;
553double *X,*Y,*Z,*Radius,*VX,*VY,*VZ;
554
555{
556 double RAAN,ArgPerigee;
557
558
559 double Xw,Yw,VXw,VYw; /* In orbital plane */
560 double Tmp;
561 double Px,Qx,Py,Qy,Pz,Qz; /* Escobal transformation 31 */
562 double CosArgPerigee,SinArgPerigee;
563 double CosRAAN,SinRAAN,CoSinclination,SinInclination;
564
565 *Radius = SemiMajorAxis*(1-SQR(Eccentricity))
566 / (1+Eccentricity*cos(TrueAnomaly));
567
568 if (*Radius <= EarthRadius)
569 *Radius = EarthRadius;
570
571
572 Xw = *Radius * cos(TrueAnomaly);
573 Yw = *Radius * sin(TrueAnomaly);
574
575 Tmp = sqrt(GM/(SemiMajorAxis*(1-SQR(Eccentricity))));
576
577 VXw = -Tmp*sin(TrueAnomaly);
578 VYw = Tmp*(cos(TrueAnomaly) + Eccentricity);
579
580 ArgPerigee = EpochArgPerigee + (T-EpochTime)*PerigeePrecession;
581 RAAN = EpochRAAN - (T-EpochTime)*RAANPrecession;
582
583 CosRAAN = cos(RAAN); SinRAAN = sin(RAAN);
584 CosArgPerigee = cos(ArgPerigee); SinArgPerigee = sin(ArgPerigee);
585 CoSinclination = cos(Inclination); SinInclination = sin(Inclination);
586
587 Px = CosArgPerigee*CosRAAN - SinArgPerigee*SinRAAN*CoSinclination;
588 Py = CosArgPerigee*SinRAAN + SinArgPerigee*CosRAAN*CoSinclination;
589 Pz = SinArgPerigee*SinInclination;
590 Qx = -SinArgPerigee*CosRAAN - CosArgPerigee*SinRAAN*CoSinclination;
591 Qy = -SinArgPerigee*SinRAAN + CosArgPerigee*CosRAAN*CoSinclination;
592 Qz = CosArgPerigee*SinInclination;
593
594 *X = Px*Xw + Qx*Yw; /* Escobal, transformation #31 */
595 *Y = Py*Xw + Qy*Yw;
596 *Z = Pz*Xw + Qz*Yw;
597
598 *VX = Px*VXw + Qx*VYw;
599 *VY = Py*VXw + Qy*VYw;
600 *VZ = Pz*VXw + Qz*VYw;
601}
602
603/* Compute the site postion and velocity in the RA based coordinate
604 system. SiteMatrix is set to a matrix which is used by GetTopoCentric
605 to convert geocentric coordinates to topocentric (observer-centered)
606 coordinates. */
607
608static void
609GetSitPosition(SiteLat,SiteLong,SiteElevation,CrntTime,
610 SiteX,SiteY,SiteZ,SiteVX,SiteVY,SiteMatrix)
611
612double SiteLat,SiteLong,SiteElevation,CrntTime;
613double *SiteX,*SiteY,*SiteZ,*SiteVX,*SiteVY;
614MAT3x3 SiteMatrix;
615
616{
617 static double G1,G2; /* Used to correct for flattening of the Earth */
618 static double CosLat,SinLat;
619 static double OldSiteLat = -100000; /* Used to avoid unneccesary recomputation */
620 static double OldSiteElevation = -100000;
621 double Lat;
622 double SiteRA; /* Right Ascension of site */
623 double CosRA,SinRA;
624
625 if ((SiteLat != OldSiteLat) || (SiteElevation != OldSiteElevation))
626 {
627 OldSiteLat = SiteLat;
628 OldSiteElevation = SiteElevation;
629 Lat = atan(1/(1-SQR(EarthFlat))*tan(SiteLat));
630
631 CosLat = cos(Lat);
632 SinLat = sin(Lat);
633
634 G1 = EarthRadius/(sqrt(1-(2*EarthFlat-SQR(EarthFlat))*SQR(SinLat)));
635 G2 = G1*SQR(1-EarthFlat);
636 G1 += SiteElevation;
637 G2 += SiteElevation;
638 }
639
640
641 SiteRA = PI2*((CrntTime-SidDay)*SiderealSolar + SidReference)
642 - SiteLong;
643 CosRA = cos(SiteRA);
644 SinRA = sin(SiteRA);
645
646
647 *SiteX = G1*CosLat*CosRA;
648 *SiteY = G1*CosLat*SinRA;
649 *SiteZ = G2*SinLat;
650 *SiteVX = -SidRate * *SiteY;
651 *SiteVY = SidRate * *SiteX;
652
653 SiteMatrix[0][0] = SinLat*CosRA;
654 SiteMatrix[0][1] = SinLat*SinRA;
655 SiteMatrix[0][2] = -CosLat;
656 SiteMatrix[1][0] = -SinRA;
657 SiteMatrix[1][1] = CosRA;
658 SiteMatrix[1][2] = 0.0;
659 SiteMatrix[2][0] = CosRA*CosLat;
660 SiteMatrix[2][1] = SinRA*CosLat;
661 SiteMatrix[2][2] = SinLat;
662}
663
664static void
665GetRange(SiteX,SiteY,SiteZ,SiteVX,SiteVY,
666 SatX,SatY,SatZ,SatVX,SatVY,SatVZ,Range,RangeRate)
667
668double SiteX,SiteY,SiteZ,SiteVX,SiteVY;
669double SatX,SatY,SatZ,SatVX,SatVY,SatVZ;
670double *Range,*RangeRate;
671{
672 double DX,DY,DZ;
673
674 DX = SatX - SiteX; DY = SatY - SiteY; DZ = SatZ - SiteZ;
675
676 *Range = sqrt(SQR(DX)+SQR(DY)+SQR(DZ));
677
678 *RangeRate = ((SatVX-SiteVX)*DX + (SatVY-SiteVY)*DY + SatVZ*DZ)
679 / *Range;
680}
681
682/* Convert from geocentric RA based coordinates to topocentric
683 (observer centered) coordinates */
684
685static void
686GetTopocentric(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,X,Y,Z)
687double SatX,SatY,SatZ,SiteX,SiteY,SiteZ;
688double *X,*Y,*Z;
689MAT3x3 SiteMatrix;
690{
691 SatX -= SiteX;
692 SatY -= SiteY;
693 SatZ -= SiteZ;
694
695 *X = SiteMatrix[0][0]*SatX + SiteMatrix[0][1]*SatY
696 + SiteMatrix[0][2]*SatZ;
697 *Y = SiteMatrix[1][0]*SatX + SiteMatrix[1][1]*SatY
698 + SiteMatrix[1][2]*SatZ;
699 *Z = SiteMatrix[2][0]*SatX + SiteMatrix[2][1]*SatY
700 + SiteMatrix[2][2]*SatZ;
701}
702
703static void
704GetBearings(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,Azimuth,Elevation)
705double SatX,SatY,SatZ,SiteX,SiteY,SiteZ;
706MAT3x3 SiteMatrix;
707double *Azimuth,*Elevation;
708{
709 double x,y,z;
710
711 GetTopocentric(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,&x,&y,&z);
712
713 *Elevation = atan(z/sqrt(SQR(x) + SQR(y)));
714
715 *Azimuth = PI - atan2(y,x);
716
717 if (*Azimuth < 0)
718 *Azimuth += PI;
719}
720
721static int
722Eclipsed(SatX,SatY,SatZ,SatRadius,CrntTime)
723double SatX,SatY,SatZ,SatRadius,CrntTime;
724{
725 double MeanAnomaly,TrueAnomaly;
726 double SunX,SunY,SunZ,SunRad;
727 double vx,vy,vz;
728 double CosTheta;
729
730 MeanAnomaly = SunMeanAnomaly+ (CrntTime-SunEpochTime)*SunMeanMotion*PI2;
731 TrueAnomaly = Kepler(MeanAnomaly,SunEccentricity);
732
733 GetSatPosition(SunEpochTime,SunRAAN,SunArgPerigee,SunSemiMajorAxis,
734 SunInclination,SunEccentricity,0.0,0.0,CrntTime,
735 TrueAnomaly,&SunX,&SunY,&SunZ,&SunRad,&vx,&vy,&vz);
736
737 CosTheta = (SunX*SatX + SunY*SatY + SunZ*SatZ)/(SunRad*SatRadius)
738 *CosPenumbra + (SatRadius/EarthRadius)*SinPenumbra;
739
740 if (CosTheta < 0)
741 if (CosTheta < -sqrt(SQR(SatRadius)-SQR(EarthRadius))/SatRadius
742 *CosPenumbra + (SatRadius/EarthRadius)*SinPenumbra)
743
744 return 1;
745 return 0;
746}
747
748/* Initialize the Sun's keplerian elements for a given epoch.
749 Formulas are from "Explanatory Supplement to the Astronomical Ephemeris".
750 Also init the sidereal reference */
751
752static void
753InitOrbitRoutines(EpochDay, AtEod)
754double EpochDay;
755int AtEod;
756{
757 double T,T2,T3,Omega;
758 int n;
759 double SunTrueAnomaly,SunDistance;
760
761 T = (floor(EpochDay)-0.5)/36525;
762 T2 = T*T;
763 T3 = T2*T;
764
765 SidDay = floor(EpochDay);
766
767 SidReference = (6.6460656 + 2400.051262*T + 0.00002581*T2)/24;
768 SidReference -= floor(SidReference);
769
770 /* Omega is used to correct for the nutation and the abberation */
771 Omega = AtEod ? (259.18 - 1934.142*T) * RadiansPerDegree : 0.0;
772 n = (int)(Omega / PI2);
773 Omega -= n*PI2;
774
775 SunEpochTime = EpochDay;
776 SunRAAN = 0;
777
778 SunInclination = (23.452294 - 0.0130125*T - 0.00000164*T2
779 + 0.000000503*T3 +0.00256*cos(Omega)) * RadiansPerDegree;
780 SunEccentricity = (0.01675104 - 0.00004180*T - 0.000000126*T2);
781 SunArgPerigee = (281.220833 + 1.719175*T + 0.0004527*T2
782 + 0.0000033*T3) * RadiansPerDegree;
783 SunMeanAnomaly = (358.475845 + 35999.04975*T - 0.00015*T2
784 - 0.00000333333*T3) * RadiansPerDegree;
785 n = (int)(SunMeanAnomaly / PI2);
786 SunMeanAnomaly -= n*PI2;
787
788 SunMeanMotion = 1/(365.24219879 - 0.00000614*T);
789
790 SunTrueAnomaly = Kepler(SunMeanAnomaly,SunEccentricity);
791 SunDistance = SunSemiMajorAxis*(1-SQR(SunEccentricity))
792 / (1+SunEccentricity*cos(SunTrueAnomaly));
793
794 SinPenumbra = (SunRadius-EarthRadius)/SunDistance;
795 CosPenumbra = sqrt(1-SQR(SinPenumbra));
796}
797
798/* For RCS Only -- Do Not Edit */
799static char *rcsid[2] = {(char *)rcsid, "@(#) $RCSfile: earthsat.c,v $ $Date: 2001-10-22 12:08:27 $ $Revision: 1.2 $ $Name: not supported by cvs2svn $"};
Note: See TracBrowser for help on using the repository browser.