mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-10-31 23:26:22 +00:00
Code Issues Releases Wiki Activity

gnav: Adding GLONASS GNAV Navigation Message Operations

Browse Source 
Adding code for navigation message computation for GLONASS GNAV code.
Continues to fix the almanac position computation and fixes bugs in code
due to compilation errors, coding style and Doxygen documentation
This commit is contained in:
Damian Miralles 2017-06-20 21:44:44 -07:00 committed by Damian Miralles
parent f8f3574090
commit c24710c585
14 changed files with 1604 additions and 322 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -6,6 +6,7 @@ docs/latex
docs/GNSS-SDR_manual.pdf docs/GNSS-SDR_manual.pdf
src/tests/data/output.dat src/tests/data/output.dat
thirdparty/ thirdparty/
.settings
.project .project
.cproject .cproject
/install /install

5
src/core/system_parameters/CMakeLists.txt
View File

@ -39,6 +39,10 @@ set(SYSTEM_PARAMETERS_SOURCES
gps_cnav_iono.cc gps_cnav_iono.cc
gps_cnav_utc_model.cc gps_cnav_utc_model.cc
rtcm.cc rtcm.cc
glonass_gnav_ephemeris.cc
glonass_gnav_almanac.cc
glonass_gnav_utc_model.cc
glonass_gnav_navigation_message.cc
) )
@ -59,4 +63,3 @@ add_library(gnss_system_parameters ${SYSTEM_PARAMETERS_SOURCES} ${SYSTEM_PARAMET
source_group(Headers FILES ${SYSTEM_PARAMETERS_HEADERS}) source_group(Headers FILES ${SYSTEM_PARAMETERS_HEADERS})
add_dependencies(gnss_system_parameters rtklib_lib glog-${glog_RELEASE}) add_dependencies(gnss_system_parameters rtklib_lib glog-${glog_RELEASE})
target_link_libraries(gnss_system_parameters rtklib_lib ${Boost_LIBRARIES}) target_link_libraries(gnss_system_parameters rtklib_lib ${Boost_LIBRARIES})

36
src/core/system_parameters/GLONASS_L1_CA.h
View File

@ -43,7 +43,7 @@ const double GLONASS_C_m_ms = 299792.4580; //!< The spe
const double GLONASS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E const double GLONASS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
const double GLONASS_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200E const double GLONASS_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200E
const double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s] const double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s]
const double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [km^3/s^2] const double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
const double GLONASS_fM_a = 0.35e9; //!< Gravitational constant of atmosphere [m^3/s^2] const double GLONASS_fM_a = 0.35e9; //!< Gravitational constant of atmosphere [m^3/s^2]
const double GLONASS_SEMI_MAJOR_AXIS = 6378136; //!< Semi-major axis of Earth [m] const double GLONASS_SEMI_MAJOR_AXIS = 6378136; //!< Semi-major axis of Earth [m]
const double GLONASS_FLATTENING = 1/29825784; //!< Flattening parameter const double GLONASS_FLATTENING = 1/29825784; //!< Flattening parameter
@ -56,26 +56,26 @@ const double GLONASS_J8 = 1.40e-11; //!< Eighth
const double GLONASS_U0 = 62636861.4; //!< Normal potential at surface of common terrestrial ellipsoid [m^2/s^2] const double GLONASS_U0 = 62636861.4; //!< Normal potential at surface of common terrestrial ellipsoid [m^2/s^2]
const double GLONASS_C20 = -1082.63e-6; //!< Second zonal coefficient of spherical harmonic expansion const double GLONASS_C20 = -1082.63e-6; //!< Second zonal coefficient of spherical harmonic expansion
const double GLONASS_EARTH_RADIUS = 6378.136; //!< Equatorial radius of Earth [km] const double GLONASS_EARTH_RADIUS = 6378.136; //!< Equatorial radius of Earth [km]
const double GLONASS_EARTH_INCLINATION = 23°26'33'' //!< Mean inclination of ecliptic to equator (23°26'33''). const double GLONASS_EARTH_INCLINATION = 0.000409148809899e3; //!< Mean inclination of ecliptic to equator (23 deg 26 min 33 sec) [rad]
const double GLONASS_TAU_0 = -334°19′46′′,40; const double GLONASS_TAU_0 = -0.005835151531174e3; //!< (-334 deg 19 min 46.40 sec) [rad];
const double GLONASS_TAU_1 = 4069°02′02′′,52; const double GLONASS_TAU_1 = 0.071018041257371e3; //!< (4069 deg 02 min 02.52 sec) [rad];
const double GLONASS_MOON_Q0 = -63°53′43′′,41; const double GLONASS_MOON_Q0 = -0.001115184961435e3; //!< (-63 deg 53 min 43.41 sec) [rad]
const double GLONASS_MOON_Q1 = 477198°50′56′′,79; const double GLONASS_MOON_Q1 = 8.328691103668023e3; //!< (477198 deg 50 min 56.79 sec) [rad]
const double GLONASS_MOON_OMEGA_0 = 259°10′59′′,79; const double GLONASS_MOON_OMEGA_0 = 0.004523601514852e3; //!< (259 deg 10 min 59.79 sec) [rad]
const double GLONASS_MOON_OMEGA_1 = -1934°08′31′′,23; const double GLONASS_MOON_OMEGA_1 = -0.033757146246552e3; //!< (-1934 deg 08 min 31.23 sec) [rad]
const double GLONASS_MOON_GM = 4902.835; //!< Lunar gravitational constant [km^3/s^2] const double GLONASS_MOON_GM = 4902.835; //!< Lunar gravitational constant [km^3/s^2]
const double GLONASS_MOON_SEMI_MAJOR_AXIS = 3.84385243e5; //!< Semi-major axis of lunar orbit [km]; const double GLONASS_MOON_SEMI_MAJOR_AXIS = 3.84385243e5; //!< Semi-major axis of lunar orbit [km];
const double GLONASS_MOON_ECCENTRICITY = 0.054900489; //!< Eccentricity of lunar orbit const double GLONASS_MOON_ECCENTRICITY = 0.054900489; //!< Eccentricity of lunar orbit
const double GLONASS_MOON_INCLINATION = 5°08'43.4'' //!< Inclination of lunar orbit to ecliptic plane (5°08'43.4'') const double GLONASS_MOON_INCLINATION = 0.000089803977407e3; //!< Inclination of lunar orbit to ecliptic plane (5 deg 08 min 43.4 sec) [rad]
const double GLONASS_SUN_OMEGA = 281°13′15′′,0 + 6189′′, 03Т; const double GLONASS_SUN_OMEGA = 0.004908229466869e3; //!< TODO What is this operation in the seconds with T?(281 deg 13 min 15.0 + 6189.03*Т sec) [rad]
const double GLONASS_SUN_Q0 = 358°28′33′′,04; const double GLONASS_SUN_Q0 = 0.006256583774423e3; //!< (358 deg 28 min 33.04 sec) [rad]
const double GLONASS_SUN_Q1 = 129596579′′,10; const double GLONASS_SUN_Q1 = 0e3; //!< TODO Why is the value greater than 60?(129596579.10 sec) [rad]
const double GLONASS_SUN_GM = 0.1325263e12; //!< Solar gravitational constant [km^3/s^2] const double GLONASS_SUN_GM = 0.1325263e12; //!< Solar gravitational constant [km^3/s^2]
const double GLONASS_SUN_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-major axis of solar orbit [km]; const double GLONASS_SUN_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-major axis of solar orbit [km];
const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit
// carrier and code frequencies // carrier and code frequencies
const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz]
@ -133,7 +133,7 @@ const std::vector<std::pair<int,int>> Y_N({{51,27}});
const std::vector<std::pair<int,int>> P3({{6,1}}); const std::vector<std::pair<int,int>> P3({{6,1}});
const std::vector<std::pair<int,int>> GAMMA_N({{7,11}}); const std::vector<std::pair<int,int>> GAMMA_N({{7,11}});
const std::vector<std::pair<int,int>> P({{19,2}}); const std::vector<std::pair<int,int>> P({{19,2}});
const std::vector<std::pair<int,int>> L_N({{21,1}}); const std::vector<std::pair<int,int>> EPH_L_N({{21,1}});
const std::vector<std::pair<int,int>> Z_N_DOT ({{22,24}}); const std::vector<std::pair<int,int>> Z_N_DOT ({{22,24}});
const std::vector<std::pair<int,int>> Z_N_DOT_DOT ({{46,5}}); const std::vector<std::pair<int,int>> Z_N_DOT_DOT ({{46,5}});
const std::vector<std::pair<int,int>> Z_N({{51,27}}); const std::vector<std::pair<int,int>> Z_N({{51,27}});
@ -153,7 +153,7 @@ const std::vector<std::pair<int,int>> N_A({{6,11}});
const std::vector<std::pair<int,int>> TAU_C({{17,32}}); const std::vector<std::pair<int,int>> TAU_C({{17,32}});
const std::vector<std::pair<int,int>> N_4({{50,5}}); const std::vector<std::pair<int,int>> N_4({{50,5}});
const std::vector<std::pair<int,int>> TAU_GPS({{55,22}}); const std::vector<std::pair<int,int>> TAU_GPS({{55,22}});
const std::vector<std::pair<int,int>> L_N({{77,1}}); const std::vector<std::pair<int,int>> ALM_L_N({{77,1}});
// STRING 6, 8, 10, 12, 14 // STRING 6, 8, 10, 12, 14
const std::vector<std::pair<int,int>> C_N({{6,1}}); const std::vector<std::pair<int,int>> C_N({{6,1}});
@ -170,7 +170,7 @@ const std::vector<std::pair<int,int>> T_LAMBDA_N_A({{22,21}});
const std::vector<std::pair<int,int>> DELTA_T_N_A({{43,22}}); const std::vector<std::pair<int,int>> DELTA_T_N_A({{43,22}});
const std::vector<std::pair<int,int>> DELTA_T_DOT_N_A({{65,7}}); const std::vector<std::pair<int,int>> DELTA_T_DOT_N_A({{65,7}});
const std::vector<std::pair<int,int>> H_N_A({{72,5}}); const std::vector<std::pair<int,int>> H_N_A({{72,5}});
const std::vector<std::pair<int,int>> L_N({{77,1}}); //const std::vector<std::pair<int,int>> L_N({{77,1}});
#endif /* GNSS_SDR_GLONASS_L1_CA_H_ */ #endif /* GNSS_SDR_GLONASS_L1_CA_H_ */

