1 #define _XOPEN_SOURCE 600
2 #include <math.h>
3 #include <string.h>
4 #include "defs.h"
5 #include "unsorted.h"
6 #include "sdp4.h"
7 #include "sgp4.h"
8 #include "sun.h"
9
10 bool is_eclipsed(const double pos[3], const double sol[3], double *depth);
11 bool predict_decayed(const predict_orbital_elements_t *orbital_elements, predict_julian_date_t time);
12
13 //length of buffer used for extracting subsets of TLE strings for parsing
14 #define SUBSTRING_BUFFER_LENGTH 50
15
predict_parse_tle(const char * tle_line_1,const char * tle_line_2)16 predict_orbital_elements_t* predict_parse_tle(const char *tle_line_1, const char *tle_line_2)
17 {
18 double tempnum;
19 predict_orbital_elements_t *m = (predict_orbital_elements_t*)malloc(sizeof(predict_orbital_elements_t));
20 if (m == NULL) return NULL;
21
22 char substring_buffer[SUBSTRING_BUFFER_LENGTH];
23 m->satellite_number = atol(SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,2,6));
24 m->element_number = atol(SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,64,67));
25 m->epoch_year = atoi(SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,18,19));
26 strncpy(m->designator, SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,9,16),8);
27 m->epoch_day = atof(SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,20,31));
28 m->inclination = atof(SubString(tle_line_2,SUBSTRING_BUFFER_LENGTH,substring_buffer,8,15));
29 m->right_ascension = atof(SubString(tle_line_2,SUBSTRING_BUFFER_LENGTH,substring_buffer,17,24));
30 m->eccentricity = 1.0e-07*atof(SubString(tle_line_2,SUBSTRING_BUFFER_LENGTH,substring_buffer,26,32));
31 m->argument_of_perigee = atof(SubString(tle_line_2,SUBSTRING_BUFFER_LENGTH,substring_buffer,34,41));
32 m->mean_anomaly = atof(SubString(tle_line_2,SUBSTRING_BUFFER_LENGTH,substring_buffer,43,50));
33 m->mean_motion = atof(SubString(tle_line_2,SUBSTRING_BUFFER_LENGTH,substring_buffer,52,62));
34 m->derivative_mean_motion = atof(SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,33,42));
35 tempnum=1.0e-5*atof(SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,44,49));
36 m->second_derivative_mean_motion = tempnum/pow(10.0,(tle_line_1[51]-'0'));
37 tempnum=1.0e-5*atof(SubString(tle_line_1,SUBSTRING_BUFFER_LENGTH,substring_buffer,53,58));
38 m->bstar_drag_term = tempnum/pow(10.0,(tle_line_1[60]-'0'));
39 m->revolutions_at_epoch = atof(SubString(tle_line_2,SUBSTRING_BUFFER_LENGTH,substring_buffer,63,67));
40
41 /* Period > 225 minutes is deep space */
42 double ao, xnodp, dd1, dd2, delo, a1, del1, r1;
43 double temp = TWO_PI/MINUTES_PER_DAY/MINUTES_PER_DAY;
44 double xno = m->mean_motion*temp*MINUTES_PER_DAY; //from old TLE struct
45 dd1=(XKE/xno);
46 dd2=TWO_THIRD;
47 a1=pow(dd1,dd2);
48 r1=cos(m->inclination*M_PI/180.0);
49 dd1=(1.0-m->eccentricity*m->eccentricity);
50 temp=CK2*1.5f*(r1*r1*3.0-1.0)/pow(dd1,1.5);
51 del1=temp/(a1*a1);
52 ao=a1*(1.0-del1*(TWO_THIRD*.5+del1*(del1*1.654320987654321+1.0)));
53 delo=temp/(ao*ao);
54 xnodp=xno/(delo+1.0);
55
56 /* Select a deep-space/near-earth ephemeris */
57 if (TWO_PI/xnodp/MINUTES_PER_DAY >= 0.15625) {
58 m->ephemeris = EPHEMERIS_SDP4;
59
60 // Allocate memory for ephemeris data
61 m->ephemeris_data = malloc(sizeof(struct _sdp4));
62
63 if (m->ephemeris_data == NULL) {
64 predict_destroy_orbital_elements(m);
65 return NULL;
66 }
67 // Initialize ephemeris data structure
68 sdp4_init(m, (struct _sdp4*)m->ephemeris_data);
69
70 } else {
71 m->ephemeris = EPHEMERIS_SGP4;
72
73 // Allocate memory for ephemeris data
74 m->ephemeris_data = malloc(sizeof(struct _sgp4));
75
76 if (m->ephemeris_data == NULL) {
77 predict_destroy_orbital_elements(m);
78 return NULL;
79 }
80 // Initialize ephemeris data structure
81 sgp4_init(m, (struct _sgp4*)m->ephemeris_data);
82 }
83
84 return m;
85 }
86
predict_destroy_orbital_elements(predict_orbital_elements_t * m)87 void predict_destroy_orbital_elements(predict_orbital_elements_t *m)
88 {
89 if (m == NULL) return;
90
91 if (m->ephemeris_data != NULL) {
92 free(m->ephemeris_data);
93 }
94
95 free(m);
96 }
97
predict_is_geosynchronous(const predict_orbital_elements_t * m)98 bool predict_is_geosynchronous(const predict_orbital_elements_t *m)
99 {
100 return (m->mean_motion >= GEOSYNCHRONOUS_LOWER_MEAN_MOTION)
101 && (m->mean_motion <= GEOSYNCHRONOUS_UPPER_MEAN_MOTION)
102 && (fabs(m->eccentricity) <= GEOSYNCHRONOUS_ECCENTRICITY_THRESHOLD)
103 && (fabs(m->inclination) <= GEOSYNCHRONOUS_INCLINATION_THRESHOLD_DEGREES);
104 }
105
predict_apogee(const predict_orbital_elements_t * m)106 double predict_apogee(const predict_orbital_elements_t *m)
107 {
108 double sma = 331.25*exp(log(1440.0/m->mean_motion)*(2.0/3.0));
109 return sma*(1.0+m->eccentricity)-EARTH_RADIUS_KM_WGS84;
110 }
111
predict_perigee(const predict_orbital_elements_t * m)112 double predict_perigee(const predict_orbital_elements_t *m)
113 {
114 double xno = m->mean_motion*TWO_PI/MINUTES_PER_DAY;
115 double a1=pow(XKE/xno,TWO_THIRD);
116 double cosio=cos(m->inclination*M_PI/180.0);
117 double theta2=cosio*cosio;
118 double x3thm1=3*theta2-1.0;
119 double eosq=m->eccentricity*m->eccentricity;
120 double betao2=1.0-eosq;
121 double betao=sqrt(betao2);
122 double del1=1.5*CK2*x3thm1/(a1*a1*betao*betao2);
123 double ao=a1*(1.0-del1*(0.5*TWO_THIRD+del1*(1.0+134.0/81.0*del1)));
124 double delo=1.5*CK2*x3thm1/(ao*ao*betao*betao2);
125 double aodp=ao/(1.0-delo);
126
127 return (aodp*(1-m->eccentricity)-AE)*EARTH_RADIUS_KM_WGS84;
128 }
129
predict_aos_happens(const predict_orbital_elements_t * m,double latitude)130 bool predict_aos_happens(const predict_orbital_elements_t *m, double latitude)
131 {
132 /* This function returns true if the satellite pointed to by
133 "x" can ever rise above the horizon of the ground station. */
134
135 double lin, apogee;
136
137 if (m->mean_motion==0.0)
138 return false;
139 else
140 {
141 lin = m->inclination;
142
143 if (lin >= 90.0) lin = 180.0-lin;
144
145 apogee = predict_apogee(m);
146
147 if ((acos(EARTH_RADIUS_KM_WGS84/(apogee+EARTH_RADIUS_KM_WGS84))+(lin*M_PI/180.0)) > fabs(latitude))
148 return true;
149 else
150 return false;
151 }
152 }
153
154 /* This is the stuff we need to do repetitively while tracking. */
155 /* This is the old Calc() function. */
predict_orbit(const predict_orbital_elements_t * orbital_elements,struct predict_position * m,double utc)156 int predict_orbit(const predict_orbital_elements_t *orbital_elements, struct predict_position *m, double utc)
157 {
158 /* Set time to now if now time is provided: */
159 if (utc == 0) utc = predict_to_julian(time(NULL));
160
161 /* Satellite position and velocity vectors */
162 vec3_set(m->position, 0, 0, 0);
163 vec3_set(m->velocity, 0, 0, 0);
164
165 m->time = utc;
166 double julTime = utc + JULIAN_TIME_DIFF;
167
168 /* Convert satellite's epoch time to Julian */
169 /* and calculate time since epoch in minutes */
170 double epoch = 1000.0*orbital_elements->epoch_year + orbital_elements->epoch_day;
171 double jul_epoch = Julian_Date_of_Epoch(epoch);
172 double tsince = (julTime - jul_epoch)*MINUTES_PER_DAY;
173
174 /* Call NORAD routines according to deep-space flag. */
175 struct model_output output;
176 switch (orbital_elements->ephemeris) {
177 case EPHEMERIS_SDP4:
178 sdp4_predict((struct _sdp4*)orbital_elements->ephemeris_data, tsince, &output);
179 break;
180 case EPHEMERIS_SGP4:
181 sgp4_predict((struct _sgp4*)orbital_elements->ephemeris_data, tsince, &output);
182 break;
183 default:
184 //Panic!
185 return -1;
186 }
187 m->position[0] = output.pos[0];
188 m->position[1] = output.pos[1];
189 m->position[2] = output.pos[2];
190 m->velocity[0] = output.vel[0];
191 m->velocity[1] = output.vel[1];
192 m->velocity[2] = output.vel[2];
193 m->phase = output.phase;
194 m->argument_of_perigee = output.omgadf;
195 m->inclination = output.xinck;
196 m->right_ascension = output.xnodek;
197
198 /* TODO: Remove? Scale position and velocity vectors to km and km/sec */
199 Convert_Sat_State(m->position, m->velocity);
200
201 /* Calculate satellite Lat North, Lon East and Alt. */
202 geodetic_t sat_geodetic;
203 Calculate_LatLonAlt(utc, m->position, &sat_geodetic);
204
205 m->latitude = sat_geodetic.lat;
206 m->longitude = sat_geodetic.lon;
207 m->altitude = sat_geodetic.alt;
208
209 // Calculate solar position
210 double solar_vector[3];
211 sun_predict(m->time, solar_vector);
212
213 // Find eclipse depth and if sat is eclipsed
214 m->eclipsed = is_eclipsed(m->position, solar_vector, &m->eclipse_depth);
215
216 // Calculate footprint
217 m->footprint = 2.0*EARTH_RADIUS_KM_WGS84*acos(EARTH_RADIUS_KM_WGS84/(EARTH_RADIUS_KM_WGS84 + m->altitude));
218
219 // Calculate current number of revolutions around Earth
220 double temp = TWO_PI/MINUTES_PER_DAY/MINUTES_PER_DAY;
221 double age = julTime - jul_epoch;
222 double xno = orbital_elements->mean_motion*temp*MINUTES_PER_DAY;
223 double xmo = orbital_elements->mean_anomaly * M_PI / 180.0;
224 m->revolutions = (long)floor((xno*MINUTES_PER_DAY/(M_PI*2.0) + age*orbital_elements->bstar_drag_term)*age + xmo/(2.0*M_PI)) + orbital_elements->revolutions_at_epoch;
225
226 //calculate whether orbit is decayed
227 m->decayed = predict_decayed(orbital_elements, utc);
228
229 return 0;
230 }
231
predict_decayed(const predict_orbital_elements_t * orbital_elements,predict_julian_date_t time)232 bool predict_decayed(const predict_orbital_elements_t *orbital_elements, predict_julian_date_t time)
233 {
234 double satepoch;
235 satepoch=DayNum(1,0,orbital_elements->epoch_year)+orbital_elements->epoch_day;
236
237 bool has_decayed = false;
238 if (satepoch + ((16.666666 - orbital_elements->mean_motion)/(10.0*fabs(orbital_elements->derivative_mean_motion))) < time)
239 {
240 has_decayed = true;
241 }
242 return has_decayed;
243 }
244
245 /* Calculates if a position is eclipsed. */
is_eclipsed(const double pos[3],const double sol[3],double * depth)246 bool is_eclipsed(const double pos[3], const double sol[3], double *depth)
247 {
248 double Rho[3], earth[3];
249
250 /* Determine partial eclipse */
251 double sd_earth = asin_(EARTH_RADIUS_KM_WGS84 / vec3_length(pos));
252 vec3_sub(sol, pos, Rho);
253 double sd_sun = asin_(SOLAR_RADIUS_KM / vec3_length(Rho));
254 vec3_mul_scalar(pos, -1, earth);
255
256 double delta = acos_( vec3_dot(sol, earth) / vec3_length(sol) / vec3_length(earth) );
257 *depth = sd_earth - sd_sun - delta;
258
259 if (sd_earth < sd_sun) return false;
260 else if (*depth >= 0) return true;
261 else return false;
262 }
263
predict_squint_angle(const predict_observer_t * observer,const struct predict_position * orbit,double alon,double alat)264 double predict_squint_angle(const predict_observer_t *observer, const struct predict_position *orbit, double alon, double alat)
265 {
266 double bx = cos(alat)*cos(alon + orbit->argument_of_perigee);
267 double by = cos(alat)*sin(alon + orbit->argument_of_perigee);
268 double bz = sin(alat);
269
270 double cx = bx;
271 double cy = by*cos(orbit->inclination) - bz*sin(orbit->inclination);
272 double cz = by*sin(orbit->inclination) + bz*cos(orbit->inclination);
273 double ax = cx*cos(orbit->right_ascension) - cy*sin(orbit->right_ascension);
274 double ay = cx*sin(orbit->right_ascension) + cy*cos(orbit->right_ascension);
275 double az = cz;
276
277 struct predict_observation obs;
278 predict_observe_orbit(observer, orbit, &obs);
279 double squint = acos(-(ax*obs.range_x + ay*obs.range_y + az*obs.range_z)/obs.range);
280
281 return squint;
282 }
283