2013-03-11 18:29:33 +00:00
/*!
* \ file gps_ephemeris . cc
2013-03-14 12:52:32 +00:00
* \ brief Interface of a GPS EPHEMERIS storage and orbital model functions
2013-03-11 18:29:33 +00:00
*
2020-01-25 12:07:03 +00:00
* See https : //www.gps.gov/technical/icwg/IS-GPS-200K.pdf Appendix II
2013-03-11 18:29:33 +00:00
* \ author Javier Arribas , 2013. jarribas ( at ) cttc . es
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2019-07-26 10:38:20 +00:00
* Copyright ( C ) 2010 - 2019 ( see AUTHORS file for a list of contributors )
2013-03-11 18:29:33 +00:00
*
* GNSS - SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS - SDR .
*
* GNSS - SDR is free software : you can redistribute it and / or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation , either version 3 of the License , or
2015-01-08 18:49:59 +00:00
* ( at your option ) any later version .
2013-03-11 18:29:33 +00:00
*
* GNSS - SDR is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
2018-05-13 20:49:11 +00:00
* along with GNSS - SDR . If not , see < https : //www.gnu.org/licenses/>.
2013-03-11 18:29:33 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
# include "gps_ephemeris.h"
2018-02-26 02:15:53 +00:00
# include "GPS_L1_CA.h"
2018-12-02 13:32:22 +00:00
# include "gnss_satellite.h"
2018-02-26 02:15:53 +00:00
# include <cmath>
2013-03-11 18:29:33 +00:00
Gps_Ephemeris : : Gps_Ephemeris ( )
{
2018-08-13 23:13:07 +00:00
i_satellite_PRN = 0U ;
2018-12-14 01:31:01 +00:00
d_TOW = 0 ;
2018-08-13 23:13:07 +00:00
d_Crs = 0.0 ;
d_Delta_n = 0.0 ;
d_M_0 = 0.0 ;
d_Cuc = 0.0 ;
d_e_eccentricity = 0.0 ;
d_Cus = 0.0 ;
d_sqrt_A = 0.0 ;
2018-12-14 01:31:01 +00:00
d_Toe = 0 ;
d_Toc = 0 ;
2018-08-13 23:13:07 +00:00
d_Cic = 0.0 ;
d_OMEGA0 = 0.0 ;
d_Cis = 0.0 ;
d_i_0 = 0.0 ;
d_Crc = 0.0 ;
d_OMEGA = 0.0 ;
d_OMEGA_DOT = 0.0 ;
d_IDOT = 0.0 ;
2013-07-04 13:47:40 +00:00
i_code_on_L2 = 0 ;
i_GPS_week = 0 ;
b_L2_P_data_flag = false ;
i_SV_accuracy = 0 ;
i_SV_health = 0 ;
2018-12-14 01:31:01 +00:00
d_IODE_SF2 = 0 ;
d_IODE_SF3 = 0 ;
d_TGD = 0.0 ; // Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
d_IODC = 0 ; // Issue of Data, Clock
i_AODO = 0 ; // Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
2013-07-04 13:47:40 +00:00
2018-03-03 01:03:39 +00:00
b_fit_interval_flag = false ; // indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
2018-08-13 23:13:07 +00:00
d_spare1 = 0.0 ;
d_spare2 = 0.0 ;
2013-07-04 13:47:40 +00:00
2018-08-13 23:13:07 +00:00
d_A_f0 = 0.0 ; // Coefficient 0 of code phase offset model [s]
d_A_f1 = 0.0 ; // Coefficient 1 of code phase offset model [s/s]
d_A_f2 = 0.0 ; // Coefficient 2 of code phase offset model [s/s^2]
2013-07-04 13:47:40 +00:00
b_integrity_status_flag = false ;
2015-02-18 00:08:19 +00:00
b_alert_flag = false ; // If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
2019-08-18 20:16:13 +00:00
b_antispoofing_flag = false ; // If true, the AntiSpoofing mode is ON in that SV
2013-07-04 13:47:40 +00:00
2016-05-02 10:06:23 +00:00
auto gnss_sat = Gnss_Satellite ( ) ;
2018-03-03 01:03:39 +00:00
std : : string _system ( " GPS " ) ;
2018-08-13 23:13:07 +00:00
for ( uint32_t i = 1 ; i < 33 ; i + + )
2016-05-02 10:06:23 +00:00
{
satelliteBlock [ i ] = gnss_sat . what_block ( _system , i ) ;
}
2015-05-13 14:40:46 +00:00
d_satClkDrift = 0.0 ;
d_dtr = 0.0 ;
d_satpos_X = 0.0 ;
d_satpos_Y = 0.0 ;
d_satpos_Z = 0.0 ;
d_satvel_X = 0.0 ;
d_satvel_Y = 0.0 ;
d_satvel_Z = 0.0 ;
2013-03-14 12:52:32 +00:00
}
double Gps_Ephemeris : : check_t ( double time )
{
2013-07-04 13:47:40 +00:00
double corrTime ;
2018-03-03 01:03:39 +00:00
double half_week = 302400.0 ; // seconds
2013-07-04 13:47:40 +00:00
corrTime = time ;
if ( time > half_week )
{
2015-05-13 14:40:46 +00:00
corrTime = time - 2.0 * half_week ;
2013-07-04 13:47:40 +00:00
}
else if ( time < - half_week )
{
2015-05-13 14:40:46 +00:00
corrTime = time + 2.0 * half_week ;
2013-07-04 13:47:40 +00:00
}
return corrTime ;
2013-03-14 12:52:32 +00:00
}
// 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
2013-06-12 15:19:32 +00:00
double Gps_Ephemeris : : sv_clock_drift ( double transmitTime )
2013-03-14 12:52:32 +00:00
{
2018-03-03 01:03:39 +00:00
// double dt;
// dt = check_t(transmitTime - d_Toc);
//
2018-08-13 23:13:07 +00:00
// for (int32_t i = 0; i < 2; i++)
2018-03-03 01:03:39 +00:00
// {
// dt -= d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt);
// }
// d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt);
2017-04-12 15:04:51 +00:00
2013-07-04 13:47:40 +00:00
double dt ;
dt = check_t ( transmitTime - d_Toc ) ;
2017-04-12 15:04:51 +00:00
d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * ( dt * dt ) + sv_clock_relativistic_term ( transmitTime ) ;
2019-08-18 20:16:13 +00:00
// Correct satellite group delay
2018-03-03 01:03:39 +00:00
d_satClkDrift - = d_TGD ;
2017-01-30 18:03:18 +00:00
2013-07-04 13:47:40 +00:00
return d_satClkDrift ;
2013-03-11 18:29:33 +00:00
}
2013-07-04 13:47:40 +00:00
2013-11-23 13:05:38 +00:00
2013-06-12 15:19:32 +00:00
// compute the relativistic correction term
double Gps_Ephemeris : : sv_clock_relativistic_term ( double transmitTime )
{
2013-07-04 13:47:40 +00:00
double tk ;
double a ;
double n ;
double n0 ;
double E ;
double E_old ;
double dE ;
double M ;
// Restore semi-major axis
2015-05-13 14:40:46 +00:00
a = d_sqrt_A * d_sqrt_A ;
2013-07-04 13:47:40 +00:00
// Time from ephemeris reference epoch
tk = check_t ( transmitTime - d_Toe ) ;
// Computed mean motion
2015-05-13 14:40:46 +00:00
n0 = sqrt ( GM / ( a * a * a ) ) ;
2013-07-04 13:47:40 +00:00
// Corrected mean motion
n = n0 + d_Delta_n ;
// Mean anomaly
M = d_M_0 + n * tk ;
// Reduce mean anomaly to between 0 and 2pi
2019-08-18 20:16:13 +00:00
// M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
2013-07-04 13:47:40 +00:00
// Initial guess of eccentric anomaly
E = M ;
// --- Iteratively compute eccentric anomaly ----------------------------
2018-08-13 23:13:07 +00:00
for ( int32_t ii = 1 ; ii < 20 ; ii + + )
2013-07-04 13:47:40 +00:00
{
2018-03-03 01:03:39 +00:00
E_old = E ;
E = M + d_e_eccentricity * sin ( E ) ;
dE = fmod ( E - E_old , 2.0 * GPS_PI ) ;
2013-07-04 13:47:40 +00:00
if ( fabs ( dE ) < 1e-12 )
{
2019-08-18 20:16:13 +00:00
// Necessary precision is reached, exit from the loop
2013-07-04 13:47:40 +00:00
break ;
}
}
// Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin ( E ) ;
return d_dtr ;
2013-06-12 15:19:32 +00:00
}
2013-03-14 12:52:32 +00:00
2013-07-04 13:47:40 +00:00
2017-01-30 18:03:18 +00:00
double Gps_Ephemeris : : satellitePosition ( double transmitTime )
2013-03-14 12:52:32 +00:00
{
2013-07-04 13:47:40 +00:00
double tk ;
double a ;
double n ;
double n0 ;
double M ;
double E ;
double E_old ;
double dE ;
double nu ;
double phi ;
double u ;
double r ;
double i ;
double Omega ;
// Find satellite's position ----------------------------------------------
// Restore semi-major axis
2017-01-30 19:26:50 +00:00
a = d_sqrt_A * d_sqrt_A ;
2013-07-04 13:47:40 +00:00
// Time from ephemeris reference epoch
tk = check_t ( transmitTime - d_Toe ) ;
// Computed mean motion
2017-01-30 19:26:50 +00:00
n0 = sqrt ( GM / ( a * a * a ) ) ;
2013-07-04 13:47:40 +00:00
// Corrected mean motion
n = n0 + d_Delta_n ;
// Mean anomaly
M = d_M_0 + n * tk ;
// Reduce mean anomaly to between 0 and 2pi
2019-08-18 20:16:13 +00:00
// M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
2013-07-04 13:47:40 +00:00
// Initial guess of eccentric anomaly
E = M ;
// --- Iteratively compute eccentric anomaly ----------------------------
2018-08-13 23:13:07 +00:00
for ( int32_t ii = 1 ; ii < 20 ; ii + + )
2013-07-04 13:47:40 +00:00
{
2018-03-03 01:03:39 +00:00
E_old = E ;
E = M + d_e_eccentricity * sin ( E ) ;
dE = fmod ( E - E_old , 2.0 * GPS_PI ) ;
2013-07-04 13:47:40 +00:00
if ( fabs ( dE ) < 1e-12 )
{
2019-08-18 20:16:13 +00:00
// Necessary precision is reached, exit from the loop
2013-07-04 13:47:40 +00:00
break ;
}
}
// Compute the true anomaly
double tmp_Y = sqrt ( 1.0 - d_e_eccentricity * d_e_eccentricity ) * sin ( E ) ;
double tmp_X = cos ( E ) - d_e_eccentricity ;
nu = atan2 ( tmp_Y , tmp_X ) ;
// Compute angle phi (argument of Latitude)
phi = nu + d_OMEGA ;
// Reduce phi to between 0 and 2*pi rad
2019-08-18 20:16:13 +00:00
// phi = fmod((phi), (2.0 * GPS_PI));
2013-07-04 13:47:40 +00:00
// Correct argument of latitude
2018-03-03 01:03:39 +00:00
u = phi + d_Cuc * cos ( 2.0 * phi ) + d_Cus * sin ( 2.0 * phi ) ;
2013-07-04 13:47:40 +00:00
// Correct radius
2018-03-03 01:03:39 +00:00
r = a * ( 1.0 - d_e_eccentricity * cos ( E ) ) + d_Crc * cos ( 2.0 * phi ) + d_Crs * sin ( 2.0 * phi ) ;
2013-07-04 13:47:40 +00:00
// Correct inclination
2017-01-30 19:26:50 +00:00
i = d_i_0 + d_IDOT * tk + d_Cic * cos ( 2.0 * phi ) + d_Cis * sin ( 2.0 * phi ) ;
2013-07-04 13:47:40 +00:00
// Compute the angle between the ascending node and the Greenwich meridian
2018-03-03 01:03:39 +00:00
Omega = d_OMEGA0 + ( d_OMEGA_DOT - OMEGA_EARTH_DOT ) * tk - OMEGA_EARTH_DOT * d_Toe ;
2013-07-04 13:47:40 +00:00
// Reduce to between 0 and 2*pi rad
2019-08-18 20:16:13 +00:00
// Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI));
2013-07-04 13:47:40 +00:00
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos ( u ) * r * cos ( Omega ) - sin ( u ) * r * cos ( i ) * sin ( Omega ) ;
d_satpos_Y = cos ( u ) * r * sin ( Omega ) + sin ( u ) * r * cos ( i ) * cos ( Omega ) ;
d_satpos_Z = sin ( u ) * r * sin ( i ) ;
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT ;
2018-03-03 01:03:39 +00:00
d_satvel_X = - Omega_dot * ( cos ( u ) * r + sin ( u ) * r * cos ( i ) ) + d_satpos_X * cos ( Omega ) - d_satpos_Y * cos ( i ) * sin ( Omega ) ;
2013-07-04 13:47:40 +00:00
d_satvel_Y = Omega_dot * ( cos ( u ) * r * cos ( Omega ) - sin ( u ) * r * cos ( i ) * sin ( Omega ) ) + d_satpos_X * sin ( Omega ) + d_satpos_Y * cos ( i ) * cos ( Omega ) ;
d_satvel_Z = d_satpos_Y * sin ( i ) ;
2017-01-30 18:03:18 +00:00
// Time from ephemeris reference clock
tk = check_t ( transmitTime - d_Toc ) ;
2017-01-30 19:26:50 +00:00
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk ;
2017-01-30 18:03:18 +00:00
/* relativity correction */
2019-02-22 09:47:24 +00:00
dtr_s - = 2.0 * sqrt ( GM * a ) * d_e_eccentricity * sin ( E ) / ( GPS_C_M_S * GPS_C_M_S ) ;
2017-01-30 18:03:18 +00:00
return dtr_s ;
2013-03-14 12:52:32 +00:00
}