167
src/core/system_parameters/glonass_gnav_almanac.cc
View File

@ -1,8 +1,7 @@
/*! /*!
* \file glonass_gnav_almanac.cc * \file glonass_gnav_almanac.cc
* \brief Interface of a GLONASS GNAV ALMANAC storage * \brief Interface of a GLONASS GNAV ALMANAC storage as described in GLONASS ICD (Edition 5.1)
* * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
* \author Damian Miralles , 2017. dmiralles2009(at)gmail.com * \author Damian Miralles , 2017. dmiralles2009(at)gmail.com
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
@ -29,29 +28,149 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include "glonass_gnav_almanac.h" #include "glonass_gnav_almanac.h"
#include <cmath>
#include "GLONASS_L1_CA.h"
#include "gnss_satellite.h"
Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() Glonass_Gnav_Almanac::Glonass_Gnav_Almanac()
{ {
int i_satellite_freq_channel = 0; i_satellite_freq_channel = 0;
double d_tau_c = 0.0; d_tau_c = 0.0;
double d_tau_gps = 0.0; d_tau_gps = 0.0;
double d_N_4 = 0.0; d_N_4 = 0.0;
double d_N_A = 0.0; d_N_A = 0.0;
double d_n_A = 0.0; d_n_A = 0.0;
double d_H_n_A = 0.0; d_H_n_A = 0.0;
double d_lambda_n_A = 0.0; d_lambda_n_A = 0.0;
double d_t_lambda_n_A = 0.0; d_t_lambda_n_A = 0.0;
double d_Delta_i_n_A = 0.0; d_Delta_i_n_A = 0.0;
double d_Delta_T_n_A = 0.0; d_Delta_T_n_A = 0.0;
double d_Delta_T_n_A_dot = 0.0; d_Delta_T_n_A_dot = 0.0;
double d_epsilon_n_A = 0.0; d_epsilon_n_A = 0.0;
double d_omega_n_A = 0.0; d_omega_n_A = 0.0;
double d_M_n_A = 0.0; d_M_n_A = 0.0;
double d_B1 = 0.0; d_KP = 0.0;
double d_B2 = 0.0; d_tau_n_A = 0.0;
double d_KP = 0.0; d_C_n_A = 0.0;
double d_tau_n_A = 0.0; }
double d_C_n_A = 0.0;
void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i)
{
double T_nom = 43200; // [seconds]
double i_nom = D2R*63.0; // [rad]
double Delta_t = 0.0;
double i = 0.0;
double T = 0.0;
double n = 0.0;
double a = 0.0;
double lambda_dot = 0.0;
double omega_dot = 0.0;
double lambda = 0.0;
double omega = 0.0;
double E_P = 0.0;
double Delta_T = 0.0;
double M = 0.0;
double E = 0.0;
double E_old = 0.0;
double dE = 0.0;
double e1_x = 0.0;
double e1_y = 0.0;
double e1_z = 0.0;
double e2_x = 0.0;
double e2_y = 0.0;
double e2_z = 0.0;
// Compute time difference to reference time
Delta_t = (N_i - d_N_A) * 86400 + (t_i + d_t_lambda_n_A);
// Compute the actual inclination
i = i_nom + d_Delta_i_n_A;
// Compute the actual orbital period:
T = T_nom + d_Delta_T_n_A;
// Compute the mean motion
n = 2*GLONASS_PI/T;
// Compute the semi-major axis:
a = cbrt(GLONASS_GM/(n*n));
// Compute correction to longitude of ascending node
lambda_dot = -10*pow(GLONASS_SEMI_MAJOR_AXIS / a, 7/2)*D2R*cos(i)/86400;
// Compute correction to argument of perigee
omega_dot = 5*pow(GLONASS_SEMI_MAJOR_AXIS / a, 7/2)*D2R*(5*cos(i)*cos(i) - 1)/86400;
// Compute corrected longitude of ascending node:
lambda = d_lambda_n_A + (lambda_dot - GLONASS_OMEGA_EARTH_DOT)*Delta_t;
// Compute corrected argument of perigee:
omega = d_omega_n_A + omega_dot*Delta_t;
// Compute eccentric anomaly at point P: Note: P is that point of the orbit the true anomaly of which is identical to the argument of perigee.
E_P = 2*atan(tan((omega/2)*(sqrt((1 - d_epsilon_n_A)*(1 + d_epsilon_n_A)))));
// Compute time difference to perigee passing
if (omega < GLONASS_PI)
{
Delta_T = (E_P - d_epsilon_n_A*sin(E_P))/n;
}
else
{
Delta_T = (E_P - d_epsilon_n_A*sin(E_P))/n + T;
}
// Compute mean anomaly at epoch t_i:
M = n * (Delta_t - Delta_T);
// Compute eccentric anomaly at epoch t_i. Note: Kepler’s equation has to be solved iteratively
// Initial guess of eccentric anomaly
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1; ii < 20; ii++)
{
E_old = E;
E = M + d_epsilon_n_A * sin(E);
dE = fmod(E - E_old, 2.0 * GLONASS_PI);
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
break;
}
}
// Compute position in orbital coordinate system
d_satpos_Xo = a*cos(E) - d_epsilon_n_A;
d_satpos_Yo = a*sqrt(1 - d_epsilon_n_A*d_epsilon_n_A)*sin(E);
d_satpos_Zo = a*0;
// Compute velocity in orbital coordinate system
d_satvel_Xo = a/(1-d_epsilon_n_A*cos(E))*(-n*sin(E));
d_satvel_Yo = a/(1-d_epsilon_n_A*cos(E))*(n*sqrt(1 - d_epsilon_n_A*d_epsilon_n_A)*cos(E));
d_satvel_Zo = a/(1-d_epsilon_n_A*cos(E))*(0);
// Determine orientation vectors of orbital coordinate system in ECEF system
e1_x = cos(omega)*cos(lambda) - sin(omega)*sin(lambda);
e1_y = cos(omega)*sin(lambda) + sin(omega)*cos(lambda)*cos(i);
e1_z = sin(omega)*sin(i);
e2_x = -sin(omega)*cos(lambda) - sin(omega)*sin(lambda)*cos(i);
e2_y = -sin(omega)*sin(lambda) + cos(omega)*cos(lambda)*cos(i);
e2_z = cos(omega)*sin(i);
// Convert position from orbital to ECEF system
d_satpos_X = d_satpos_Xo*e1_x + d_satpos_Xo*e2_x;
d_satpos_Y = d_satpos_Yo*e1_z + d_satpos_Yo*e2_y;
d_satpos_Z = d_satpos_Zo*e1_z + d_satpos_Zo*e2_z;
// Convert position from orbital to ECEF system
d_satvel_X = d_satvel_Xo*e1_x + d_satvel_Xo*e2_x + GLONASS_OMEGA_EARTH_DOT*d_satpos_Y;
d_satvel_Y = d_satvel_Yo*e1_z + d_satvel_Yo*e2_y - GLONASS_OMEGA_EARTH_DOT*d_satpos_X;
d_satvel_Z = d_satvel_Zo*e1_z + d_satvel_Zo*e2_z;
} }

65
src/core/system_parameters/glonass_gnav_almanac.h
View File

@ -32,16 +32,20 @@
#ifndef GNSS_SDR_GLONASS_ALMANAC_H_ #ifndef GNSS_SDR_GLONASS_ALMANAC_H_
#define GNSS_SDR_GLONASS_ALMANAC_H_ #define GNSS_SDR_GLONASS_ALMANAC_H_
#include <map>
#include <string>
#include "boost/assign.hpp"
#include <boost/serialization/nvp.hpp>
/*! /*!
* \brief This class is a storage for the GLONASS SV ALMANAC data as described in IS-GPS-200E * \brief This class is a storage for the GLONASS SV ALMANAC data as described GLONASS ICD (Edition 5.1)
* \todo Add proper links for GLONASS ICD *
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
*/ */
class Glonass_Gnav_Almanac class Glonass_Gnav_Almanac
{ {
public: public:
int i_satellite_freq_channel; //!< SV PRN NUMBER int i_satellite_freq_channel; //!< SV Frequency Channel NUMBER
double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s]
double d_tau_gps; //!< Correction to GPS time to GLONASS time [day] double d_tau_gps; //!< Correction to GPS time to GLONASS time [day]
double d_N_4; //!< Four year interval number starting from 1996 [4 year interval] double d_N_4; //!< Four year interval number starting from 1996 [4 year interval]
@ -50,17 +54,64 @@ public:
double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless] double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless]
double d_lambda_n_A; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles] double d_lambda_n_A; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles]
double d_t_lambda_n_A; //!< Time of first ascending node passage [s] double d_t_lambda_n_A; //!< Time of first ascending node passage [s]
double d_Delta_i_n_A //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles] double d_Delta_i_n_A; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles]
double d_Delta_T_n_A; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period] double d_Delta_T_n_A; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period]
double d_Delta_T_n_A_dot; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2] double d_Delta_T_n_A_dot; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2]
double d_epsilon_n_A; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless] double d_epsilon_n_A; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless]
double d_omega_n_A; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles] double d_omega_n_A; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles]
double d_M_n_A; //!< Type of satellite n_A [dimensionless] double d_M_n_A; //!< Type of satellite n_A [dimensionless]
double d_B1; //!< Coefficient to determine DeltaUT1 [s]
double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd]
double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless] double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless]
double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s] double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s]
double d_C_n_A; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless] double d_C_n_A; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless]
// satellite positions
double d_satpos_Xo; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
double d_satpos_Yo; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
double d_satpos_Zo; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
// Satellite velocity
double d_satvel_Xo; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Yo; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Zo; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
// satellite positions
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
// Satellite velocity
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
template<class Archive>
/*!
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the almanac data on disk file.
*/
void serialize(Archive& archive, const unsigned int version)
{
using boost::serialization::make_nvp;
if(version){};
archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel);
archive & make_nvp("d_tau_c", d_tau_c);
archive & make_nvp("d_tau_gps", d_tau_gps);
archive & make_nvp("d_N_4", d_N_4);
archive & make_nvp("d_N_A", d_N_A);
archive & make_nvp("d_n_A", d_n_A);
archive & make_nvp("d_H_n_A", d_H_n_A);
archive & make_nvp("d_lambda_n_A", d_lambda_n_A);
archive & make_nvp("d_t_lambda_n_A", d_t_lambda_n_A);
archive & make_nvp("d_Delta_i_n_A", d_Delta_i_n_A);
archive & make_nvp("d_Delta_T_n_A", d_Delta_T_n_A);
archive & make_nvp("d_Delta_T_n_A_dot", d_Delta_T_n_A_dot);
archive & make_nvp("d_epsilon_n_A", d_epsilon_n_A);
archive & make_nvp("d_omega_n_A", d_omega_n_A);
archive & make_nvp("d_M_n_A", d_M_n_A);
archive & make_nvp("d_KP", d_KP);
archive & make_nvp("d_tau_n_A", d_tau_n_A);
archive & make_nvp("d_C_n_A", d_C_n_A);
}
void satellite_position(double N_i, double t_i);
/*! /*!
* Default constructor * Default constructor
*/ */

