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

Last change on this file since 4010 was 3654, checked in by cmv, 16 years ago

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