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
*
* See http : //www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
* \ author Javier Arribas , 2013. jarribas ( at ) cttc . es
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2015-01-08 18:49:59 +00:00
* Copyright ( C ) 2010 - 2015 ( 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
* along with GNSS - SDR . If not , see < http : //www.gnu.org/licenses/>.
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
# include "gps_ephemeris.h"
2016-01-10 13:30:04 +00:00
# include <cmath>
# include "GPS_L1_CA.h"
2013-03-11 18:29:33 +00:00
Gps_Ephemeris : : Gps_Ephemeris ( )
{
2013-07-04 13:47:40 +00:00
i_satellite_PRN = 0 ;
d_TOW = 0 ;
d_Crs = 0 ;
d_Delta_n = 0 ;
d_M_0 = 0 ;
d_Cuc = 0 ;
d_e_eccentricity = 0 ;
d_Cus = 0 ;
d_sqrt_A = 0 ;
d_Toe = 0 ;
d_Toc = 0 ;
d_Cic = 0 ;
d_OMEGA0 = 0 ;
d_Cis = 0 ;
d_i_0 = 0 ;
d_Crc = 0 ;
d_OMEGA = 0 ;
d_OMEGA_DOT = 0 ;
d_IDOT = 0 ;
i_code_on_L2 = 0 ;
i_GPS_week = 0 ;
b_L2_P_data_flag = false ;
i_SV_accuracy = 0 ;
i_SV_health = 0 ;
2015-02-18 00:08:19 +00:00
d_IODE_SF2 = 0 ;
d_IODE_SF3 = 0 ;
d_TGD = 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
2015-02-18 00:08:19 +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.
2013-07-04 13:47:40 +00:00
d_spare1 = 0 ;
d_spare2 = 0 ;
2015-02-18 00:08:19 +00:00
d_A_f0 = 0 ; // Coefficient 0 of code phase offset model [s]
d_A_f1 = 0 ; // Coefficient 1 of code phase offset model [s/s]
d_A_f2 = 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.
b_antispoofing_flag = false ; // If true, the AntiSpoofing mode is ON in that SV
2013-07-04 13:47:40 +00:00
//Plane A (info from http://www.navcen.uscg.gov/?Do=constellationStatus)
satelliteBlock [ 9 ] = " IIA " ;
satelliteBlock [ 31 ] = " IIR-M " ;
satelliteBlock [ 8 ] = " IIA " ;
satelliteBlock [ 7 ] = " IIR-M " ;
satelliteBlock [ 27 ] = " IIA " ;
//Plane B
satelliteBlock [ 16 ] = " IIR " ;
satelliteBlock [ 25 ] = " IIF " ;
satelliteBlock [ 28 ] = " IIR " ;
satelliteBlock [ 12 ] = " IIR-M " ;
satelliteBlock [ 30 ] = " IIA " ;
//Plane C
satelliteBlock [ 29 ] = " IIR-M " ;
satelliteBlock [ 3 ] = " IIA " ;
satelliteBlock [ 19 ] = " IIR " ;
satelliteBlock [ 17 ] = " IIR-M " ;
satelliteBlock [ 6 ] = " IIA " ;
//Plane D
satelliteBlock [ 2 ] = " IIR " ;
satelliteBlock [ 1 ] = " IIF " ;
satelliteBlock [ 21 ] = " IIR " ;
satelliteBlock [ 4 ] = " IIA " ;
satelliteBlock [ 11 ] = " IIR " ;
satelliteBlock [ 24 ] = " IIA " ; // Decommissioned from active service on 04 Nov 2011
//Plane E
satelliteBlock [ 20 ] = " IIR " ;
satelliteBlock [ 22 ] = " IIR " ;
satelliteBlock [ 5 ] = " IIR-M " ;
satelliteBlock [ 18 ] = " IIR " ;
satelliteBlock [ 32 ] = " IIA " ;
satelliteBlock [ 10 ] = " IIA " ;
//Plane F
satelliteBlock [ 14 ] = " IIR " ;
satelliteBlock [ 15 ] = " IIR-M " ;
satelliteBlock [ 13 ] = " IIR " ;
satelliteBlock [ 23 ] = " IIR " ;
satelliteBlock [ 26 ] = " IIA " ;
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 ;
2015-05-13 14:40:46 +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
{
2013-07-04 13:47:40 +00:00
double dt ;
dt = check_t ( transmitTime - d_Toc ) ;
2014-08-28 06:11:32 +00:00
d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * ( dt * dt ) + sv_clock_relativistic_term ( transmitTime ) ;
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
2015-05-13 14:40:46 +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 ----------------------------
2015-05-13 14:40:46 +00:00
for ( int ii = 1 ; ii < 20 ; ii + + )
2013-07-04 13:47:40 +00:00
{
E_old = E ;
E = M + d_e_eccentricity * sin ( E ) ;
2015-05-13 14:40:46 +00:00
dE = fmod ( E - E_old , 2.0 * GPS_PI ) ;
2013-07-04 13:47:40 +00:00
if ( fabs ( dE ) < 1e-12 )
{
//Necessary precision is reached, exit from the loop
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
2013-03-14 12:52:32 +00:00
void Gps_Ephemeris : : satellitePosition ( double transmitTime )
{
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
a = d_sqrt_A * d_sqrt_A ;
// Time from ephemeris reference epoch
tk = check_t ( transmitTime - d_Toe ) ;
// Computed mean motion
n0 = sqrt ( GM / ( a * a * a ) ) ;
// Corrected mean motion
n = n0 + d_Delta_n ;
// Mean anomaly
M = d_M_0 + n * tk ;
// Reduce mean anomaly to between 0 and 2pi
M = fmod ( ( M + 2 * GPS_PI ) , ( 2 * GPS_PI ) ) ;
// Initial guess of eccentric anomaly
E = M ;
// --- Iteratively compute eccentric anomaly ----------------------------
for ( int ii = 1 ; ii < 20 ; ii + + )
{
E_old = E ;
E = M + d_e_eccentricity * sin ( E ) ;
dE = fmod ( E - E_old , 2 * GPS_PI ) ;
if ( fabs ( dE ) < 1e-12 )
{
//Necessary precision is reached, exit from the loop
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
phi = fmod ( ( phi ) , ( 2 * GPS_PI ) ) ;
// Correct argument of latitude
u = phi + d_Cuc * cos ( 2 * phi ) + d_Cus * sin ( 2 * phi ) ;
// Correct radius
r = a * ( 1 - d_e_eccentricity * cos ( E ) ) + d_Crc * cos ( 2 * phi ) + d_Crs * sin ( 2 * phi ) ;
// Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos ( 2 * phi ) + d_Cis * sin ( 2 * phi ) ;
// Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + ( d_OMEGA_DOT - OMEGA_EARTH_DOT ) * tk - OMEGA_EARTH_DOT * d_Toe ;
// Reduce to between 0 and 2*pi rad
Omega = fmod ( ( Omega + 2 * GPS_PI ) , ( 2 * GPS_PI ) ) ;
// --- 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 ;
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 ) ;
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 ) ;
2013-03-14 12:52:32 +00:00
}