638
src/core/system_parameters/glonass_gnav_ephemeris.cc
View File

@ -1,9 +1,11 @@
/*! /*!
* \file gps_ephemeris.cc * \file glonass_gnav_ephemeris.cc
* \brief Interface of a GPS EPHEMERIS storage and orbital model functions * \brief Interface of a GLONASS GNAV EPHEMERIS storage and orbital model functions
* *
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II * See Russian Institute of Space Device Engineering,
* \author Javier Arribas, 2013. jarribas(at)cttc.es “Global Navigation Satellite System GLONASS Interface Control Document,
Navigational radiosignal in bands L1, L2,” Moscow, Edition 5.1, 2008.
* \author Damian Miralles, 2017. dmiralles2009(at)gmail.com
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
@ -30,12 +32,12 @@
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include "gps_ephemeris.h" #include "glonass_gnav_ephemeris.h"
#include <cmath> #include <cmath>
#include "GLONASS_L1_CA.h" #include "GLONASS_L1_CA.h"
#include "gnss_satellite.h" #include "gnss_satellite.h"
Gps_Ephemeris::Gps_Ephemeris() Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris()
{ {
i_satellite_freq_channel = 0; i_satellite_freq_channel = 0;
d_m = 0.0; //!< String number within frame [dimensionless] d_m = 0.0; //!< String number within frame [dimensionless]
@ -75,53 +77,56 @@ Gps_Ephemeris::Gps_Ephemeris()
} }
void Glonass_GNAV_Ephemeris::gravitational_perturbations() void Glonass_Gnav_Ephemeris::gravitational_perturbations()
{ {
double T; double T = 0.0;
double sigma_days; double sigma_days = 0.0;
double tau_prime; double tau_prime = 0.0;
double Omega_moon; double Omega_moon = 0.0;
double q_moon; double q_moon = 0.0;
double q_sun; double q_sun = 0.0;
double Tau_11; double Tau_12;
double Eta_11; double Eta_12;
double xi_11; double xi_12;
double xi_star;
double eta_star;
double zeta_star;
double E_moon;
double E_sun;
double xi_11;
double xi_12;
double eta_11;
double eta_12;
double zeta_11;
double zeta_12;
double sin_theta_moon; double xi_star = 0.0;
double cos_theta_moon; double eta_star = 0.0;
double theta_moon; double zeta_star = 0.0;
double xi_moon_e; double E_moon = 0.0;
double eta_moon_e; double E_sun = 0.0;
double zeta_moon_e; double xi_11 = 0.0;
double xi_12 = 0.0;
double eta_11 = 0.0;
double eta_12 = 0.0;
double zeta_11 = 0.0;
double zeta_12 = 0.0;
double sin_theta_sun; double sin_theta_moon = 0.0;
double cos_theta_sun; double cos_theta_moon = 0.0;
double theta_sun; double theta_moon = 0.0;
double xi_sun_e; double xi_moon_e = 0.0;
double eta_sun_e; double eta_moon_e = 0.0;
double zeta_sun_e; double zeta_moon_e = 0.0;
double r_moon_e = 0.0;
double r_moon_e; double sin_theta_sun = 0.0;
double mu_bar_moon; double cos_theta_sun = 0.0;
double x_bar_moon; double theta_sun = 0.0;
double y_bar_moon; double xi_sun_e = 0.0;
double Delta_o_moon; double eta_sun_e = 0.0;
double mu_bar_sun; double zeta_sun_e = 0.0;
double x_bar_sun; double r_sun_e = 0.0;
double y_bar_sun;
double Delta_o_sun;
double mu_bar_moon = 0.0;
double x_bar_moon = 0.0;
double y_bar_moon = 0.0;
double z_bar_moon = 0.0;
double Delta_o_moon = 0.0;
double mu_bar_sun = 0.0;
double x_bar_sun = 0.0;
double y_bar_sun = 0.0;
double z_bar_sun = 0.0;
double Delta_o_sun = 0.0;
double t_e = 0.0;
//<! Directive Cosine computation //<! Directive Cosine computation
/// \TODO Need to define sigma days /// \TODO Need to define sigma days
@ -165,17 +170,17 @@ void Glonass_GNAV_Ephemeris::gravitational_perturbations()
r_sun_e = GLONASS_SUN_SEMI_MAJOR_AXIS*(1-GLONASS_SUN_ECCENTRICITY*cos(E_sun)); r_sun_e = GLONASS_SUN_SEMI_MAJOR_AXIS*(1-GLONASS_SUN_ECCENTRICITY*cos(E_sun));
//<! Gravitational computation //<! Gravitational computation
mu_bar_moon = GLONASS_MOON_GM/(d_r_moon_e*d_r_moon_e); mu_bar_moon = GLONASS_MOON_GM/(r_moon_e*r_moon_e);
x_bar_moon = d_satpos_X/d_r_moon_e; x_bar_moon = d_Xn/r_moon_e;
y_bar_moon = d_satpos_Y/d_r_moon_e; y_bar_moon = d_Yn/r_moon_e;
z_bar_moon = d_satpos_Z/d_r_moon_e; z_bar_moon = d_Zn/r_moon_e;
Delta_o_moon = sqrt((d_xi_moon_e - x_bar_moon)*(d_xi_moon_e - x_bar_moon) + (d_eta_moon_e - y_bar_moon)*(d_eta_moon_e - y_bar_moon) + (d_zeta_moon_e - z_bar_moon)*(d_zeta_moon_e - z_bar_moon)); Delta_o_moon = sqrt((xi_moon_e - x_bar_moon)*(xi_moon_e - x_bar_moon) + (eta_moon_e - y_bar_moon)*(eta_moon_e - y_bar_moon) + (zeta_moon_e - z_bar_moon)*(zeta_moon_e - z_bar_moon));
mu_bar_sun = GLONASS_MOON_GM/(d_r_sun_e*d_r_sun_e); mu_bar_sun = GLONASS_MOON_GM/(r_sun_e*r_sun_e);
x_bar_sun = d_satpos_X/d_r_sun_e; x_bar_sun = d_Xn/r_sun_e;
y_bar_sun = d_satpos_Y/d_r_sun_e; y_bar_sun = d_Yn/r_sun_e;
z_bar_sun = d_satpos_Z/d_r_sun_e; z_bar_sun = d_Zn/r_sun_e;
Delta_o_sun = sqrt((d_xi_sun_e - x_bar_sun)*(d_xi_sun_e - x_bar_sun) + (d_eta_sun_e - y_bar_sun)*(d_eta_sun_e - y_bar_sun) + (d_zeta_sun_e - z_bar_sun)*(d_zeta_sun_e - z_bar_sun)); Delta_o_sun = sqrt((xi_sun_e - x_bar_sun)*(xi_sun_e - x_bar_sun) + (eta_sun_e - y_bar_sun)*(eta_sun_e - y_bar_sun) + (zeta_sun_e - z_bar_sun)*(zeta_sun_e - z_bar_sun));
d_Jx_moon = mu_bar_moon*(xi_moon_e - x_bar_moon)/pow(Delta_o_moon,3.0) - xi_moon_e; d_Jx_moon = mu_bar_moon*(xi_moon_e - x_bar_moon)/pow(Delta_o_moon,3.0) - xi_moon_e;
d_Jy_moon = mu_bar_moon*(eta_moon_e - y_bar_moon)/pow(Delta_o_moon,3.0) - eta_moon_e; d_Jy_moon = mu_bar_moon*(eta_moon_e - y_bar_moon)/pow(Delta_o_moon,3.0) - eta_moon_e;
@ -188,7 +193,7 @@ void Glonass_GNAV_Ephemeris::gravitational_perturbations()
} }
double Glonass_GNAV_Ephemeris::check_t(double time) double Glonass_Gnav_Ephemeris::check_t(double time)
{ {
double corrTime; double corrTime;
double half_day = 43200.0; // seconds double half_day = 43200.0; // seconds
@ -204,9 +209,9 @@ double Glonass_GNAV_Ephemeris::check_t(double time)
return corrTime; return corrTime;
} }
// FIXME Fix reference here
// 20.3.3.3.3.1 User Algorithm for SV Clock Correction. // 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
double Glonass_GNAV_Ephemeris::sv_clock_drift(double transmitTime, double timeCorrUTC) double Glonass_Gnav_Ephemeris::sv_clock_drift(double transmitTime, double timeCorrUTC)
{ {
double dt; double dt;
dt = check_t(transmitTime - d_t_b); dt = check_t(transmitTime - d_t_b);
@ -217,93 +222,148 @@ double Glonass_GNAV_Ephemeris::sv_clock_drift(double transmitTime, double timeCo
return d_satClkDrift; return d_satClkDrift;
} }
// TODO is this the right formula
// compute the relativistic correction term // compute the relativistic correction term
double Glonass_GNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) double Glonass_Gnav_Ephemeris::sv_clock_relativistic_term(double transmitTime)
{ {
double tk;
double a;
double n;
double n0;
double E;
double E_old;
double dE;
double M;
// 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.0 * GPS_PI), (2.0 * 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.0 * GPS_PI);
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
break;
}
}
// Compute relativistic correction term // Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E); d_dtr = 0.0;
return d_dtr; return d_dtr;
} }
double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime) double Glonass_Gnav_Ephemeris::simplified_satellite_position(double transmitTime)
{ {
double dt = 0.0; double dt = 0.0;
double tau = 0.0;
double tk = 0.0; double tk = 0.0;
int numberOfIntegrations = 0; int numberOfIntegrations = 0;
// RK 1
double x_0 = 0.0;
double y_0 = 0.0;
double z_0 = 0.0;
double Vx_0 = 0.0;
double Vy_0 = 0.0;
double Vz_0 = 0.0;
double Ax_0 = 0.0;
double Ay_0 = 0.0;
double Az_0 = 0.0;
double r0 = 0.0;
double r0_2 = 0.0;
double r0_3 = 0.0;
double r0_5 = 0.0;
double dVx_1 = 0.0;
double dVy_1 = 0.0;
double dVz_1 = 0.0;
double dx_1 = 0.0;
double dy_1 = 0.0;
double dz_1 = 0.0;
// Runge-Kutta Integration Stage K2
double x_1 = 0.0;
double y_1 = 0.0;
double z_1 = 0.0;
double Vx_1 = 0.0;
double Vy_1 = 0.0;
double Vz_1 = 0.0;
double r1 = 0.0;
double r1_2 = 0.0;
double r1_3 = 0.0;
double r1_5 = 0.0;
double dVx_2 = 0.0;
double dVy_2 = 0.0;
double dVz_2 = 0.0;
double dx_2 = 0.0;
double dy_2 = 0.0;
double dz_2 = 0.0;
// Runge-Kutta Integration Stage K3
double x_2 = 0.0;
double y_2 = 0.0;
double z_2 = 0.0;
double Vx_2 = 0.0;
double Vy_2 = 0.0;
double Vz_2 = 0.0;
double r2 = 0.0;
double r2_2 = 0.0;
double r2_3 = 0.0;
double r2_5 = 0.0;
double dVx_3 = 0.0;
double dVy_3 = 0.0;
double dVz_3 = 0.0;
double dx_3 = 0.0;
double dy_3 = 0.0;
double dz_3 = 0.0;
// Runge-Kutta Integration Stage K4
double x_3 = 0.0;
double y_3 = 0.0;
double z_3 = 0.0;
double Vx_3 = 0.0;
double Vy_3 = 0.0;
double Vz_3 = 0.0;
double r3 = 0.0;
double r3_2 = 0.0;
double r3_3 = 0.0;
double r3_5 = 0.0;
double dVx_4 = 0.0;
double dVy_4 = 0.0;
double dVz_4 = 0.0;
double dx_4 = 0.0;
double dy_4 = 0.0;
double dz_4 = 0.0;
// Find time difference // Find time difference
dt = check_t(transmitTime - d_t_b); dt = check_t(transmitTime - d_t_b);
// Calculate clock correction // Calculate clock correction
satClkCorr = -(d_tau_n + d_tau_c - d_gamma_n * dt); d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt);
// Find integration time // Find integration time
tk = dt - satClkCorr; tk = dt - d_satClkDrift;
// Check if to integrate forward or backward // Check if to integrate forward or backward
if tk < 0 if(tk < 0)
tau = -60; {
tau = -60;
}
else else
tau = 60; {
end tau = 60;
}
// x,y,z coordinates to meters // x,y,z coordinates to meters
x0 = d_satpos_X * 1e3; x_0 = d_Xn * 1e3;
y0 = d_satpos_Y * 1e3; y_0 = d_Yn * 1e3;
z0 = d_satpos_Z * 1e3; z_0 = d_Zn * 1e3;
// x,y,z velocities to meters/s // x,y,z velocities to meters/s
Vx0 = d_satvel_X * 1e3; Vx_0 = d_VXn * 1e3;
Vy0 = d_satvel_Y * 1e3; Vy_0 = d_VYn * 1e3;
Vz0 = d_satvel_Z * 1e3; Vz_0 = d_VZn * 1e3;
// x,y,z accelerations to meters/sec^2 // x,y,z accelerations to meters/sec^2
Ax0 = d_satacc_X * 1e3; Ax_0 = d_AXn * 1e3;
Ay0 = d_satacc_Y * 1e3; Ay_0 = d_AYn * 1e3;
Az0 = d_satacc_Z * 1e3; Az_0 = d_AZn * 1e3;
for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau) for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau)
{ {
@ -311,9 +371,9 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime)
if(fabs(numberOfIntegrations) > fabs(tk)) if(fabs(numberOfIntegrations) > fabs(tk))
{ {
// if there is more time left to integrate... // if there is more time left to integrate...
if mod(tk,tau) ~= 0 if(fmod(tk,tau) != 0)
{ {
tau = mod(time,tau); tau = fmod(tk,tau);
} }
else // otherwise make a zero step. else // otherwise make a zero step.
{ {
@ -322,49 +382,49 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime)
} }
// Runge-Kutta Integration Stage K1 // Runge-Kutta Integration Stage K1
r0 = sqrt(x0*x0 + y0*y0 + z0*z0); r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0);
r0_2 = r0*r0; r0_2 = r0*r0;
r0_3 = r0*r0*r0; r0_3 = r0*r0*r0;
r0_5 = r0*r0*r0*r0*r0; r0_5 = r0*r0*r0*r0*r0;
dVx_1 = - GLONASS_GM / r0_3 * x0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * x0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * x0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy0 + Ax0; dVx_1 = - GLONASS_GM / r0_3 * x_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * x_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_0 + Ax_0;
dVy_1 = - GLONASS_GM / r0_3 * y0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * y0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * y0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx0 + Ay0; dVy_1 = - GLONASS_GM / r0_3 * y_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * y_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_0 + Ay_0;
dVz_1 = - GLONASS_GM / r0_3 * z0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * z0 * (3 - 5 * (z0*z0) / r0_2) + Az0; dVz_1 = - GLONASS_GM / r0_3 * z_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * z_0 * (3 - 5 * (z_0*z_0) / r0_2) + Az_0;
dx_1 = Vx0; dx_1 = Vx_0;
dy_1 = Vy0; dy_1 = Vy_0;
dz_1 = Vz0; dz_1 = Vz_0;
// Runge-Kutta Integration Stage K2 // Runge-Kutta Integration Stage K2
Vx_1 = Vx0 + 1/2 * tau * dVx_1; Vx_1 = Vx_0 + 1/2 * tau * dVx_1;
Vy_1 = Vy0 + 1/2 * tau * dVy_1; Vy_1 = Vy_0 + 1/2 * tau * dVy_1;
Vz_1 = Vz0 + 1/2 * tau * dVz_1; Vz_1 = Vz_0 + 1/2 * tau * dVz_1;
x_1 = x0 + 1/2 * tau * dx_1; x_1 = x_0 + 1/2 * tau * dx_1;
y_1 = y0 + 1/2 * tau * dy_1; y_1 = y_0 + 1/2 * tau * dy_1;
z_1 = z0 + 1/2 * tau * dz_1; z_1 = z_0 + 1/2 * tau * dz_1;
r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 );
r1_2 = r1*r1; r1_2 = r1*r1;
r1_3 = r1*r1*r1; r1_3 = r1*r1*r1;
r1_5 = r1*r1*r1*r1*r1; r1_5 = r1*r1*r1*r1*r1;
dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0; dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0;
dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0; dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0;
dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0; dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0;
dx_2 = Vx_1; dx_2 = Vx_1;
dy_2 = Vy_1; dy_2 = Vy_1;
dz_2 = Vz_1; dz_2 = Vz_1;
// Runge-Kutta Integration Stage K2 // Runge-Kutta Integration Stage K2
Vx_2 = Vx0 + 1/2 * tau * dVx_2; Vx_2 = Vx_0 + 1/2 * tau * dVx_2;
Vy_2 = Vy0 + 1/2 * tau * dVy_2; Vy_2 = Vy_0 + 1/2 * tau * dVy_2;
Vz_2 = Vz0 + 1/2 * tau * dVz_2; Vz_2 = Vz_0 + 1/2 * tau * dVz_2;
x_2 = x0 + 1/2 * tau * dx_2; x_2 = x_0 + 1/2 * tau * dx_2;
y_2 = y0 + 1/2 * tau * dy_2; y_2 = y_0 + 1/2 * tau * dy_2;
z_2 = z0 + 1/2 * tau * dz_2; z_2 = z_0 + 1/2 * tau * dz_2;
r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 );
r2_2 = r2*r2; r2_2 = r2*r2;
@ -372,109 +432,205 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime)
r2_5 = r2*r2*r2*r2*r2; r2_5 = r2*r2*r2*r2*r2;
dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0; dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0;
dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0; dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0;
dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0; dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0;
dx_3 = Vx_2; dx_3 = Vx_2;
dy_3 = Vy_2; dy_3 = Vy_2;
dz_3 = Vz_2; dz_3 = Vz_2;
// Runge-Kutta Integration Stage K3 // Runge-Kutta Integration Stage K3
Vx_3 = Vx0 + tau * dVx_3; Vx_3 = Vx_0 + tau * dVx_3;
Vy_3 = Vy0 + tau * dVy_3; Vy_3 = Vy_0 + tau * dVy_3;
Vz_3 = Vz0 + tau * dVz_3; Vz_3 = Vz_0 + tau * dVz_3;
x_3 = x0 + tau * dx_3; x_3 = x_0 + tau * dx_3;
y_3 = y0 + tau * dy_3; y_3 = y_0 + tau * dy_3;
z_3 = z0 + tau * dz_3; z_3 = z_0 + tau * dz_3;
r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 );
r3_2 = r3*r3; r3_2 = r3*r3;
r3_3 = r3*r3*r3; r3_3 = r3*r3*r3;
r3_5 = r3*r3*r3*r3*r3; r3_5 = r3*r3*r3*r3*r3;
dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0; dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0;
dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0; dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0;
dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0; dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0;
dx_4 = Vx_3; dx_4 = Vx_3;
dy_4 = Vy_3; dy_4 = Vy_3;
dz_4 = Vz_3; dz_4 = Vz_3;
// Final results showcased here // Final results showcased here
Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); Vx_0 = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 );
Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); Vy_0 = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 );
Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); Vz_0 = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 );
x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); x_0 = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 );
y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); y_0 = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 );
z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); z_0 = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 );
} }
// Reasign position, velocities and accelerations for next integration // Reasign position, velocities and accelerations for next integration
d_satpos_X = x0; d_satpos_X = x_0;
d_satpos_Y = y0; d_satpos_Y = y_0;
d_satpos_Z = z0; d_satpos_Z = z_0;
d_satvel_X = Vx0; d_satvel_X = Vx_0;
d_satvel_Y = Vy0; d_satvel_Y = Vy_0;
d_satvel_Z = Vz0; d_satvel_Z = Vz_0;
d_satacc_X = d_satacc_X; // No change in accelerations reported over interval d_satacc_X = d_AXn; // No change in accelerations reported over interval
d_satacc_Y = d_satacc_Y; // No change in accelerations reported over interval d_satacc_Y = d_AYn; // No change in accelerations reported over interval
d_satacc_Z = d_satacc_Z; // No change in accelerations reported over interval d_satacc_Z = d_AZn; // No change in accelerations reported over interval
// Time from ephemeris reference clock // Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc); //tk = check_t(transmitTime - d_Toc);
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */ /* relativity correction */
dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
return dtr_s; return 0;
} }
double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) double Glonass_Gnav_Ephemeris::satellite_position(double transmitTime)
{ {
double dt = 0.0; double dt = 0.0;
double tk = 0.0; double tk = 0.0;
double tau = 0.0;
int numberOfIntegrations = 0; int numberOfIntegrations = 0;
// RK 1
double x_0 = 0.0;
double y_0 = 0.0;
double z_0 = 0.0;
double Vx_0 = 0.0;
double Vy_0 = 0.0;
double Vz_0 = 0.0;
double Ax_0 = 0.0;
double Ay_0 = 0.0;
double Az_0 = 0.0;
double r0 = 0.0;
double r0_2 = 0.0;
double r0_3 = 0.0;
double r0_5 = 0.0;
double dVx_1 = 0.0;
double dVy_1 = 0.0;
double dVz_1 = 0.0;
double dx_1 = 0.0;
double dy_1 = 0.0;
double dz_1 = 0.0;
// Runge-Kutta Integration Stage K2
double x_1 = 0.0;
double y_1 = 0.0;
double z_1 = 0.0;
double Vx_1 = 0.0;
double Vy_1 = 0.0;
double Vz_1 = 0.0;
double r1 = 0.0;
double r1_2 = 0.0;
double r1_3 = 0.0;
double r1_5 = 0.0;
double dVx_2 = 0.0;
double dVy_2 = 0.0;
double dVz_2 = 0.0;
double dx_2 = 0.0;
double dy_2 = 0.0;
double dz_2 = 0.0;
// Runge-Kutta Integration Stage K3
double x_2 = 0.0;
double y_2 = 0.0;
double z_2 = 0.0;
double Vx_2 = 0.0;
double Vy_2 = 0.0;
double Vz_2 = 0.0;
double r2 = 0.0;
double r2_2 = 0.0;
double r2_3 = 0.0;
double r2_5 = 0.0;
double dVx_3 = 0.0;
double dVy_3 = 0.0;
double dVz_3 = 0.0;
double dx_3 = 0.0;
double dy_3 = 0.0;
double dz_3 = 0.0;
// Runge-Kutta Integration Stage K4
double x_3 = 0.0;
double y_3 = 0.0;
double z_3 = 0.0;
double Vx_3 = 0.0;
double Vy_3 = 0.0;
double Vz_3 = 0.0;
double r3 = 0.0;
double r3_2 = 0.0;
double r3_3 = 0.0;
double r3_5 = 0.0;
double dVx_4 = 0.0;
double dVy_4 = 0.0;
double dVz_4 = 0.0;
double dx_4 = 0.0;
double dy_4 = 0.0;
double dz_4 = 0.0;
// Find time difference // Find time difference
dt = check_t(transmitTime - d_t_b); dt = check_t(transmitTime - d_t_b);
// Calculate clock correction // Calculate clock correction
satClkCorr = -(d_tau_n + d_tau_c - d_gamma_n * dt); d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt);
// Find integration time // Find integration time
tk = dt - satClkCorr; tk = dt - d_satClkDrift;
// Check if to integrate forward or backward // Check if to integrate forward or backward
if tk < 0 if (tk < 0)
tau = -60; {
tau = -60;
}
else else
tau = 60; {
end tau = 60;
}
// Coordinates transformation to an inertial reference frame // Coordinates transformation to an inertial reference frame
// x,y,z coordinates to meters // x,y,z coordinates to meters
x0 = d_satpos_X * 1e3; x_0 = d_Xn * 1e3;
y0 = d_satpos_Y * 1e3; y_0 = d_Yn * 1e3;
z0 = d_satpos_Z * 1e3; z_0 = d_Zn * 1e3;
// x,y,z velocities to meters/s // x,y,z velocities to meters/s
Vx0 = d_satvel_X * 1e3; Vx_0 = d_VXn * 1e3;
Vy0 = d_satvel_Y * 1e3; Vy_0 = d_VYn * 1e3;
Vz0 = d_satvel_Z * 1e3; Vz_0 = d_VZn * 1e3;
// x,y,z accelerations to meters/sec^2 // x,y,z accelerations to meters/sec^2
Ax0 = d_satacc_X * 1e3; Ax_0 = d_AXn * 1e3;
Ay0 = d_satacc_Y * 1e3; Ay_0 = d_AYn * 1e3;
Az0 = d_satacc_Z * 1e3; Az_0 = d_AZn * 1e3;
for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau) for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau)
{ {
@ -482,9 +638,9 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime)
if(fabs(numberOfIntegrations) > fabs(tk)) if(fabs(numberOfIntegrations) > fabs(tk))
{ {
// if there is more time left to integrate... // if there is more time left to integrate...
if mod(tk,tau) ~= 0 if (fmod(tk,tau) != 0)
{ {
tau = mod(time,tau); tau = fmod(tk,tau);
} }
else // otherwise make a zero step. else // otherwise make a zero step.
{ {
@ -493,53 +649,53 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime)
} }
// Runge-Kutta Integration Stage K1 // Runge-Kutta Integration Stage K1
r0 = sqrt(x0*x0 + y0*y0 + z0*z0); r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0);
r0_2 = r0*r0; r0_2 = r0*r0;
r0_3 = r0*r0*r0; r0_3 = r0*r0*r0;
r0_5 = r0*r0*r0*r0*r0; r0_5 = r0*r0*r0*r0*r0;
dx_1 = Vx0; dx_1 = Vx_0;
dy_1 = Vy0; dy_1 = Vy_0;
dz_1 = Vz0; dz_1 = Vz_0;
dVx_1 = - GLONASS_GM / r0_3 * x0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * x0 * (1 - 5 * 1 / (z0*z0)) + Jx_moon + Jx_sun; dVx_1 = - GLONASS_GM / r0_3 * x_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * x_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jx_moon + d_Jx_sun;
dVy_1 = - GLONASS_GM / r0_3 * y0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * y0 * (1 - 5 * 1 / (z0*z0)) + Jy_moon + Jy_sun; dVy_1 = - GLONASS_GM / r0_3 * y_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * y_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jy_moon + d_Jy_sun;
dVz_1 = - GLONASS_GM / r0_3 * z0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * z0 * (3 - 5 * 1 / (z0*z0)) + Jz_moon + Jz_sun; dVz_1 = - GLONASS_GM / r0_3 * z_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * z_0 * (3 - 5 * 1 / (z_0*z_0)) + d_Jz_moon + d_Jz_sun;
dx_1 = Vx0; dx_1 = Vx_0;
dy_1 = Vy0; dy_1 = Vy_0;
dz_1 = Vz0; dz_1 = Vz_0;
// Runge-Kutta Integration Stage K2 // Runge-Kutta Integration Stage K2
Vx_1 = Vx0 + 1/2 * tau * dVx_1; Vx_1 = Vx_0 + 1/2 * tau * dVx_1;
Vy_1 = Vy0 + 1/2 * tau * dVy_1; Vy_1 = Vy_0 + 1/2 * tau * dVy_1;
Vz_1 = Vz0 + 1/2 * tau * dVz_1; Vz_1 = Vz_0 + 1/2 * tau * dVz_1;
x_1 = x0 + 1/2 * tau * dx_1; x_1 = x_0 + 1/2 * tau * dx_1;
y_1 = y0 + 1/2 * tau * dy_1; y_1 = y_0 + 1/2 * tau * dy_1;
z_1 = z0 + 1/2 * tau * dz_1; z_1 = z_0 + 1/2 * tau * dz_1;
r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 );
r1_2 = r1*r1; r1_2 = r1*r1;
r1_3 = r1*r1*r1; r1_3 = r1*r1*r1;
r1_5 = r1*r1*r1*r1*r1; r1_5 = r1*r1*r1*r1*r1;
dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0; dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0;
dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0; dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0;
dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0; dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0;
dx_2 = Vx_1; dx_2 = Vx_1;
dy_2 = Vy_1; dy_2 = Vy_1;
dz_2 = Vz_1; dz_2 = Vz_1;
// Runge-Kutta Integration Stage K2 // Runge-Kutta Integration Stage K2
Vx_2 = Vx0 + 1/2 * tau * dVx_2; Vx_2 = Vx_0 + 1/2 * tau * dVx_2;
Vy_2 = Vy0 + 1/2 * tau * dVy_2; Vy_2 = Vy_0 + 1/2 * tau * dVy_2;
Vz_2 = Vz0 + 1/2 * tau * dVz_2; Vz_2 = Vz_0 + 1/2 * tau * dVz_2;
x_2 = x0 + 1/2 * tau * dx_2; x_2 = x_0 + 1/2 * tau * dx_2;
y_2 = y0 + 1/2 * tau * dy_2; y_2 = y_0 + 1/2 * tau * dy_2;
z_2 = z0 + 1/2 * tau * dz_2; z_2 = z_0 + 1/2 * tau * dz_2;
r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 );
r2_2 = r2*r2; r2_2 = r2*r2;
@ -547,54 +703,54 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime)
r2_5 = r2*r2*r2*r2*r2; r2_5 = r2*r2*r2*r2*r2;
dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0; dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0;
dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0; dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0;
dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0; dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0;
dx_3 = Vx_2; dx_3 = Vx_2;
dy_3 = Vy_2; dy_3 = Vy_2;
dz_3 = Vz_2; dz_3 = Vz_2;
// Runge-Kutta Integration Stage K3 // Runge-Kutta Integration Stage K3
Vx_3 = Vx0 + tau * dVx_3; Vx_3 = Vx_0 + tau * dVx_3;
Vy_3 = Vy0 + tau * dVy_3; Vy_3 = Vy_0 + tau * dVy_3;
Vz_3 = Vz0 + tau * dVz_3; Vz_3 = Vz_0 + tau * dVz_3;
x_3 = x0 + tau * dx_3; x_3 = x_0 + tau * dx_3;
y_3 = y0 + tau * dy_3; y_3 = y_0 + tau * dy_3;
z_3 = z0 + tau * dz_3; z_3 = z_0 + tau * dz_3;
r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 );
r3_2 = r3*r3; r3_2 = r3*r3;
r3_3 = r3*r3*r3; r3_3 = r3*r3*r3;
r3_5 = r3*r3*r3*r3*r3; r3_5 = r3*r3*r3*r3*r3;
dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0; dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0;
dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0; dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0;
dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0; dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0;
dx_4 = Vx_3; dx_4 = Vx_3;
dy_4 = Vy_3; dy_4 = Vy_3;
dz_4 = Vz_3; dz_4 = Vz_3;
// Final results showcased here // Final results showcased here
Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); d_satvel_X = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 );
Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); d_satvel_Y = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 );
Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); d_satvel_Z = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 );
x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); d_satpos_X = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 );
y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); d_satpos_Y = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 );
z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); d_satpos_Z = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 );
} }
// Time from ephemeris reference clock // Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc); //tk = check_t(transmitTime - d_Toc);
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */ /* relativity correction */
dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
return dtr_s; return d_dtr;
} }

