mirror of https://github.com/gnss-sdr/gnss-sdr
271 lines
10 KiB
C++
271 lines
10 KiB
C++
/*!
|
||
* \file galileo_ephemeris.cc
|
||
* \brief Interface of a GPS EPHEMERIS storage and orbital model functions
|
||
*
|
||
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
|
||
* \author Javier Arribas, 2013. jarribas(at)cttc.es
|
||
* \author Mara Branzanti 2013. mara.branzanti(at)gmail.com
|
||
* -------------------------------------------------------------------------
|
||
*
|
||
* Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
|
||
*
|
||
* 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
|
||
* at your option) any later version.
|
||
*
|
||
* 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 "galileo_ephemeris.h"
|
||
|
||
|
||
Galileo_Ephemeris::Galileo_Ephemeris()
|
||
{
|
||
flag_all_ephemeris = false;
|
||
IOD_ephemeris = 0;
|
||
IOD_nav_1 = 0;
|
||
|
||
SV_ID_PRN_4 = 0;
|
||
M0_1 = 0; // Mean anomaly at reference time [semi-circles]
|
||
delta_n_3 = 0; // Mean motion difference from computed value [semi-circles/sec]
|
||
e_1 = 0; // Eccentricity
|
||
A_1 = 0; // Square root of the semi-major axis [metres^1/2]
|
||
OMEGA_0_2 = 0; // Longitude of ascending node of orbital plane at weekly epoch [semi-circles]
|
||
i_0_2 = 0; // Inclination angle at reference time [semi-circles]
|
||
omega_2 = 0; // Argument of perigee [semi-circles]
|
||
OMEGA_dot_3 = 0; // Rate of right ascension [semi-circles/sec]
|
||
iDot_2 = 0; // Rate of inclination angle [semi-circles/sec]
|
||
C_uc_3 = 0; // Amplitude of the cosine harmonic correction term to the argument of latitude [radians]
|
||
C_us_3 = 0; // Amplitude of the sine harmonic correction term to the argument of latitude [radians]
|
||
C_rc_3 = 0; // Amplitude of the cosine harmonic correction term to the orbit radius [meters]
|
||
C_rs_3 = 0; // Amplitude of the sine harmonic correction term to the orbit radius [meters]
|
||
C_ic_4 = 0; // Amplitude of the cosine harmonic correction term to the angle of inclination [radians]
|
||
C_is_4 = 0; // Amplitude of the sine harmonic correction term to the angle of inclination [radians]
|
||
t0e_1 = 0; // Ephemeris reference time [s]
|
||
|
||
/*Clock correction parameters*/
|
||
t0c_4 = 0; // Clock correction data reference Time of Week [sec]
|
||
af0_4 = 0; // SV clock bias correction coefficient [s]
|
||
af1_4 = 0; // SV clock drift correction coefficient [s/s]
|
||
af2_4 = 0; //SV clock drift rate correction coefficient [s/s^2]
|
||
|
||
/*GST*/
|
||
WN_5 = 0;
|
||
TOW_5 = 0;
|
||
|
||
}
|
||
|
||
|
||
double Galileo_Ephemeris::Galileo_System_Time(double WN, double TOW){
|
||
/* GALIELO SYSTEM TIME, ICD 5.1.2
|
||
* input parameter:
|
||
* WN: The Week Number is an integer counter that gives the sequential week number
|
||
from the origin of the Galileo time. It covers 4096 weeks (about 78 years).
|
||
Then the counter is reset to zero to cover additional period modulo 4096
|
||
|
||
TOW: The Time of Week is defined as the number of seconds that have occurred since
|
||
the transition from the previous week. The TOW covers an entire week from 0 to
|
||
604799 seconds and is reset to zero at the end of each week
|
||
|
||
WN and TOW are received in page 5
|
||
|
||
output:
|
||
t: it is the transmitted time in Galileo System Time (expressed in seconds)
|
||
|
||
The GST start epoch shall be 00:00 UT on Sunday 22nd August 1999 (midnight between 21st and 22nd August).
|
||
At the start epoch, GST shall be ahead of UTC by thirteen (13)
|
||
leap seconds. Since the next leap second was inserted at 01.01.2006, this implies that
|
||
as of 01.01.2006 GST is ahead of UTC by fourteen (14) leap seconds.
|
||
|
||
The epoch denoted in the navigation messages by TOW and WN
|
||
will be measured relative to the leading edge of the first chip of the
|
||
first code sequence of the first page symbol. The transmission timing of the navigation
|
||
message provided through the TOW is synchronised to each satellite’s version of Galileo System Time (GST).
|
||
*
|
||
*/
|
||
double t = 0;
|
||
double sec_in_day = 86400;
|
||
double day_in_week = 7;
|
||
t = WN*sec_in_day*day_in_week + TOW; // second from the origin of the Galileo time
|
||
|
||
return t;
|
||
}
|
||
|
||
|
||
|
||
double Galileo_Ephemeris::sv_clock_drift(double transmitTime){
|
||
/* Satellite Time Correction Algorithm, ICD 5.1.4
|
||
*
|
||
*/
|
||
double dt;
|
||
dt = transmitTime - t0c_4;
|
||
Galileo_satClkDrift = af0_4 + af1_4*dt + (af2_4 * dt)*(af2_4 * dt) + Galileo_dtr;
|
||
return Galileo_satClkDrift;
|
||
}
|
||
|
||
// compute the relativistic correction term
|
||
double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) //Satellite Time Correction Algorithm, ICD 5.1.4
|
||
{
|
||
double tk;
|
||
double a;
|
||
double n;
|
||
double n0;
|
||
double E;
|
||
double E_old;
|
||
double dE;
|
||
double M;
|
||
|
||
// Restore semi-major axis
|
||
a = A_1*A_1;
|
||
|
||
n0 = sqrt(GALILEO_GM / (a*a*a));
|
||
|
||
// Time from ephemeris reference epoch
|
||
//tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
|
||
//t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
|
||
tk = transmitTime - t0e_1;
|
||
|
||
// Corrected mean motion
|
||
n = n0 + delta_n_3;
|
||
|
||
// Mean anomaly
|
||
M = M0_1 + n * tk;
|
||
|
||
// Reduce mean anomaly to between 0 and 2pi
|
||
M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
|
||
|
||
// Initial guess of eccentric anomaly
|
||
E = M;
|
||
|
||
// --- Iteratively compute eccentric anomaly ----------------------------
|
||
for (int ii = 1; ii<20; ii++)
|
||
{
|
||
E_old = E;
|
||
E = M + e_1 * sin(E);
|
||
dE = fmod(E - E_old, 2*GALILEO_PI);
|
||
if (fabs(dE) < 1e-12)
|
||
{
|
||
//Necessary precision is reached, exit from the loop
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
// Compute relativistic correction term
|
||
Galileo_dtr = GALILEO_F * e_1* A_1 * sin(E);
|
||
return Galileo_dtr;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
void Galileo_Ephemeris::satellitePosition(double transmitTime) //when this function in used, the input must be the transmitted time (t) in second computed by Galileo_System_Time (above function)
|
||
{
|
||
double tk; // Time from ephemeris reference epoch
|
||
//double t; // Galileo System Time (ICD, paragraph 5.1.2)
|
||
double a; // Semi-major axis
|
||
double n; // Corrected mean motion
|
||
double n0; // Computed mean motion
|
||
double M; // Mean anomaly
|
||
double E; //Eccentric Anomaly (to be solved by iteration)
|
||
double E_old;
|
||
double dE;
|
||
double nu; //True anomaly
|
||
double phi; //argument of Latitude
|
||
double u; // Correct argument of latitude
|
||
double r; // Correct radius
|
||
double i;
|
||
double Omega;
|
||
|
||
// Find Galileo satellite's position ----------------------------------------------
|
||
|
||
// Restore semi-major axis
|
||
a = A_1*A_1;
|
||
|
||
// Computed mean motion
|
||
n0 = sqrt(GALILEO_GM / (a*a*a));
|
||
|
||
// Time from ephemeris reference epoch
|
||
//tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
|
||
//t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
|
||
tk = transmitTime - t0e_1;
|
||
|
||
// Corrected mean motion
|
||
n = n0 + delta_n_3;
|
||
|
||
// Mean anomaly
|
||
M = M0_1 + n * tk;
|
||
|
||
// Reduce mean anomaly to between 0 and 2pi
|
||
M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
|
||
|
||
// Initial guess of eccentric anomaly
|
||
E = M;
|
||
|
||
// --- Iteratively compute eccentric anomaly ----------------------------
|
||
for (int ii = 1; ii<20; ii++)
|
||
{
|
||
E_old = E;
|
||
E = M + e_1 * sin(E);
|
||
dE = fmod(E - E_old, 2*GALILEO_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 - e_1 * e_1) * sin(E);
|
||
double tmp_X = cos(E) - e_1;
|
||
nu = atan2(tmp_Y, tmp_X);
|
||
|
||
// Compute angle phi (argument of Latitude)
|
||
phi = nu + omega_2;
|
||
|
||
// Reduce phi to between 0 and 2*pi rad
|
||
phi = fmod((phi), (2*GALILEO_PI));
|
||
|
||
// Correct argument of latitude
|
||
u = phi + C_uc_3 * cos(2*phi) + C_us_3 * sin(2*phi);
|
||
|
||
// Correct radius
|
||
r = a * (1 - e_1*cos(E)) + C_rc_3 * cos(2*phi) + C_rs_3 * sin(2*phi);
|
||
|
||
// Correct inclination
|
||
i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2*phi) + C_is_4 * sin(2*phi);
|
||
|
||
// Compute the angle between the ascending node and the Greenwich meridian
|
||
Omega = OMEGA_0_2 + (OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT)*tk - GALILEO_OMEGA_EARTH_DOT * t0e_1;
|
||
|
||
// Reduce to between 0 and 2*pi rad
|
||
Omega = fmod((Omega + 2*GALILEO_PI), (2*GALILEO_PI));
|
||
|
||
// --- Compute satellite coordinates in Earth-fixed coordinates
|
||
galileo_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
|
||
galileo_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); //***********************NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus
|
||
galileo_satpos_Z = sin(u) * r * sin(i);
|
||
|
||
// Satellite's velocity. Can be useful for Vector Tracking loops
|
||
double Omega_dot = OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT;
|
||
galileo_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + galileo_satpos_X * cos(Omega) - galileo_satpos_Y * cos(i) * sin(Omega);
|
||
galileo_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + galileo_satpos_X * sin(Omega) + galileo_satpos_Y * cos(i) * cos(Omega);
|
||
galileo_satvel_Z = galileo_satpos_Y * sin(i);
|
||
}
|