1 #include <predict/predict.h>
2 #include "unsorted.h"
3 #include <stdlib.h>
4 #include <string.h>
5 #include "defs.h"
6 #include "sun.h"
7
8 void observer_calculate(const predict_observer_t *observer, double time, const double pos[3], const double vel[3], struct predict_observation *result);
9
predict_create_observer(const char * name,double lat,double lon,double alt)10 predict_observer_t *predict_create_observer(const char *name, double lat, double lon, double alt)
11 {
12 // Allocate memory
13 predict_observer_t *obs = (predict_observer_t*)malloc(sizeof(predict_observer_t));
14 if (obs == NULL) return NULL;
15
16 strncpy(obs->name, name, 128);
17 obs->name[127] = '\0';
18 obs->latitude = lat;
19 obs->longitude = lon;
20 obs->altitude = alt;
21
22 return obs;
23 }
24
predict_destroy_observer(predict_observer_t * obs)25 void predict_destroy_observer(predict_observer_t *obs)
26 {
27 if (obs != NULL) {
28 free(obs);
29 }
30 }
31
32
33 /**
34 * \brief Calculates range, azimuth, elevation and relative velocity.
35 *
36 * Calculated range, azimuth, elevation and relative velocity from the
37 * given observer position.
38 **/
predict_observe_orbit(const predict_observer_t * observer,const struct predict_position * orbit,struct predict_observation * obs)39 void predict_observe_orbit(const predict_observer_t *observer, const struct predict_position *orbit, struct predict_observation *obs)
40 {
41 if (obs == NULL) return;
42
43 double julTime = orbit->time + JULIAN_TIME_DIFF;
44
45 observer_calculate(observer, julTime, orbit->position, orbit->velocity, obs);
46
47 // Calculate visibility status of the orbit: Orbit is visible if sun elevation is low enough and the orbit is above the horizon, but still in sunlight.
48 obs->visible = false;
49 struct predict_observation sun_obs;
50 predict_observe_sun(observer, orbit->time, &sun_obs);
51 if (!(orbit->eclipsed) && (sun_obs.elevation*180.0/M_PI < NAUTICAL_TWILIGHT_SUN_ELEVATION) && (obs->elevation*180.0/M_PI > 0)) {
52 obs->visible = true;
53 }
54 obs->time = orbit->time;
55 }
56
observer_calculate(const predict_observer_t * observer,double time,const double pos[3],const double vel[3],struct predict_observation * result)57 void observer_calculate(const predict_observer_t *observer, double time, const double pos[3], const double vel[3], struct predict_observation *result)
58 {
59
60 /* The procedures Calculate_Obs and Calculate_RADec calculate */
61 /* the *topocentric* coordinates of the object with ECI position, */
62 /* {pos}, and velocity, {vel}, from location {geodetic} at {time}. */
63 /* The {obs_set} returned for Calculate_Obs consists of azimuth, */
64 /* elevation, range, and range rate (in that order) with units of */
65 /* radians, radians, kilometers, and kilometers/second, respectively. */
66 /* The WGS '72 geoid is used and the effect of atmospheric refraction */
67 /* (under standard temperature and pressure) is incorporated into the */
68 /* elevation calculation; the effect of atmospheric refraction on */
69 /* range and range rate has not yet been quantified. */
70
71 /* The {obs_set} for Calculate_RADec consists of right ascension and */
72 /* declination (in that order) in radians. Again, calculations are */
73 /* based on *topocentric* position using the WGS '72 geoid and */
74 /* incorporating atmospheric refraction. */
75
76
77 double obs_pos[3];
78 double obs_vel[3];
79 double range[3];
80 double rgvel[3];
81
82 geodetic_t geodetic;
83 geodetic.lat = observer->latitude;
84 geodetic.lon = observer->longitude;
85 geodetic.alt = observer->altitude / 1000.0;
86 geodetic.theta = 0.0;
87 Calculate_User_PosVel(time, &geodetic, obs_pos, obs_vel);
88
89 vec3_sub(pos, obs_pos, range);
90 vec3_sub(vel, obs_vel, rgvel);
91
92 double range_length = vec3_length(range);
93 double range_rate_length = vec3_dot(range, rgvel) / range_length;
94
95 double theta_dot = 2*M_PI*EARTH_ROTATIONS_PER_SIDERIAL_DAY/SECONDS_PER_DAY;
96 double sin_lat = sin(geodetic.lat);
97 double cos_lat = cos(geodetic.lat);
98 double sin_theta = sin(geodetic.theta);
99 double cos_theta = cos(geodetic.theta);
100
101 double top_s = sin_lat*cos_theta*range[0] + sin_lat*sin_theta*range[1] - cos_lat*range[2];
102 double top_e = -sin_theta*range[0] + cos_theta*range[1];
103 double top_z = cos_lat*cos_theta*range[0] + cos_lat*sin_theta*range[1] + sin_lat*range[2];
104
105
106 double top_s_dot = sin_lat*(cos_theta*rgvel[0] - sin_theta*range[0]*theta_dot) +
107 sin_lat*(sin_theta*rgvel[1] + cos_theta*range[1]*theta_dot) -
108 cos_lat*rgvel[2];
109 double top_e_dot = - (sin_theta*rgvel[0] + cos_theta*range[0]*theta_dot) +
110 (cos_theta*rgvel[1] - sin_theta*range[1]*theta_dot);
111
112 double top_z_dot = cos_lat * ( cos_theta*(rgvel[0] + range[1]*theta_dot) +
113 sin_theta*(rgvel[1] - range[0]*theta_dot) ) +
114 sin_lat*rgvel[2];
115
116 // Azimut
117 double y = -top_e / top_s;
118 double az = atan(-top_e / top_s);
119
120 if (top_s > 0.0) az = az + M_PI;
121 if (az < 0.0) az = az + 2*M_PI;
122
123 // Azimut rate
124 double y_dot = - (top_e_dot*top_s - top_s_dot*top_e) / (top_s*top_s);
125 double az_dot = y_dot / (1 + y*y);
126
127 // Elevation
128 double x = top_z / range_length;
129 double el = asin_(x);
130
131 // Elevation rate
132 double x_dot = (top_z_dot*range_length - range_rate_length*top_z) / (range_length * range_length);
133 double el_dot = x_dot / sqrt( 1 - x*x );
134
135 result->azimuth = az;
136 result->azimuth_rate = az_dot;
137 result->elevation = el;
138 result->elevation_rate = el_dot;
139 result->range = range_length;
140 result->range_rate = range_rate_length;
141 result->range_x = range[0];
142 result->range_y = range[1];
143 result->range_z = range[2];
144
145 }
146
predict_next_aos(const predict_observer_t * observer,const predict_orbital_elements_t * orbital_elements,double start_utc)147 struct predict_observation predict_next_aos(const predict_observer_t *observer, const predict_orbital_elements_t *orbital_elements, double start_utc)
148 {
149 double curr_time = start_utc;
150 struct predict_observation obs;
151 double time_step = 0;
152
153 struct predict_position orbit;
154 predict_orbit(orbital_elements, &orbit, curr_time);
155 predict_observe_orbit(observer, &orbit, &obs);
156
157 //check whether AOS can happen after specified start time
158 if (predict_aos_happens(orbital_elements, observer->latitude) && !predict_is_geosynchronous(orbital_elements) && !orbit.decayed) {
159 //TODO: Time steps have been found in FindAOS/LOS().
160 //Might be based on some pre-existing source, root-finding techniques
161 //or something. Find them, and improve readability of the code and so that
162 //the mathematical stability of the iteration can be checked.
163 //Bisection method, Brent's algorithm? Given a coherent root finding algorithm,
164 //can rather have one function for iterating the orbit and then let get_next_aos/los
165 //specify bounding intervals for the root finding.
166
167 //skip the rest of the pass if the satellite is currently in range, since we want the _next_ AOS.
168 if (obs.elevation > 0.0) {
169 struct predict_observation los = predict_next_los(observer, orbital_elements, curr_time);
170 curr_time = los.time;
171 curr_time += 1.0/(MINUTES_PER_DAY*1.0)*20; //skip 20 minutes. LOS might still be within the elevation threshold. (rough quickfix from predict)
172 predict_orbit(orbital_elements, &orbit, curr_time);
173 predict_observe_orbit(observer, &orbit, &obs);
174 }
175
176 //iteration until the orbit is roughly in range again, before the satellite pass
177 while ((obs.elevation*180.0/M_PI < -1.0) || (obs.elevation_rate < 0)) {
178 time_step = 0.00035*(obs.elevation*180.0/M_PI*((orbit.altitude/8400.0)+0.46)-2.0);
179 curr_time -= time_step;
180 predict_orbit(orbital_elements, &orbit, curr_time);
181 predict_observe_orbit(observer, &orbit, &obs);
182 }
183
184 //fine tune the results until the elevation is within a low enough threshold
185 while (fabs(obs.elevation*180/M_PI) > AOSLOS_HORIZON_THRESHOLD) {
186 time_step = obs.elevation*180.0/M_PI*sqrt(orbit.altitude)/530000.0;
187 curr_time -= time_step;
188 predict_orbit(orbital_elements, &orbit, curr_time);
189 predict_observe_orbit(observer, &orbit, &obs);
190 }
191 }
192 return obs;
193 }
194
195 /**
196 * Pass stepping direction used for pass stepping function below.
197 **/
198 enum step_pass_direction{POSITIVE_DIRECTION, NEGATIVE_DIRECTION};
199
200 /**
201 * Rough stepping through a pass. Uses weird time steps from Predict.
202 *
203 * \param observer Ground station
204 * \param orbital_elements Orbital elements of satellite
205 * \param curr_time Time from which to start stepping
206 * \param direction Either POSITIVE_DIRECTION (step from current time to pass end) or NEGATIVE_DIRECTION (step from current time to start of pass). In case of the former, the pass will be stepped until either elevation is negative or the derivative of the elevation is negative
207 * \return Time for when we have stepped out of the pass
208 * \copyright GPLv2+
209 **/
step_pass(const predict_observer_t * observer,const predict_orbital_elements_t * orbital_elements,double curr_time,enum step_pass_direction direction)210 double step_pass(const predict_observer_t *observer, const predict_orbital_elements_t *orbital_elements, double curr_time, enum step_pass_direction direction) {
211 struct predict_position orbit;
212 struct predict_observation obs;
213 do {
214 predict_orbit(orbital_elements, &orbit, curr_time);
215 predict_observe_orbit(observer, &orbit, &obs);
216
217 //weird time stepping from Predict, but which magically works
218 double time_step = cos(obs.elevation - 1.0)*sqrt(orbit.altitude)/25000.0;
219 if (((direction == POSITIVE_DIRECTION) && time_step < 0) || ((direction == NEGATIVE_DIRECTION) && time_step > 0)) {
220 time_step = -time_step;
221 }
222
223 curr_time += time_step;
224 } while ((obs.elevation >= 0) || ((direction == POSITIVE_DIRECTION) && (obs.elevation_rate > 0.0)));
225 return curr_time;
226 }
227
predict_next_los(const predict_observer_t * observer,const predict_orbital_elements_t * orbital_elements,double start_utc)228 struct predict_observation predict_next_los(const predict_observer_t *observer, const predict_orbital_elements_t *orbital_elements, double start_utc)
229 {
230 double curr_time = start_utc;
231 struct predict_observation obs;
232 double time_step = 0;
233
234 struct predict_position orbit;
235 predict_orbit(orbital_elements, &orbit, curr_time);
236 predict_observe_orbit(observer, &orbit, &obs);
237
238 //check whether AOS/LOS can happen after specified start time
239 if (predict_aos_happens(orbital_elements, observer->latitude) && !predict_is_geosynchronous(orbital_elements) && !orbit.decayed) {
240 //iteration algorithm from Predict, see comments in predict_next_aos().
241
242 //iterate until next satellite pass
243 if (obs.elevation < 0.0) {
244 struct predict_observation aos = predict_next_aos(observer, orbital_elements, curr_time);
245 curr_time = aos.time;
246 predict_orbit(orbital_elements, &orbit, curr_time);
247 predict_observe_orbit(observer, &orbit, &obs);
248 }
249
250 //step through the pass
251 curr_time = step_pass(observer, orbital_elements, curr_time, POSITIVE_DIRECTION);
252
253 //fine tune to elevation threshold
254 do {
255 time_step = obs.elevation*180.0/M_PI*sqrt(orbit.altitude)/502500.0;
256 curr_time += time_step;
257 predict_orbit(orbital_elements, &orbit, curr_time);
258 predict_observe_orbit(observer, &orbit, &obs);
259 } while (fabs(obs.elevation*180.0/M_PI) > AOSLOS_HORIZON_THRESHOLD);
260 }
261 return obs;
262 }
263
264 /**
265 * Convenience function for calculation of derivative of elevation at specific time.
266 *
267 * \param observer Ground station
268 * \param orbital_elements Orbital elements for satellite
269 * \param time Time
270 * \return Derivative of elevation at input time
271 **/
elevation_derivative(const predict_observer_t * observer,const predict_orbital_elements_t * orbital_elements,double time)272 double elevation_derivative(const predict_observer_t *observer, const predict_orbital_elements_t *orbital_elements, double time)
273 {
274 struct predict_position orbit;
275 struct predict_observation observation;
276 predict_orbit(orbital_elements, &orbit, time);
277 predict_observe_orbit(observer, &orbit, &observation);
278 return observation.elevation_rate;
279 }
280
281 /**
282 * Find maximum elevation bracketed by input lower and upper time.
283 *
284 * \param observer Ground station
285 * \param orbital_elements Orbital elements of satellite
286 * \param lower_time Lower time bracket
287 * \param upper_time Upper time bracket
288 * \return Observation of satellite for maximum elevation between lower and upper time brackets
289 **/
find_max_elevation(const predict_observer_t * observer,const predict_orbital_elements_t * orbital_elements,double lower_time,double upper_time)290 struct predict_observation find_max_elevation(const predict_observer_t *observer, const predict_orbital_elements_t *orbital_elements, double lower_time, double upper_time)
291 {
292 double max_ele_time_candidate = (upper_time + lower_time)/2.0;
293 int iteration = 0;
294 while ((fabs(lower_time - upper_time) > MAXELE_TIME_EQUALITY_THRESHOLD) && (iteration < MAXELE_MAX_NUM_ITERATIONS)) {
295 max_ele_time_candidate = (upper_time + lower_time)/2.0;
296
297 //calculate derivatives for lower, upper and candidate
298 double candidate_deriv = elevation_derivative(observer, orbital_elements, max_ele_time_candidate);
299 double lower_deriv = elevation_derivative(observer, orbital_elements, lower_time);
300 double upper_deriv = elevation_derivative(observer, orbital_elements, upper_time);
301
302 //check whether derivative has changed sign
303 if (candidate_deriv*lower_deriv < 0) {
304 upper_time = max_ele_time_candidate;
305 } else if (candidate_deriv*upper_deriv < 0) {
306 lower_time = max_ele_time_candidate;
307 } else {
308 break;
309 }
310 iteration++;
311 }
312
313 //prepare output
314 struct predict_position orbit;
315 struct predict_observation observation;
316 predict_orbit(orbital_elements, &orbit, max_ele_time_candidate);
317 predict_observe_orbit(observer, &orbit, &observation);
318 return observation;
319 }
320
predict_at_max_elevation(const predict_observer_t * observer,const predict_orbital_elements_t * orbital_elements,predict_julian_date_t start_time)321 struct predict_observation predict_at_max_elevation(const predict_observer_t *observer, const predict_orbital_elements_t *orbital_elements, predict_julian_date_t start_time)
322 {
323 struct predict_observation ret_observation = {0};
324
325 if (predict_is_geosynchronous(orbital_elements)) {
326 return ret_observation;
327 }
328
329 struct predict_position orbit;
330 struct predict_observation observation;
331 predict_orbit(orbital_elements, &orbit, start_time);
332 if (orbit.decayed) {
333 return ret_observation;
334 }
335
336 predict_observe_orbit(observer, &orbit, &observation);
337
338 //bracket the solution by approximate times for AOS and LOS of the current or next pass
339 double lower_time = start_time;
340 double upper_time = start_time;
341 if (observation.elevation < 0) {
342 struct predict_observation aos = predict_next_aos(observer, orbital_elements, start_time);
343 lower_time = aos.time;
344 } else {
345 lower_time = step_pass(observer, orbital_elements, lower_time, NEGATIVE_DIRECTION);
346 }
347 struct predict_observation los = predict_next_los(observer, orbital_elements, lower_time);
348 upper_time = los.time;
349
350 //assume that we can only have two potential local maxima along the elevation curve, and be content with that. For most cases, we will only have one, unless it is a satellite in deep space with long passes and weird effects.
351
352 //bracket by AOS/LOS
353 struct predict_observation candidate_center = find_max_elevation(observer, orbital_elements, lower_time, upper_time);
354
355 //bracket by a combination of the found candidate above and either AOS or LOS (will thus cover solutions within [aos, candidate] and [candidate, los])
356 struct predict_observation candidate_lower = find_max_elevation(observer, orbital_elements, candidate_center.time - MAXELE_TIME_EQUALITY_THRESHOLD, upper_time);
357 struct predict_observation candidate_upper = find_max_elevation(observer, orbital_elements, lower_time, candidate_center.time + MAXELE_TIME_EQUALITY_THRESHOLD);
358
359 //return the best candidate
360 if ((candidate_center.elevation > candidate_lower.elevation) && (candidate_center.elevation > candidate_upper.elevation)) {
361 return candidate_center;
362 } else if (candidate_lower.elevation > candidate_upper.elevation) {
363 return candidate_lower;
364 } else {
365 return candidate_upper;
366 }
367 }
368
predict_doppler_shift(const struct predict_observation * obs,double frequency)369 double predict_doppler_shift(const struct predict_observation *obs, double frequency)
370 {
371 double sat_range_rate = obs->range_rate*1000.0; //convert to m/s
372 return -frequency*sat_range_rate/SPEED_OF_LIGHT; //assumes that sat_range <<<<< speed of light, which is very ok
373 }
374