52
src/core/system_parameters/glonass_gnav_ephemeris.h
View File

@ -41,11 +41,11 @@
/*! /*!
* \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD Edition 5.1 * \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD (Edition 5.1)
* *
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
*/ */
class Glonass_GNAV_Ephemeris class Glonass_Gnav_Ephemeris
{ {
private: private:
/* /*
@ -57,10 +57,15 @@ private:
*/ */
double check_t(double time); double check_t(double time);
double gravitational_perturbations(const double mu); void gravitational_perturbations();
double d_Jx_moon; //!< Moon gravitational perturbation double d_Jx_moon; //!< Moon gravitational perturbation
double d_Jy_moon; //!< Moon gravitational perturbation
double d_Jz_moon; //!< Moon gravitational perturbation
double d_Jx_sun; //!< Sun gravitational perturbation double d_Jx_sun; //!< Sun gravitational perturbation
double d_Jy_sun; //!< Sun gravitational perturbation
double d_Jz_sun; //!< Sun gravitational perturbation
public: public:
unsigned int i_satellite_freq_channel; // SV Frequency Channel Number unsigned int i_satellite_freq_channel; // SV Frequency Channel Number
@ -70,18 +75,20 @@ public:
double d_M; //!< Type of satellite transmitting navigation signal [dimensionless] double d_M; //!< Type of satellite transmitting navigation signal [dimensionless]
double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless] double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless]
double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te), double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te),
// satellite positions // satellite positions
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. double d_Xn; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] double d_Yn; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] double d_Zn; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
// Satellite velocity // Satellite velocity
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] double d_VXn; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] double d_VYn; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] double d_VZn; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
// Satellite acceleration // Satellite acceleration
double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] double d_AXn; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] double d_AYn; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] double d_AZn; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
double d_B_n; //!< Health flag [dimensionless] double d_B_n; //!< Health flag [dimensionless]
double d_P; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless] double d_P; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless]
double d_N_T; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days] double d_N_T; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days]
@ -99,6 +106,19 @@ public:
double d_satClkDrift; //!< GPS clock error double d_satClkDrift; //!< GPS clock error
double d_dtr; //!< relativistic clock correction term double d_dtr; //!< relativistic clock correction term
// satellite positions
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
// Satellite velocity
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
// Satellite acceleration
double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
template<class Archive> template<class Archive>
/*! /*!
@ -137,7 +157,7 @@ public:
* \param transmitTime Time of ephemeris transmission * \param transmitTime Time of ephemeris transmission
* \return clock bias of satellite * \return clock bias of satellite
*/ */
double simplifiedSatellitePosition(double transmitTime); double simplified_satellite_position(double transmitTime);
/*! /*!
* \brief Compute the ECEF SV coordinates and ECEF velocity * \brief Compute the ECEF SV coordinates and ECEF velocity
@ -146,7 +166,7 @@ public:
* \param transmitTime Time of ephemeris transmission * \param transmitTime Time of ephemeris transmission
* \return clock bias of satellite * \return clock bias of satellite
*/ */
double satellitePosition(double transmitTime); double satellite_position(double transmitTime);
/*! /*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
@ -164,7 +184,7 @@ public:
/*! /*!
* Default constructor * Default constructor
*/ */
Glonass_GNAV_Ephemeris(); Glonass_Gnav_Ephemeris();
}; };
#endif #endif

