forked from swift-nav/libswiftnav-legacy
/
almanac.c
executable file
·172 lines (147 loc) · 6.21 KB
/
almanac.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include <math.h>
#include "pvt.h"
#include "linear_algebra.h"
#include "coord_system.h"
#include "almanac.h"
/** \defgroup almanac Almanac
* Functions and calculations related to the GPS almanac.
*
* \note All positions are referenced to the WGS84 coordinate system.
* \see coord_system
* \{ */
/** Calculate the position / velocity state of a satellite from the almanac.
*
* \param alm Pointer to an almanac structure for the satellite of interest.
* \param t GPS time of week at which to calculate the satellite state in
* seconds since Sunday.
* \param week GPS week number modulo 1024 or pass -1 to assume within one
* half-week of the almanac time of applicability.
* \param pos The satellite position in ECEF coordinates is returned in this
* vector.
* \param vel The satellite velocity in ECEF coordinates is returned in this
* vector. Ignored if NULL.
*/
void calc_sat_state_almanac(almanac_t* alm, double t, s16 week,
double pos[3], double vel[3])
{
/* Seconds since the almanac reference epoch. */
double dt = t - alm->toa;
if (week < 0) {
/* Week number unknown, correct time for beginning or end of week
* crossovers and limit to +/- 302400 (i.e. assume dt is within a
* half-week). */
if (dt > 302400)
dt -= 604800;
else if (dt < -302400)
dt += 604800;
} else {
/* Week number specified, correct time using week number difference. */
s32 dweeks = week - alm->week;
dt += dweeks * 604800;
}
/* Calculate position and velocity per ICD-GPS-200D Table 20-IV. */
/* Calculate mean motion in radians/sec. */
double ma_dot = sqrt (NAV_GM / (alm->a * alm->a * alm->a));
/* Calculate corrected mean anomaly in radians. */
double ma = alm->ma + ma_dot * dt;
/* Iteratively solve for the Eccentric Anomaly
* (from Keith Alter and David Johnston). */
double ea = ma; /* Starting value for E. */
double ea_old;
double temp;
do {
ea_old = ea;
temp = 1.0 - alm->ecc * cos(ea_old);
ea = ea + (ma - ea_old + alm->ecc * sin(ea_old)) / temp;
} while (fabs(ea - ea_old) > 1.0e-14);
double ea_dot = ma_dot / temp;
/* Begin calculation for True Anomaly and Argument of Latitude. */
double temp2 = sqrt(1.0 - alm->ecc * alm->ecc);
/* Argument of Latitude = True Anomaly + Argument of Perigee. */
double al = atan2(temp2 * sin(ea), cos(ea) - alm->ecc) + alm->argp;
double al_dot = temp2 * ea_dot / temp;
/* Calculate corrected radius based on argument of latitude. */
double r = alm->a * temp;
double r_dot = alm->a * alm->ecc * sin(ea) * ea_dot;
/* Calculate position and velocity in orbital plane. */
double x = r * cos(al);
double y = r * sin(al);
double x_dot = r_dot * cos(al) - y * al_dot;
double y_dot = r_dot * sin(al) + x * al_dot;
/* Corrected longitude of ascending node. */
double om_dot = alm->rora - NAV_OMEGAE_DOT;
double om = alm->raaw + dt * om_dot - NAV_OMEGAE_DOT * alm->toa;
/* Compute the satellite's position in Earth-Centered Earth-Fixed
* coordinates. */
pos[0] = x * cos(om) - y * cos(alm->inc) * sin(om);
pos[1] = x * sin(om) + y * cos(alm->inc) * cos(om);
pos[2] = y * sin(alm->inc);
/* Compute the satellite's velocity in Earth-Centered Earth-Fixed
* coordinates. */
if (vel) {
temp = y_dot * cos(alm->inc);
vel[0] = -om_dot * pos[1] + x_dot * cos(om) - temp * sin(om);
vel[1] = om_dot * pos[0] + x_dot * sin(om) + temp * cos(om);
vel[2] = y_dot * sin(alm->inc);
}
}
/** Calculate the azimuth and elevation of a satellite from a reference
* position given the satellite almanac.
*
* \param alm Pointer to an almanac structure for the satellite of interest.
* \param t GPS time of week at which to calculate the az/el.
* \param week GPS week number modulo 1024 or pass -1 to assume within one
* half-week of the almanac time of applicability.
* \param ref ECEF coordinates of the reference point from which the azimuth
* and elevation is to be determined, passed as [X, Y, Z], all in
* meters.
* \param az Pointer to where to store the calculated azimuth output.
* \param el Pointer to where to store the calculated elevation output.
*/
void calc_sat_az_el_almanac(almanac_t* alm, double t, s16 week,
double ref[3], double* az, double* el)
{
double sat_pos[3];
calc_sat_state_almanac(alm, t, week, sat_pos, 0);
wgsecef2azel(sat_pos, ref, az, el);
}
/** Calculate the Doppler shift of a satellite as observed at a reference
* position given the satellite almanac.
*
* \param alm Pointer to an almanac structure for the satellite of interest.
* \param t GPS time of week at which to calculate the Doppler shift.
* \param week GPS week number modulo 1024 or pass -1 to assume within one
* half-week of the almanac time of applicability.
* \param ref ECEF coordinates of the reference point from which the azimuth
* and elevation is to be determined, passed as [X, Y, Z], all in
* meters.
* \return The Doppler shift in Hz.
*/
double calc_sat_doppler_almanac(almanac_t* alm, double t, s16 week,
double ref[3])
{
double sat_pos[3];
double sat_vel[3];
double vec_ref_sat[3];
calc_sat_state_almanac(alm, t, week, sat_pos, sat_vel);
/* Find the vector from the reference position to the satellite. */
vector_subtract(3, sat_pos, ref, vec_ref_sat);
/* Find the satellite velocity projected on the line of sight vector from the
* reference position to the satellite. */
double radial_velocity = vector_dot(3, vec_ref_sat, sat_vel) / \
vector_norm(3, vec_ref_sat);
/* Return the Doppler shift. */
return GPS_L1_HZ * radial_velocity / NAV_C;
}
/** \} */