2424
2525float decode_ura_index (const u8 index );
2626u32 decode_fit_interval (u8 fit_interval_flag , u16 iodc );
27+ /* maximum step length in seconds for Runge-Kutta aglorithm */
28+ #define GLO_MAX_STEP_LENGTH 30
2729
2830/** \defgroup ephemeris Ephemeris
2931 * Functions and calculations related to the GPS ephemeris.
@@ -74,6 +76,138 @@ static s8 calc_sat_state_xyz(const ephemeris_t *e, const gps_time_t *t,
7476 return 0 ;
7577}
7678
79+ /** Re-calculation of ephemeris within the interval of measurement
80+ * ICD 5.1: A.3.1.2, with corrections from RTCM 3.2 p.186
81+ *
82+ * \param ydot Pointer to output array
83+ * \param pos Pointer to position input array
84+ * \param vel Pointer to velocity input array
85+ * \param acc Pointer to acceleration input array
86+ * \param e Pointer to GLO ephemeris
87+ */
88+ static void calc_ydot (double ydot [6 ],
89+ const double pos [3 ],
90+ const double vel [3 ],
91+ const double acc [3 ])
92+ {
93+
94+ double r = sqrt (pow (pos [0 ], 2 ) +
95+ pow (pos [1 ], 2 ) +
96+ pow (pos [2 ], 2 ));
97+
98+ double m_r3 = GLO_GM / pow (r , 3 );
99+
100+ double g_term = 3.0 /2.0 * GLO_J02 * GLO_GM *
101+ pow (GLO_A_E , 2 ) / pow (r , 5 );
102+
103+ double lg_term = (1.0 - 5.0 * pow (pos [2 ], 2 ) / pow (r , 2 ));
104+
105+ double omega_sqr = pow (GPS_OMEGAE_DOT , 2 );
106+
107+ ydot [0 ] = vel [0 ];
108+ ydot [1 ] = vel [1 ];
109+ ydot [2 ] = vel [2 ];
110+
111+ ydot [3 ] = - m_r3 * pos [0 ]
112+ - g_term * pos [0 ] * lg_term
113+ + omega_sqr * pos [0 ]
114+ + 2.0 * GLO_OMEGAE_DOT * vel [1 ]
115+ + acc [0 ];
116+
117+ ydot [4 ] = - m_r3 * pos [1 ]
118+ - g_term * pos [1 ] * lg_term
119+ + omega_sqr * pos [1 ]
120+ - 2.0 * GLO_OMEGAE_DOT * vel [0 ]
121+ + acc [1 ];
122+
123+
124+ ydot [5 ] = - m_r3 * pos [2 ]
125+ - g_term * pos [2 ] * (2.0 + lg_term )
126+ + acc [2 ];
127+ }
128+
129+ /** Calculate satellite position, velocity and clock offset from GLO ephemeris.
130+ *
131+ * \param e Pointer to an ephemeris structure for the satellite of interest
132+ * \param t time at which to calculate the satellite state
133+ * \param pos Array into which to write calculated satellite position [m]
134+ * \param vel Array into which to write calculated satellite velocity [m/s]
135+ * \param clock_err Pointer to where to store the calculated satellite clock
136+ * error [s]
137+ * \param clock_rate_err Pointer to where to store the calculated satellite
138+ * clock error [s/s]
139+ *
140+ * \return 0 on success,
141+ * -1 if ephemeris is not valid or too old
142+ */
143+ static s8 calc_sat_state_glo (const ephemeris_t * e , const gps_time_t * t ,
144+ double pos [3 ], double vel [3 ],
145+ double * clock_err , double * clock_rate_err )
146+ {
147+ assert (e != NULL );
148+ assert (t != NULL );
149+ assert (pos != NULL );
150+ assert (vel != NULL );
151+ assert (clock_err != NULL );
152+ assert (clock_rate_err != NULL );
153+
154+ double dt = abs (gpsdifftime (t , & e -> toe ));
155+
156+ if (dt > 900 ) {
157+ log_error ("GLO: Integration end point is not within 900 s of TOE" );
158+ return 1 ;
159+ }
160+
161+ u32 num_steps = ceil (dt / GLO_MAX_STEP_LENGTH );
162+ double h = gpsdifftime (t , & e -> toe ) / num_steps ;
163+
164+ if (num_steps ) {
165+ double ydot [6 ], y [6 ];
166+
167+ calc_ydot (ydot , e -> glo .pos , e -> glo .vel , e -> glo .acc );
168+ memcpy (& y [0 ], e -> glo .pos , sizeof (double ) * 3 );
169+ memcpy (& y [3 ], e -> glo .vel , sizeof (double ) * 3 );
170+
171+ /* Runge-Kutta integration algorithm */
172+ for (u8 i = 0 ; i < num_steps ; i ++ ) {
173+ double k1 [6 ], k2 [6 ], k3 [6 ], k4 [6 ], y_tmp [6 ];
174+ u8 j ;
175+
176+ memcpy (k1 , ydot , sizeof (k1 ));
177+
178+ for (j = 0 ; j < 6 ; j ++ )
179+ y_tmp [j ] = y [j ] + h /2 * k1 [j ];
180+
181+ calc_ydot (k2 , & y_tmp [0 ], & y_tmp [3 ], e -> glo .acc );
182+
183+ for (j = 0 ; j < 6 ; j ++ )
184+ y_tmp [j ] = y [j ] + h /2 * k2 [j ];
185+
186+ calc_ydot (k3 , & y_tmp [0 ], & y_tmp [3 ], e -> glo .acc );
187+
188+ for (j = 0 ; j < 6 ; j ++ )
189+ y_tmp [j ] = y [j ] + h * k3 [j ];
190+
191+ calc_ydot (k4 , & y_tmp [0 ], & y_tmp [3 ], e -> glo .acc );
192+
193+ for (j = 0 ; j < 6 ; j ++ )
194+ y [j ] += h /6 * (k1 [j ] + 2 * k2 [j ] + 2 * k3 [j ] + k4 [j ]);
195+
196+ calc_ydot (ydot , & y [0 ], & y [3 ], e -> glo .acc );
197+ }
198+ memcpy (pos , & y [0 ], sizeof (double ) * 3 );
199+ memcpy (vel , & y [3 ], sizeof (double ) * 3 );
200+ } else {
201+ memcpy (pos , e -> glo .pos , sizeof (double ) * 3 );
202+ memcpy (vel , e -> glo .vel , sizeof (double ) * 3 );
203+ }
204+
205+ * clock_err = e -> glo .tau + e -> glo .gamma * dt ;
206+ * clock_rate_err = e -> glo .gamma ;
207+
208+ return 0 ;
209+ }
210+
77211/** Calculate satellite position, velocity and clock offset from GPS ephemeris.
78212 *
79213 * References:
@@ -231,6 +365,8 @@ s8 calc_sat_state(const ephemeris_t *e, const gps_time_t *t,
231365 return calc_sat_state_kepler (e , t , pos , vel , clock_err , clock_rate_err );
232366 case CONSTELLATION_SBAS :
233367 return calc_sat_state_xyz (e , t , pos , vel , clock_err , clock_rate_err );
368+ case CONSTELLATION_GLO :
369+ return calc_sat_state_glo (e , t , pos , vel , clock_err , clock_rate_err );
234370 default :
235371 assert (!"Unsupported constellation" );
236372 return -1 ;
@@ -666,6 +802,14 @@ static bool ephemeris_kepler_equal(const ephemeris_kepler_t *a,
666802 (a -> toc .tow == b -> toc .tow );
667803}
668804
805+ static bool ephemeris_glo_equal (const ephemeris_glo_t * a ,
806+ const ephemeris_glo_t * b )
807+ {
808+ return (memcmp (a -> pos , b -> pos , sizeof (a -> pos )) == 0 ) &&
809+ (memcmp (a -> vel , b -> vel , sizeof (a -> vel )) == 0 ) &&
810+ (memcmp (a -> acc , b -> acc , sizeof (a -> acc )) == 0 );
811+ }
812+
669813/** Are the two ephemerides the same?
670814 *
671815 * \param a First ephemeris
@@ -688,6 +832,8 @@ bool ephemeris_equal(const ephemeris_t *a, const ephemeris_t *b)
688832 return ephemeris_kepler_equal (& a -> kepler , & b -> kepler );
689833 case CONSTELLATION_SBAS :
690834 return ephemeris_xyz_equal (& a -> xyz , & b -> xyz );
835+ case CONSTELLATION_GLO :
836+ return ephemeris_glo_equal (& a -> glo , & b -> glo );
691837 default :
692838 assert (!"Unsupported constellation" );
693839 return false;
0 commit comments