577
src/core/system_parameters/glonass_gnav_navigation_message.cc Normal file
View File

@ -0,0 +1,577 @@
/*!
m * \file glonass_gnav_navigation_message.cc
* \brief Implementation of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1)
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
* \author Damian Miralles, 2017. dmiralles2009(at)gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (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 "glonass_gnav_navigation_message.h"
#include <cmath>
#include <iostream>
#include <gnss_satellite.h>
void Glonass_Gnav_Navigation_Message::reset()
{
b_valid_ephemeris_set_flag = false;
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
d_TOW_SF1 = 0; //!< Time of GPS Week from HOW word of Subframe 1 [s]
d_TOW_SF2 = 0; //!< Time of GPS Week from HOW word of Subframe 2 [s]
d_TOW_SF3 = 0; //!< Time of GPS Week from HOW word of Subframe 3 [s]
d_TOW_SF4 = 0; //!< Time of GPS Week from HOW word of Subframe 4 [s]
d_TOW_SF5 = 0; //!< Time of GPS Week from HOW word of Subframe 5 [s]
d_m = 0.0; //!< String number within frame [dimensionless]
d_t_k = 0.0; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds]
d_t_b = 0.0; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes]
d_M = 0.0; //!< Type of satellite transmitting navigation signal [dimensionless]
d_gamma_n = 0.0; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless]
d_tau_n = 0.0; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te),
d_B_n = 0.0; //!< Health flag [dimensionless]
d_P = 0.0; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless]
d_N_T = 0.0; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days]
d_F_T = 0.0; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless]
d_n = 0.0; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation
d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless]
d_E_n = 0.0; //!< Characterises "age" of a current information [days]
d_P_1 = 0.0; //!< Flag of the immediate data updating.
d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless]
d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless]
d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless]
d_l_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless]
// Almanac and Not Inmediate Information
d_tau_c = 0.0; //!< GLONASS time scale correction to UTC(SU) time. [s]
d_tau_gps = 0.0; //!< Correction to GPS time to GLONASS time [day]
d_N_4 = 0.0; //!< Four year interval number starting from 1996 [4 year interval]
d_N_A = 0.0; //!< Calendar day number within the four-year period beginning since the leap year [days]
d_n_A = 0.0; //!< Conventional number of satellite within GLONASS space segment [dimensionless]
d_H_n_A = 0.0; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless]
d_lambda_n_A = 0.0; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles]
d_t_lambda_n_A = 0.0; //!< Time of first ascending node passage [s]
d_Delta_i_n_A = 0.0; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles]
d_Delta_T_n_A = 0.0; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period]
d_Delta_T_n_A_dot = 0.0; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2]
d_epsilon_n_A = 0.0; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless]
d_omega_n_A = 0.0; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles]
d_M_n_A = 0.0; //!< Type of satellite n_A [dimensionless]
d_B1 = 0.0; //!< Coefficient to determine DeltaUT1 [s]
d_B2 = 0.0; //!< Coefficient to determine DeltaUT1 [s/msd]
d_KP = 0.0; //!< Notification on forthcoming leap second correction of UTC [dimensionless]
d_tau_n_A = 0.0; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s]
d_C_n_A = 0.0; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless]
std::map<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus
/*! \brief If true, enhanced level of integrity assurance.
*
* If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.
* That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound
* value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less
* than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of
* integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73
* times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an
* accompanying alert, is less than 1E-8 per hour.
*/
b_integrity_status_flag = false;
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
// Clock terms
d_satClkCorr = 0.0; // Satellite clock error
d_dtr = 0.0; // Relativistic clock correction term
d_satClkDrift = 0.0; // Satellite clock drift
// satellite identification info
int i_channel_ID = 0;
int i_satellite_freq_channel = 0; //!< SV PRN NUMBER
// time synchro
d_subframe_timestamp_ms = 0; //[ms]
// UTC parameters
bool flag_utc_model_valid = false; //!< If set, it indicates that the UTC model parameters are filled
// satellite positions
d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
d_satpos_Z = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
// Satellite velocity
d_satvel_X = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
d_satvel_Y = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
d_satvel_Z = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
// Satellite acceleration
d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
auto gnss_sat = Gnss_Satellite();
std::string _system ("GLONASS");
//TODO SHould number of channels be hardcoded?
for(unsigned int i = 1; i < 14; i++)
{
satelliteBlock[i] = gnss_sat.what_block(_system, i);
}
}
Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message()
{
reset();
}
bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter)
{
bool value;
if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1)
{
value = true;
}
else
{
value = false;
}
return value;
}
unsigned long int Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter)
{
unsigned long int value = 0;
int num_of_slices = parameter.size();
for (int i = 0; i < num_of_slices; i++)
{
for (int j = 0; j < parameter[i].second; j++)
{
value <<= 1; //shift left
if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1)
{
value += 1; // insert the bit
}
}
}
return value;
}
signed long int Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter)
{
signed long int value = 0;
int num_of_slices = parameter.size();
// Discriminate between 64 bits and 32 bits compiler
int long_int_size_bytes = sizeof(signed long int);
if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system
{
// read the MSB and perform the sign extension
if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1)
{
value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable
}
else
{
value &= 0;
}
for (int i = 0; i < num_of_slices; i++)
{
for (int j = 0; j < parameter[i].second; j++)
{
value <<= 1; //shift left
value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable)
if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1)
{
value += 1; // insert the bit
}
}
}
}
else // we assume we are in a 32 bits system
{
// read the MSB and perform the sign extension
if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1)
{
value ^= 0xFFFFFFFF;
}
else
{
value &= 0;
}
for (int i = 0; i < num_of_slices; i++)
{
for (int j = 0; j < parameter[i].second; j++)
{
value <<= 1; //shift left
value &= 0xFFFFFFFE; //reset the corresponding bit
if (bits[GPS_SUBFRAME_BITS - parameter[i].first - j] == 1)
{
value += 1; // insert the bit
}
}
}
}
return value;
}
int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string, int frame_ID)
{
int string_ID = 0;
// UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE
std::bitset<GLONASS_GNAV_STRING_BITS> string_bits(std::string(frame_string));
string_ID = static_cast<int>(read_navigation_unsigned(string_bits, STRING_ID));
// Decode all 15 string messages
switch (string_ID)
{
case 1:
//--- It is string 1 -----------------------------------------------
gnav_ephemeris.d_P_1 = static_cast<double>(read_navigation_unsigned(string_bits, P1));
gnav_ephemeris.d_t_k = static_cast<double>(read_navigation_unsigned(string_bits, T_K));
gnav_ephemeris.d_VXn = static_cast<double>(read_navigation_signed(string_bits, X_N_DOT));
gnav_ephemeris.d_AXn = static_cast<double>(read_navigation_signed(string_bits, X_N_DOT_DOT));
gnav_ephemeris.d_Xn = static_cast<double>(read_navigation_signed(string_bits, X_N));
break;
case 2:
//--- It is string 2 -----------------------------------------------
gnav_ephemeris.d_B_n = static_cast<double>(read_navigation_unsigned(string_bits, B_N));
gnav_ephemeris.d_P_2 = static_cast<double>(read_navigation_unsigned(string_bits, P2));
gnav_ephemeris.d_t_b = static_cast<double>(read_navigation_unsigned(string_bits, T_B));
gnav_ephemeris.d_VYn = static_cast<double>(read_navigation_signed(string_bits, Y_N_DOT));
gnav_ephemeris.d_AYn = static_cast<double>(read_navigation_signed(string_bits, Y_N_DOT_DOT));
gnav_ephemeris.d_Yn = static_cast<double>(read_navigation_signed(string_bits, Y_N));
break;
case 3:
// --- It is string 3 ----------------------------------------------
gnav_ephemeris.d_P_3 = static_cast<double>(read_navigation_unsigned(string_bits, P3));
gnav_ephemeris.d_gamma_n = static_cast<double>(read_navigation_signed(string_bits, GAMMA_N));
gnav_ephemeris.d_P = static_cast<double>(read_navigation_unsigned(string_bits, P));
gnav_ephemeris.d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, EPH_L_N));
gnav_ephemeris.d_VZn = static_cast<double>(read_navigation_signed(string_bits, Z_N_DOT));
gnav_ephemeris.d_AZn = static_cast<double>(read_navigation_signed(string_bits, Z_N_DOT_DOT));
gnav_ephemeris.d_Zn = static_cast<double>(read_navigation_signed(string_bits, Z_N));
break;
case 4:
// --- It is subframe 4 --------------------------------------------
// TODO signed vs unsigned reading from datasheet
gnav_ephemeris.d_tau_n = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N));
gnav_ephemeris.d_Delta_tau_n = static_cast<double>(read_navigation_signed(string_bits, DELTA_TAU_N));
gnav_ephemeris.d_E_n = static_cast<double>(read_navigation_unsigned(string_bits, E_N));
gnav_ephemeris.d_P_4 = static_cast<double>(read_navigation_unsigned(string_bits, P4));
gnav_ephemeris.d_F_T = static_cast<double>(read_navigation_unsigned(string_bits, F_T));
gnav_ephemeris.d_N_T = static_cast<double>(read_navigation_unsigned(string_bits, N_T));
gnav_ephemeris.d_n = static_cast<double>(read_navigation_unsigned(string_bits, N));
gnav_ephemeris.d_M = static_cast<double>(read_navigation_unsigned(string_bits, M));
break;
case 5:
// --- It is string 5 ----------------------------------------------
// TODO signed vs unsigned reading from datasheet
d_N_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
d_tau_c = static_cast<double>(read_navigation_unsigned(string_bits, TAU_C));
d_N_4 = static_cast<double>(read_navigation_unsigned(string_bits, N_4));
d_tau_c = static_cast<double>(read_navigation_unsigned(string_bits, TAU_C));
d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
break;
case 6:
// --- It is string 6 ----------------------------------------------
// TODO signed vs unsigned reading from datasheet
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
flag_almanac_str_6 = true;
break;
case 7:
// --- It is string 7 ----------------------------------------------
if (flag_almanac_str_6 == true)
{
// TODO signed vs unsigned reading from datasheet
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
flag_almanac_str_7 = true;
}
break;
case 8:
// --- It is string 8 ----------------------------------------------
// TODO signed vs unsigned reading from datasheet
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
flag_almanac_str_8 = true;
break;
case 9:
// --- It is string 9 ----------------------------------------------
if (flag_almanac_str_8 == true)
{
// TODO signed vs unsigned reading from datasheet
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
flag_almanac_str_9 = true;
}
case 10:
// --- It is string 8 ----------------------------------------------
// TODO signed vs unsigned reading from datasheet
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
flag_almanac_str_10 = true;
break;
case 11:
// --- It is string 9 ----------------------------------------------
if (flag_almanac_str_10 == true)
{
// TODO signed vs unsigned reading from datasheet
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
flag_almanac_str_11 = true;
}
break;
case 12:
// --- It is string 8 ----------------------------------------------
// TODO signed vs unsigned reading from datasheet
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
flag_almanac_str_12 = true;
break;
case 13:
// --- It is string 9 ----------------------------------------------
if (flag_almanac_str_12 == true)
{
// TODO signed vs unsigned reading from datasheet
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
flag_almanac_str_13 = true;
}
case 14:
// --- It is string 8 ----------------------------------------------
if( frame_number == 5)
{
gnav_utc_model.B1 = static_cast<double>(read_navigation_unsigned(string_bits, B1));
gnav_utc_model.B2 = static_cast<double>(read_navigation_unsigned(string_bits, B2));
}
else
{
// TODO signed vs unsigned reading from datasheet
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
flag_almanac_str_14 = true;
}
break;
case 15:
// --- It is string 9 ----------------------------------------------
if (frame_number != 5 and flag_almanac_str_14 = true )
{
// TODO signed vs unsigned reading from datasheet
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
flag_almanac_str_15 = true;
}
default:
break;
} // switch subframeID ...
return subframe_ID;
}
double Glonass_Gnav_Navigation_Message::utc_time(const double glonass_time_corrected) const
{
double t_utc;
t_utc = glonass_time_corrected + 3*3600 + d_tau_c;
return t_utc;
}
Glonass_Gnav_Ephemeris Glonass_Gnav_Navigation_Message::get_ephemeris()
{
Glonass_Gnav_Ephemeris ephemeris;
ephemeris.i_satellite_freq_channel = i_satellite_freq_channel;
ephemeris.d_m = d_m;
ephemeris.d_t_k = d_t_k;
ephemeris.d_t_b = d_t_b;
ephemeris.d_M = d_M;
ephemeris.d_gamma_n = d_gamma_n;
ephemeris.d_tau_n = d_tau_n;
// satellite positions
ephemeris.d_satpos_X = d_satpos_X;
ephemeris.d_satpos_Y = d_satpos_Y;
ephemeris.d_satpos_Z = d_satpos_Z;
// Satellite velocity
ephemeris.d_satvel_X = d_satvel_X;
ephemeris.d_satvel_Y = d_satvel_Y;
ephemeris.d_satvel_Z = d_satvel_Z;
// Satellite acceleration
ephemeris.d_satacc_X = d_satacc_X;
ephemeris.d_satacc_Y = d_satacc_Y;
ephemeris.d_satacc_Z = d_satacc_Z;
ephemeris.d_B_n = d_B_n;
ephemeris.d_P = d_P;
ephemeris.d_N_T = d_N_T;
ephemeris.d_F_T = d_F_T;
ephemeris.d_n = d_n;
ephemeris.d_Delta_tau_n = d_Delta_tau_n;
ephemeris.d_E_n = d_E_n;
ephemeris.d_P_1 = d_P_1;
ephemeris.d_P_2 = d_P_2;
ephemeris.d_P_3 = d_P_3;
ephemeris.d_P_4 = d_P_4;
ephemeris.d_l_n = d_l_n;
// clock terms derived from ephemeris data
ephemeris.d_satClkDrift = d_satClkDrift;
ephemeris.d_dtr = d_dtr;
return ephemeris;
}
Glonass_Gnav_Utc_Model Glonass_Gnav_Navigation_Message::get_utc_model()
{
Gps_Utc_Model utc_model;
utc_model.valid = flag_utc_model_valid;
// UTC parameters
utc_model.d_A1 = d_A1;
utc_model.d_A0 = d_A0;
utc_model.d_t_OT = d_t_OT;
utc_model.i_WN_T = i_WN_T;
utc_model.d_DeltaT_LS = d_DeltaT_LS;
utc_model.i_WN_LSF = i_WN_LSF;
utc_model.i_DN = i_DN;
utc_model.d_DeltaT_LSF = d_DeltaT_LSF;
// warning: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
flag_utc_model_valid = false;
return utc_model;
}
bool Glonass_Gnav_Navigation_Message::satellite_validation()
{
bool flag_data_valid = false;
b_valid_ephemeris_set_flag = false;
// First Step:
// check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception
// and check if the data have been filled (!=0)
if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0)
{
if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1)
{
flag_data_valid = true;
b_valid_ephemeris_set_flag = true;
}
}
return flag_data_valid;
}

