diff --git a/.gitignore b/.gitignore index 2d1eebded..cbccacdac 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ docs/latex docs/GNSS-SDR_manual.pdf src/tests/data/output.dat thirdparty/ +.settings .project .cproject /install diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index df419aebd..9143a038b 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -39,6 +39,10 @@ set(SYSTEM_PARAMETERS_SOURCES gps_cnav_iono.cc gps_cnav_utc_model.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}) add_dependencies(gnss_system_parameters rtklib_lib glog-${glog_RELEASE}) target_link_libraries(gnss_system_parameters rtklib_lib ${Boost_LIBRARIES}) - diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index d3c234801..cc5ff2db4 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -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_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_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_SEMI_MAJOR_AXIS = 6378136; //!< Semi-major axis of Earth [m] 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_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_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_1 = 4069°02′02′′,52; +const double GLONASS_TAU_0 = -0.005835151531174e3; //!< (-334 deg 19 min 46.40 sec) [rad]; +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_Q1 = 477198°50′56′′,79; -const double GLONASS_MOON_OMEGA_0 = 259°10′59′′,79; -const double GLONASS_MOON_OMEGA_1 = -1934°08′31′′,23; +const double GLONASS_MOON_Q0 = -0.001115184961435e3; //!< (-63 deg 53 min 43.41 sec) [rad] +const double GLONASS_MOON_Q1 = 8.328691103668023e3; //!< (477198 deg 50 min 56.79 sec) [rad] +const double GLONASS_MOON_OMEGA_0 = 0.004523601514852e3; //!< (259 deg 10 min 59.79 sec) [rad] +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_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_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_Q0 = 358°28′33′′,04; -const double GLONASS_SUN_Q1 = 129596579′′,10; -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_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit +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 = 0.006256583774423e3; //!< (358 deg 28 min 33.04 sec) [rad] +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_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-major axis of solar orbit [km]; +const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit // carrier and code frequencies const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz] @@ -133,7 +133,7 @@ const std::vector> Y_N({{51,27}}); const std::vector> P3({{6,1}}); const std::vector> GAMMA_N({{7,11}}); const std::vector> P({{19,2}}); -const std::vector> L_N({{21,1}}); +const std::vector> EPH_L_N({{21,1}}); const std::vector> Z_N_DOT ({{22,24}}); const std::vector> Z_N_DOT_DOT ({{46,5}}); const std::vector> Z_N({{51,27}}); @@ -153,7 +153,7 @@ const std::vector> N_A({{6,11}}); const std::vector> TAU_C({{17,32}}); const std::vector> N_4({{50,5}}); const std::vector> TAU_GPS({{55,22}}); -const std::vector> L_N({{77,1}}); +const std::vector> ALM_L_N({{77,1}}); // STRING 6, 8, 10, 12, 14 const std::vector> C_N({{6,1}}); @@ -170,7 +170,7 @@ const std::vector> T_LAMBDA_N_A({{22,21}}); const std::vector> DELTA_T_N_A({{43,22}}); const std::vector> DELTA_T_DOT_N_A({{65,7}}); const std::vector> H_N_A({{72,5}}); -const std::vector> L_N({{77,1}}); +//const std::vector> L_N({{77,1}}); #endif /* GNSS_SDR_GLONASS_L1_CA_H_ */ diff --git a/src/core/system_parameters/glonass_gnav_almanac.cc b/src/core/system_parameters/glonass_gnav_almanac.cc index 45174cabb..ae1f492c7 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.cc +++ b/src/core/system_parameters/glonass_gnav_almanac.cc @@ -1,8 +1,7 @@ /*! * \file glonass_gnav_almanac.cc - * \brief Interface of a GLONASS GNAV ALMANAC storage - * - * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + * \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 * \author Damian Miralles , 2017. dmiralles2009(at)gmail.com * * ------------------------------------------------------------------------- @@ -29,29 +28,149 @@ * * ------------------------------------------------------------------------- */ - #include "glonass_gnav_almanac.h" +#include +#include "GLONASS_L1_CA.h" +#include "gnss_satellite.h" Glonass_Gnav_Almanac::Glonass_Gnav_Almanac() { - int i_satellite_freq_channel = 0; - double d_tau_c = 0.0; - double d_tau_gps = 0.0; - double d_N_4 = 0.0; - double d_N_A = 0.0; - double d_n_A = 0.0; - double d_H_n_A = 0.0; - double d_lambda_n_A = 0.0; - double d_t_lambda_n_A = 0.0; - double d_Delta_i_n_A = 0.0; - double d_Delta_T_n_A = 0.0; - double d_Delta_T_n_A_dot = 0.0; - double d_epsilon_n_A = 0.0; - double d_omega_n_A = 0.0; - double d_M_n_A = 0.0; - double d_B1 = 0.0; - double d_B2 = 0.0; - double d_KP = 0.0; - double d_tau_n_A = 0.0; - double d_C_n_A = 0.0; + i_satellite_freq_channel = 0; + d_tau_c = 0.0; + d_tau_gps = 0.0; + d_N_4 = 0.0; + d_N_A = 0.0; + d_n_A = 0.0; + d_H_n_A = 0.0; + d_lambda_n_A = 0.0; + d_t_lambda_n_A = 0.0; + d_Delta_i_n_A = 0.0; + d_Delta_T_n_A = 0.0; + d_Delta_T_n_A_dot = 0.0; + d_epsilon_n_A = 0.0; + d_omega_n_A = 0.0; + d_M_n_A = 0.0; + d_KP = 0.0; + d_tau_n_A = 0.0; + 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; + } diff --git a/src/core/system_parameters/glonass_gnav_almanac.h b/src/core/system_parameters/glonass_gnav_almanac.h index 3e13802fc..f4ce97312 100644 --- a/src/core/system_parameters/glonass_gnav_almanac.h +++ b/src/core/system_parameters/glonass_gnav_almanac.h @@ -32,16 +32,20 @@ #ifndef GNSS_SDR_GLONASS_ALMANAC_H_ #define GNSS_SDR_GLONASS_ALMANAC_H_ +#include +#include +#include "boost/assign.hpp" +#include /*! - * \brief This class is a storage for the GLONASS SV ALMANAC data as described in IS-GPS-200E - * \todo Add proper links for GLONASS ICD - * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + * \brief This class is a storage for the GLONASS SV ALMANAC data as described GLONASS ICD (Edition 5.1) + * + * See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf */ class Glonass_Gnav_Almanac { 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_gps; //!< Correction to GPS time to GLONASS time [day] 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_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_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_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_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_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_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] + + // 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 + /*! + * \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 */ diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index a6e6436c2..7f48c1051 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -1,9 +1,11 @@ /*! - * \file gps_ephemeris.cc - * \brief Interface of a GPS EPHEMERIS storage and orbital model functions + * \file glonass_gnav_ephemeris.cc + * \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 - * \author Javier Arribas, 2013. jarribas(at)cttc.es + * See Russian Institute of Space Device Engineering, + “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 #include "GLONASS_L1_CA.h" #include "gnss_satellite.h" -Gps_Ephemeris::Gps_Ephemeris() +Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() { i_satellite_freq_channel = 0; 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 sigma_days; - double tau_prime; - double Omega_moon; - double q_moon; - double q_sun; - 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 T = 0.0; + double sigma_days = 0.0; + double tau_prime = 0.0; + double Omega_moon = 0.0; + double q_moon = 0.0; + double q_sun = 0.0; - double sin_theta_moon; - double cos_theta_moon; - double theta_moon; - double xi_moon_e; - double eta_moon_e; - double zeta_moon_e; + double xi_star = 0.0; + double eta_star = 0.0; + double zeta_star = 0.0; + double E_moon = 0.0; + double E_sun = 0.0; + 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 cos_theta_sun; - double theta_sun; - double xi_sun_e; - double eta_sun_e; - double zeta_sun_e; + double sin_theta_moon = 0.0; + double cos_theta_moon = 0.0; + double theta_moon = 0.0; + double xi_moon_e = 0.0; + double eta_moon_e = 0.0; + double zeta_moon_e = 0.0; + double r_moon_e = 0.0; - double r_moon_e; - double mu_bar_moon; - double x_bar_moon; - double y_bar_moon; - double Delta_o_moon; - double mu_bar_sun; - double x_bar_sun; - double y_bar_sun; - double Delta_o_sun; + double sin_theta_sun = 0.0; + double cos_theta_sun = 0.0; + double theta_sun = 0.0; + double xi_sun_e = 0.0; + double eta_sun_e = 0.0; + double zeta_sun_e = 0.0; + double r_sun_e = 0.0; + 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; // fabs(tk)) { // 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. { @@ -322,49 +382,49 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime) } // 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_3 = 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; - 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; - 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; + 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 * 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 * 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; - dy_1 = Vy0; - dz_1 = Vz0; + dx_1 = Vx_0; + dy_1 = Vy_0; + dz_1 = Vz_0; // Runge-Kutta Integration Stage K2 - Vx_1 = Vx0 + 1/2 * tau * dVx_1; - Vy_1 = Vy0 + 1/2 * tau * dVy_1; - Vz_1 = Vz0 + 1/2 * tau * dVz_1; + Vx_1 = Vx_0 + 1/2 * tau * dVx_1; + Vy_1 = Vy_0 + 1/2 * tau * dVy_1; + Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - x_1 = x0 + 1/2 * tau * dx_1; - y_1 = y0 + 1/2 * tau * dy_1; - z_1 = z0 + 1/2 * tau * dz_1; + x_1 = x_0 + 1/2 * tau * dx_1; + y_1 = y_0 + 1/2 * tau * dy_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_2 = r1*r1; r1_3 = 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; - 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; - 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; + 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 * 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 * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; dx_2 = Vx_1; dy_2 = Vy_1; dz_2 = Vz_1; // Runge-Kutta Integration Stage K2 - Vx_2 = Vx0 + 1/2 * tau * dVx_2; - Vy_2 = Vy0 + 1/2 * tau * dVy_2; - Vz_2 = Vz0 + 1/2 * tau * dVz_2; + Vx_2 = Vx_0 + 1/2 * tau * dVx_2; + Vy_2 = Vy_0 + 1/2 * tau * dVy_2; + Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - x_2 = x0 + 1/2 * tau * dx_2; - y_2 = y0 + 1/2 * tau * dy_2; - z_2 = z0 + 1/2 * tau * dz_2; + x_2 = x_0 + 1/2 * tau * dx_2; + y_2 = y_0 + 1/2 * tau * dy_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_2 = r2*r2; @@ -372,109 +432,205 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime) 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; - 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; - 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; + 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 * 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 * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; dx_3 = Vx_2; dy_3 = Vy_2; dz_3 = Vz_2; // Runge-Kutta Integration Stage K3 - Vx_3 = Vx0 + tau * dVx_3; - Vy_3 = Vy0 + tau * dVy_3; - Vz_3 = Vz0 + tau * dVz_3; + Vx_3 = Vx_0 + tau * dVx_3; + Vy_3 = Vy_0 + tau * dVy_3; + Vz_3 = Vz_0 + tau * dVz_3; - x_3 = x0 + tau * dx_3; - y_3 = y0 + tau * dy_3; - z_3 = z0 + tau * dz_3; + x_3 = x_0 + tau * dx_3; + y_3 = y_0 + tau * dy_3; + z_3 = z_0 + tau * dz_3; r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); r3_2 = r3*r3; r3_3 = 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; - 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; - 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; + 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 * 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 * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; dx_4 = Vx_3; dy_4 = Vy_3; dz_4 = Vz_3; // Final results showcased here - Vx0 = Vx0 + 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 ); - Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); + Vx_0 = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); + Vy_0 = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_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 ); - y0 = y0 + 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 ); + x_0 = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); + y_0 = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_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 - d_satpos_X = x0; - d_satpos_Y = y0; - d_satpos_Z = z0; + d_satpos_X = x_0; + d_satpos_Y = y_0; + d_satpos_Z = z_0; - d_satvel_X = Vx0; - d_satvel_Y = Vy0; - d_satvel_Z = Vz0; + d_satvel_X = Vx_0; + d_satvel_Y = Vy_0; + d_satvel_Z = Vz_0; - d_satacc_X = d_satacc_X; // No change in accelerations reported over interval - d_satacc_Y = d_satacc_Y; // No change in accelerations reported over interval - d_satacc_Z = d_satacc_Z; // No change in accelerations reported over interval + d_satacc_X = d_AXn; // No change in accelerations reported over interval + d_satacc_Y = d_AYn; // No change in accelerations reported over interval + d_satacc_Z = d_AZn; // No change in accelerations reported over interval // 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 */ - 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 tk = 0.0; + double tau = 0.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 dt = check_t(transmitTime - d_t_b); // 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 - tk = dt - satClkCorr; + tk = dt - d_satClkDrift; // Check if to integrate forward or backward - if tk < 0 - tau = -60; + if (tk < 0) + { + tau = -60; + } else - tau = 60; - end + { + tau = 60; + } // Coordinates transformation to an inertial reference frame // x,y,z coordinates to meters - x0 = d_satpos_X * 1e3; - y0 = d_satpos_Y * 1e3; - z0 = d_satpos_Z * 1e3; + x_0 = d_Xn * 1e3; + y_0 = d_Yn * 1e3; + z_0 = d_Zn * 1e3; // x,y,z velocities to meters/s - Vx0 = d_satvel_X * 1e3; - Vy0 = d_satvel_Y * 1e3; - Vz0 = d_satvel_Z * 1e3; + Vx_0 = d_VXn * 1e3; + Vy_0 = d_VYn * 1e3; + Vz_0 = d_VZn * 1e3; // x,y,z accelerations to meters/sec^2 - Ax0 = d_satacc_X * 1e3; - Ay0 = d_satacc_Y * 1e3; - Az0 = d_satacc_Z * 1e3; + Ax_0 = d_AXn * 1e3; + Ay_0 = d_AYn * 1e3; + Az_0 = d_AZn * 1e3; 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 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. { @@ -493,53 +649,53 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) } // 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_3 = r0*r0*r0; r0_5 = r0*r0*r0*r0*r0; - dx_1 = Vx0; - dy_1 = Vy0; - dz_1 = Vz0; + dx_1 = Vx_0; + dy_1 = Vy_0; + 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; - 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; - 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; + 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 * 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 * 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; - dy_1 = Vy0; - dz_1 = Vz0; + dx_1 = Vx_0; + dy_1 = Vy_0; + dz_1 = Vz_0; // Runge-Kutta Integration Stage K2 - Vx_1 = Vx0 + 1/2 * tau * dVx_1; - Vy_1 = Vy0 + 1/2 * tau * dVy_1; - Vz_1 = Vz0 + 1/2 * tau * dVz_1; + Vx_1 = Vx_0 + 1/2 * tau * dVx_1; + Vy_1 = Vy_0 + 1/2 * tau * dVy_1; + Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - x_1 = x0 + 1/2 * tau * dx_1; - y_1 = y0 + 1/2 * tau * dy_1; - z_1 = z0 + 1/2 * tau * dz_1; + x_1 = x_0 + 1/2 * tau * dx_1; + y_1 = y_0 + 1/2 * tau * dy_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_2 = r1*r1; r1_3 = 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; - 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; - 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; + 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 * 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 * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; dx_2 = Vx_1; dy_2 = Vy_1; dz_2 = Vz_1; // Runge-Kutta Integration Stage K2 - Vx_2 = Vx0 + 1/2 * tau * dVx_2; - Vy_2 = Vy0 + 1/2 * tau * dVy_2; - Vz_2 = Vz0 + 1/2 * tau * dVz_2; + Vx_2 = Vx_0 + 1/2 * tau * dVx_2; + Vy_2 = Vy_0 + 1/2 * tau * dVy_2; + Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - x_2 = x0 + 1/2 * tau * dx_2; - y_2 = y0 + 1/2 * tau * dy_2; - z_2 = z0 + 1/2 * tau * dz_2; + x_2 = x_0 + 1/2 * tau * dx_2; + y_2 = y_0 + 1/2 * tau * dy_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_2 = r2*r2; @@ -547,54 +703,54 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime) 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; - 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; - 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; + 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 * 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 * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; dx_3 = Vx_2; dy_3 = Vy_2; dz_3 = Vz_2; // Runge-Kutta Integration Stage K3 - Vx_3 = Vx0 + tau * dVx_3; - Vy_3 = Vy0 + tau * dVy_3; - Vz_3 = Vz0 + tau * dVz_3; + Vx_3 = Vx_0 + tau * dVx_3; + Vy_3 = Vy_0 + tau * dVy_3; + Vz_3 = Vz_0 + tau * dVz_3; - x_3 = x0 + tau * dx_3; - y_3 = y0 + tau * dy_3; - z_3 = z0 + tau * dz_3; + x_3 = x_0 + tau * dx_3; + y_3 = y_0 + tau * dy_3; + z_3 = z_0 + tau * dz_3; r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); r3_2 = r3*r3; r3_3 = 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; - 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; - 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; + 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 * 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 * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; dx_4 = Vx_3; dy_4 = Vy_3; dz_4 = Vz_3; // Final results showcased here - Vx0 = Vx0 + 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 ); - Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); + d_satvel_X = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); + d_satvel_Y = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_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 ); - y0 = y0 + 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_X = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); + d_satpos_Y = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); + d_satpos_Z = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); } // 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 */ - 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; } diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 6a2ab046d..d0edcc69b 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -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: /* @@ -57,10 +57,15 @@ private: */ double check_t(double time); - double gravitational_perturbations(const double mu); + void gravitational_perturbations(); 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_Jy_sun; //!< Sun gravitational perturbation + double d_Jz_sun; //!< Sun gravitational perturbation public: 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_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), + // 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] + double d_Xn; //!< Earth-fixed coordinate x 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_Zn; //!< 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] + double d_VXn; //!< Earth-fixed velocity coordinate x 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_VZn; //!< 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] + double d_AXn; //!< Earth-fixed acceleration coordinate x 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_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_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] @@ -99,6 +106,19 @@ public: double d_satClkDrift; //!< GPS clock error 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 /*! @@ -137,7 +157,7 @@ public: * \param transmitTime Time of ephemeris transmission * \return clock bias of satellite */ - double simplifiedSatellitePosition(double transmitTime); + double simplified_satellite_position(double transmitTime); /*! * \brief Compute the ECEF SV coordinates and ECEF velocity @@ -146,7 +166,7 @@ public: * \param transmitTime Time of ephemeris transmission * \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 @@ -164,7 +184,7 @@ public: /*! * Default constructor */ - Glonass_GNAV_Ephemeris(); + Glonass_Gnav_Ephemeris(); }; #endif diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc new file mode 100644 index 000000000..0dc90d25d --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "glonass_gnav_navigation_message.h" +#include +#include +#include + + +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 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 bits, const std::vector> 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 bits, const std::vector> 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 bits, const std::vector> 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 string_bits(std::string(frame_string)); + + string_ID = static_cast(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(read_navigation_unsigned(string_bits, P1)); + gnav_ephemeris.d_t_k = static_cast(read_navigation_unsigned(string_bits, T_K)); + gnav_ephemeris.d_VXn = static_cast(read_navigation_signed(string_bits, X_N_DOT)); + gnav_ephemeris.d_AXn = static_cast(read_navigation_signed(string_bits, X_N_DOT_DOT)); + gnav_ephemeris.d_Xn = static_cast(read_navigation_signed(string_bits, X_N)); + + break; + + case 2: + //--- It is string 2 ----------------------------------------------- + gnav_ephemeris.d_B_n = static_cast(read_navigation_unsigned(string_bits, B_N)); + gnav_ephemeris.d_P_2 = static_cast(read_navigation_unsigned(string_bits, P2)); + gnav_ephemeris.d_t_b = static_cast(read_navigation_unsigned(string_bits, T_B)); + gnav_ephemeris.d_VYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT)); + gnav_ephemeris.d_AYn = static_cast(read_navigation_signed(string_bits, Y_N_DOT_DOT)); + gnav_ephemeris.d_Yn = static_cast(read_navigation_signed(string_bits, Y_N)); + + break; + + case 3: + // --- It is string 3 ---------------------------------------------- + gnav_ephemeris.d_P_3 = static_cast(read_navigation_unsigned(string_bits, P3)); + gnav_ephemeris.d_gamma_n = static_cast(read_navigation_signed(string_bits, GAMMA_N)); + gnav_ephemeris.d_P = static_cast(read_navigation_unsigned(string_bits, P)); + gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, EPH_L_N)); + gnav_ephemeris.d_VZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT)); + gnav_ephemeris.d_AZn = static_cast(read_navigation_signed(string_bits, Z_N_DOT_DOT)); + gnav_ephemeris.d_Zn = static_cast(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(read_navigation_unsigned(string_bits, TAU_N)); + gnav_ephemeris.d_Delta_tau_n = static_cast(read_navigation_signed(string_bits, DELTA_TAU_N)); + gnav_ephemeris.d_E_n = static_cast(read_navigation_unsigned(string_bits, E_N)); + gnav_ephemeris.d_P_4 = static_cast(read_navigation_unsigned(string_bits, P4)); + gnav_ephemeris.d_F_T = static_cast(read_navigation_unsigned(string_bits, F_T)); + gnav_ephemeris.d_N_T = static_cast(read_navigation_unsigned(string_bits, N_T)); + gnav_ephemeris.d_n = static_cast(read_navigation_unsigned(string_bits, N)); + gnav_ephemeris.d_M = static_cast(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(read_navigation_unsigned(string_bits, N_A)); + d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + d_N_4 = static_cast(read_navigation_unsigned(string_bits, N_4)); + d_tau_c = static_cast(read_navigation_unsigned(string_bits, TAU_C)); + d_l_n = static_cast(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(read_navigation_unsigned(string_bits, N_A)); + + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(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(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(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(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(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(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(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(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(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(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(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(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(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(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(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(read_navigation_unsigned(string_bits, B1)); + gnav_utc_model.B2 = static_cast(read_navigation_unsigned(string_bits, B2)); + } + else + { + // TODO signed vs unsigned reading from datasheet + i_satellite_slot_number = = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast(read_navigation_unsigned(string_bits, C_N)); + gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast(read_navigation_unsigned(string_bits, M_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast(read_navigation_unsigned(string_bits, N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast(read_navigation_unsigned(string_bits, TAU_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_I_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast(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(read_navigation_unsigned(string_bits, OMEGA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(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; +} diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.h b/src/core/system_parameters/glonass_gnav_navigation_message.h new file mode 100644 index 000000000..0098f6d9a --- /dev/null +++ b/src/core/system_parameters/glonass_gnav_navigation_message.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_ +#define GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_ + + +#include +#include +#include +#include +#include +#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 bits, const std::vector> parameter); + signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); + bool read_navigation_bool(std::bitset bits, const std::vector> parameter); + bool _CRC_test(std::bitset 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 diff --git a/src/core/system_parameters/glonass_gnav_utc_model.cc b/src/core/system_parameters/glonass_gnav_utc_model.cc index 921195298..55ca5ccee 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.cc +++ b/src/core/system_parameters/glonass_gnav_utc_model.cc @@ -35,6 +35,11 @@ Glonass_Gnav_Utc_Model::Glonass_Gnav_Utc_Model() { valid = false; 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) @@ -43,6 +48,6 @@ double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected) // GLONASS Time is relative to UTC Moscow, so we simply add its time difference t_utc = glonass_time_corrected + 3*3600 + d_tau_c; - + return t_utc; } diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index b08c4bdce..51d47f6c3 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -45,7 +45,30 @@ class Glonass_Gnav_Utc_Model public: bool valid; // 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 + /*! + * \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 @@ -58,18 +81,6 @@ public: */ double utc_time(double glonass_time_corrected); - template - /* - * \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 diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 60b33a4d0..36c5620d7 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -59,6 +59,11 @@ #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; DECLARE_string(log_dir); @@ -139,7 +144,8 @@ DECLARE_string(log_dir); #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) concurrent_queue global_gps_acq_assist_queue; diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc new file mode 100644 index 000000000..650daa341 --- /dev/null +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_almanac_test.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include "gps_sdr_signal_processing.h" +#include "gnss_signal_processing.h" + + + +#include +#include +#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 ); +} diff --git a/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc b/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc new file mode 100644 index 000000000..44497f114 --- /dev/null +++ b/src/tests/unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#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); +}