203
src/core/system_parameters/glonass_gnav_navigation_message.h Normal file
View File

@ -0,0 +1,203 @@
/*!
* \file glonass_gnav_navigation_message.h
* \brief Interface of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1)
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
* \author Damian Miralles, 2017. dmiralles2009(at)gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (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/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_
#define GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_
#include <bitset>
#include <map>
#include <string>
#include <utility>
#include <vector>
#include "GLONASS_L1_CA.h"
#include "glonass_gnav_ephemeris.h"
//#include "gps_iono.h"
#include "glonass_gnav_almanac.h"
#include "glonass_gnav_utc_model.h"
/*!
* \brief This class decodes a GLONASS GNAV Data message as described in GLONASS ICD (Edition 5.1)
*
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdfdescribed in IS-GPS-200E
*
*/
class Glonass_Gnav_Navigation_Message
{
private:
unsigned long int read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter);
signed long int read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter);
bool read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter);
bool _CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits,boost::uint32_t checksum);
unsigned int get_frame_number(unsigned int satellite_slot_number);
public:
bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check
int Page_type_time_stamp;
int flag_even_word;
std::string page_Even;
bool flag_CRC_test;
Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded
Glonass_Gnav_Iono gnav_iono; //!< Iono corrections information
Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information
Glonass_Gnav_Almanac gnav_almanac[24]; //!< Almanac information for all 24 satellites
//!< Ephmeris Flags
bool flag_all_ephemeris; //!< Flag indicating that all strings containing ephemeris have been received
bool flag_ephemeris_str_1; //!< Flag indicating that ephemeris 1/4 (word 1) have been received
bool flag_ephemeris_str_2; //!< Flag indicating that ephemeris 2/4 (word 2) have been received
bool flag_ephemeris_str_3; //!< Flag indicating that ephemeris 3/4 (word 3) have been received
bool flag_ephemeris_str_4; //!< Flag indicating that ephemeris 4/4 (word 4) have been received
bool flag_iono_and_GST; //!< Flag indicating that ionospheric and GST parameters (word 5) have been received
bool flag_TOW_5;
bool flag_TOW_6;
bool flag_TOW_set; //!< it is true when page 5 or page 6 arrives
bool flag_utc_model; //!< Flag indicating that utc model parameters (word 6) have been received
bool flag_all_almanac; //!< Flag indicating that all almanac have been received
bool flag_almanac_str_6; //!< Flag indicating that almanac 1/4 (word 7) have been received
bool flag_almanac_str_7; //!< Flag indicating that almanac 2/4 (word 8) have been received
bool flag_almanac_str_8; //!< Flag indicating that almanac 3/4 (word 9) have been received
bool flag_almanac_str_9; //!< Flag indicating that almanac 4/4 (word 10) have been received
bool flag_almanac_str_10; //!< Flag indicating that almanac 4/4 (word 10) have been received
bool flag_almanac_str_11; //!< Flag indicating that almanac 4/4 (word 10) have been received
bool flag_almanac_str_12; //!< Flag indicating that almanac 4/4 (word 10) have been received
bool flag_almanac_str_13; //!< Flag indicating that almanac 4/4 (word 10) have been received
bool flag_almanac_str_14; //!< Flag indicating that almanac 4/4 (word 10) have been received
bool flag_almanac_str_15; //!< Flag indicating that almanac 4/4 (word 10) have been received
//broadcast orbit 1
//TODO Need to send the information regarding the frame number
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s]
double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s]
double d_TOW_F3; //!< Time of GPS Week from HOW word of Subframe 3 [s]
double d_TOW_F4; //!< Time of GPS Week from HOW word of Subframe 4 [s]
double d_TOW_F5; //!< Time of GPS Week from HOW word of Subframe 5 [s]
// Clock terms
double d_satClkCorr; // Satellite clock error
double d_dtr; // Relativistic clock correction term
double d_satClkDrift; // Satellite clock drift
// satellite identification info
int i_channel_ID;
unsigned int i_satellite_PRN; //!< SV PRN Number
int i_satellite_freq_channel; //!< SV Frequency Slot Number
int i_satellite_slot_number; //!< SV Orbit Slot Number
// UTC parameters
bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled
// satellite positions
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
// Satellite velocity
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
// Satellite acceleration
double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
/*!
* \brief Reset GLONASS GNAV Navigation Information
*/
void reset();
/*!
* \brief Obtain a GLONASS GNAV SV Ephemeris class filled with current SV data
*/
Glonass_Gnav_Ephemeris get_ephemeris();
/*!
// TODO Should I keep this function ?
* \brief Obtain a GPS ionospheric correction parameters class filled with current SV data
*/
Glonass_Gnav_Iono get_iono();
/*!
* \brief Obtain a GLONASS GNAV UTC model parameters class filled with current SV data
*/
Glonass_Gnav_Utc_Model get_utc_model();
/*
* \brief Returns a Galileo_Almanac object filled with the latest navigation data received
*/
Glonass_Gnav_Almanac get_almanac();
/*
* \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed
*/
bool have_new_ephemeris();
/*
* \brief Returns true if new Iono model has arrived. The flag is set to false when the function is executed
*/
bool have_new_iono();
/*
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
*/
bool have_new_utc_model();
/*
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
*/
bool have_new_almanac();
/*!
* \brief Decodes the GLONASS GNAV frame
*/
int string_decoder(char *string, int frame_ID);
/*!
* \brief Computes the Coordinated Universal Time (UTC) and returns it in [s]
*/
double utc_time(const double gpstime_corrected) const;
/*!
* Default constructor
*/
Glonass_Gnav_Navigation_Message();
};
#endif

5
src/core/system_parameters/glonass_gnav_utc_model.cc
View File

@ -35,6 +35,11 @@ Glonass_Gnav_Utc_Model::Glonass_Gnav_Utc_Model()
{ {
valid = false; valid = false;
d_tau_c = 0.0; d_tau_c = 0.0;
d_tau_gps = 0.0;
d_N_4 = 0.0;
d_N_A = 0.0;
d_B1 = 0.0;
d_B2 = 0.0;
} }
double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected) double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected)

37
src/core/system_parameters/glonass_gnav_utc_model.h
View File

@ -45,7 +45,30 @@ class Glonass_Gnav_Utc_Model
public: public:
bool valid; bool valid;
// UTC // UTC
double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s] double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s]
double d_tau_gps; //!< Correction to GPS time to GLONASS time [day]
double d_N_4; //!< Four year interval number starting from 1996 [4 year interval]
double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year [days]
double d_B1; //!< Coefficient to determine DeltaUT1 [s]
double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd]
template<class Archive>
/*!
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the almanac data on disk file.
*/
void serialize(Archive& archive, const unsigned int version)
{
using boost::serialization::make_nvp;
if(version){};
archive & make_nvp("valid",valid);
archive & make_nvp("d_tau_c", d_tau_c);
archive & make_nvp("d_tau_gps", d_tau_gps);
archive & make_nvp("d_N_4", d_N_4);
archive & make_nvp("d_N_A", d_N_A);
archive & make_nvp("d_B1", d_B1);
archive & make_nvp("d_B2", d_B2);
}
/*! /*!
* Default constructor * Default constructor
@ -58,18 +81,6 @@ public:
*/ */
double utc_time(double glonass_time_corrected); double utc_time(double glonass_time_corrected);
template<class Archive>
/*
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.
*/
void serialize(Archive& archive, const unsigned int version)
{
using boost::serialization::make_nvp;
if(version){};
archive & make_nvp("valid",valid);
archive & make_nvp("d_tau_c",d_tau_c);
}
}; };
#endif #endif

8
src/tests/test_main.cc
View File

@ -59,6 +59,11 @@
#include "sbas_ephemeris.h" #include "sbas_ephemeris.h"
#include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_almanac.h"
#include "glonass_gnav_iono.h"
#include "glonass_gnav_utc_model.h"
using google::LogMessage; using google::LogMessage;
DECLARE_string(log_dir); DECLARE_string(log_dir);
@ -139,7 +144,8 @@ DECLARE_string(log_dir);
#endif #endif
#endif #endif
#include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc"
#include "unit-tests/system-parameters/glonass_gnav_almanac_test.cc"
// For GPS NAVIGATION (L1) // For GPS NAVIGATION (L1)
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue; concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;

76
src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc Normal file
View File

@ -0,0 +1,76 @@
/*!
* \file code_generation_test.cc
* \brief This file implements tests for the generation of complex exponentials.
* \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (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 <complex>
#include <ctime>
#include "gps_sdr_signal_processing.h"
#include "gnss_signal_processing.h"
#include <complex>
#include <ctime>
#include "gnss_signal_processing.h"
#include "glonass_gnav_almanac.h"
// See A 3.2.3
TEST(GlonassGnavEphemerisTest, SatellitePosition)
{
double N_i = 615; // [days]
double t_i = 33300.0 // [seconds]
double Xoi = 10947.021572; // [km]
double Yoi = 13078.978287; // [km]
double Zoi = 18922.063362; // [km]
double Vxoi = -3.375497 // [m/s]
double Vyoi = -0.161453; // [Кm/s]
double Vzoi = 2.060844; // [Кm/s]
Glonass_Gnav_Ephemeris gnav_almanac;
gnav_almanac.d_N_A = 615; // [days]
gnav_almanac.d_lambda_n_A = -0.189986229; // [half cycles]
gnav_almanac.d_t_lambda_n_A = 27122.09375; // [second]
gnav_almanac.d_Delta_i_n_A = 0.011929512; // [half cycle]
gnav_almanac.d_Delta_T_n_A = -2655.76171875; // [seconds]
gnav_almanac.d_Delta_T_n_A_dot = 0.000549316; // [Secjnds/cycle2]
gnav_almanac.d_epsilon_n_A = 0.001482010; // [unitless]
gnav_almanac.d_omega_n_A = 0.440277100; // [Half cycle]
gnav_almanac.satellite_position(N_i, t_i);
ASSERT_TRUE(gnav_almanac.d_satpos_Xo - Xoi < DBL_EPSILON );
ASSERT_TRUE(gnav_almanac.d_satpos_Yo - Yoi < DBL_EPSILON );
ASSERT_TRUE(gnav_almanac.d_satpos_Zo - Zoi < DBL_EPSILON );
ASSERT_TRUE(gnav_almanac.d_satvel_Xo - Vxoi < DBL_EPSILON );
ASSERT_TRUE(gnav_almanac.d_satvel_Yo - Vyoi < DBL_EPSILON );
ASSERT_TRUE(gnav_almanac.d_satvel_Zo - Vzoi < DBL_EPSILON );
}

54
src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc Normal file
View File

@ -0,0 +1,54 @@
/*!
* \file code_generation_test.cc
* \brief This file implements tests for the generation of complex exponentials.
* \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (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 <complex>
#include <ctime>
#include "gnss_signal_processing.h"
#include "glonass_gnav_ephemeris.h"
TEST(GlonassGnavEphemerisTest, SatellitePosition)
{
Glonass_Gnav_Ephemeris gnav_eph;
gnav_eph.d_Xn = 12317.934082000;
gnav_eph.d_Yn = -2245.13232422;
gnav_eph.d_Zn = 22212.8173828;
gnav_eph.d_VXn = -1.25356674194;
gnav_eph.d_VYn = 2.774200439450;
gnav_eph.d_VZn = 0.9808206558230000;
gnav_eph.d_AXn = -0.931322574616e-9;
gnav_eph.d_AYn = 0.0000000000000000;
gnav_eph.d_AZn = -0.186264514923e-8;
gnav_eph.simplified_satellite_position(60);
}