diff --git a/src/core/libs/gnss_sdr_supl_client.cc b/src/core/libs/gnss_sdr_supl_client.cc index 10dda2be4..90c5fd0e6 100644 --- a/src/core/libs/gnss_sdr_supl_client.cc +++ b/src/core/libs/gnss_sdr_supl_client.cc @@ -19,6 +19,7 @@ #include "gnss_sdr_supl_client.h" #include "GPS_L1_CA.h" +#include "MATH_CONSTANTS.h" #include #include #include @@ -858,6 +859,7 @@ bool Gnss_Sdr_Supl_Client::read_gal_almanac_from_gsa(const std::string& file_nam Galileo_Almanac gal_alm; try { + const double sqrtAnominal = 5440.588203494; // square root of Galileo nominal orbit semi-major axis uint32_t prn = static_cast(std::stoi(almanac.child_value("SVID"))); gal_alm.PRN = prn; gal_alm.toa = std::stoi(almanac.child("almanac").child_value("t0a")); @@ -866,7 +868,7 @@ bool Gnss_Sdr_Supl_Client::read_gal_almanac_from_gsa(const std::string& file_nam gal_alm.delta_i = std::stod(almanac.child("almanac").child_value("deltai")); gal_alm.M_0 = std::stod(almanac.child("almanac").child_value("m0")); gal_alm.ecc = std::stod(almanac.child("almanac").child_value("ecc")); - gal_alm.sqrtA = std::stod(almanac.child("almanac").child_value("aSqRoot")); + gal_alm.sqrtA = std::stod(almanac.child("almanac").child_value("aSqRoot")) + sqrtAnominal; gal_alm.OMEGA_0 = std::stod(almanac.child("almanac").child_value("omega0")); gal_alm.omega = std::stod(almanac.child("almanac").child_value("w")); gal_alm.OMEGAdot = std::stod(almanac.child("almanac").child_value("omegaDot")); diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index e88306991..49d8fdb80 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -6,6 +6,7 @@ set(SYSTEM_PARAMETERS_SOURCES + gnss_almanac.cc gnss_ephemeris.cc gnss_satellite.cc gnss_signal.cc diff --git a/src/core/system_parameters/beidou_dnav_almanac.h b/src/core/system_parameters/beidou_dnav_almanac.h index 03610990f..6bf616fc6 100644 --- a/src/core/system_parameters/beidou_dnav_almanac.h +++ b/src/core/system_parameters/beidou_dnav_almanac.h @@ -36,7 +36,10 @@ public: /*! * Default constructor */ - Beidou_Dnav_Almanac() = default; + Beidou_Dnav_Almanac() + { + this->System = 'B'; + }; int SV_health{}; //!< SV Health diff --git a/src/core/system_parameters/galileo_almanac.h b/src/core/system_parameters/galileo_almanac.h index a76a3af66..9b65f84bd 100644 --- a/src/core/system_parameters/galileo_almanac.h +++ b/src/core/system_parameters/galileo_almanac.h @@ -36,7 +36,10 @@ public: /*! * Default constructor */ - Galileo_Almanac() = default; + Galileo_Almanac() + { + this->System = 'E'; + }; int32_t IODa{}; int32_t E5b_HS{}; diff --git a/src/core/system_parameters/gnss_almanac.cc b/src/core/system_parameters/gnss_almanac.cc new file mode 100644 index 000000000..2c609a53d --- /dev/null +++ b/src/core/system_parameters/gnss_almanac.cc @@ -0,0 +1,275 @@ +/*! + * \file gnss_almanac.cc + * \brief Base class for GNSS almanac storage + * \author Carles Fernandez, 2022. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "gnss_almanac.h" +#include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" +#include +#include +#include +#include +#include + +double Gnss_Almanac::check_t(double time) const +{ + const double half_week = 302400.0; // seconds + double corrTime = time; + if (time > half_week) + { + corrTime = time - 2.0 * half_week; + } + else if (time < -half_week) + { + corrTime = time + 2.0 * half_week; + } + return corrTime; +} + +double Gnss_Almanac::predicted_doppler(double rx_time_s, + double lat, + double lon, + double h, + double ve, + double vn, + double vu, + int band) const +{ + const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m) + const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84) + const double lat_rad = lat * D2R; + const double lon_rad = lon * D2R; + + const double sinp = sin(lat_rad); + const double cosp = cos(lat_rad); + const double sinl = sin(lon_rad); + const double cosl = cos(lon_rad); + + const double e2 = FE_WGS84 * (2.0 - FE_WGS84); + const double v = RE_WGS84 / std::sqrt(1.0 - e2 * sinp * sinp); + + // Position in EFEF + const std::vector pos_rx = {(v + h) * cosp * cosl, (v + h) * cosp * sinl, (v * (1.0 - e2) + h) * sinp}; + + // Velovity in EFEF + const double t = cosp * vu - sinp * vn; + const std::vector vel_rx = {cosl * t - sinl * ve, sinl * t + cosl * ve, sinp * vu + cosp * vn}; + + std::array sat_pos_vel = {0}; + satellitePosVelComputation(rx_time_s, sat_pos_vel); + const std::vector pos_sat = {sat_pos_vel[0], sat_pos_vel[1], sat_pos_vel[2]}; + const std::vector vel_sat = {sat_pos_vel[3], sat_pos_vel[4], sat_pos_vel[5]}; + + std::vector x_sr = pos_sat; + std::transform(x_sr.begin(), x_sr.end(), pos_rx.begin(), x_sr.begin(), std::minus()); // pos_sat - pos_rx + + const double norm_x_sr = std::sqrt(std::inner_product(x_sr.begin(), x_sr.end(), x_sr.begin(), 0.0)); // Euclidean norm + + std::vector v_sr = vel_sat; + std::transform(v_sr.begin(), v_sr.end(), vel_rx.begin(), v_sr.begin(), std::minus()); // vel_sat - vel_rx + + const double radial_vel = std::inner_product(v_sr.begin(), v_sr.end(), x_sr.begin(), 0.0) / norm_x_sr; + const double predicted_doppler_normalized = -(radial_vel / SPEED_OF_LIGHT_M_S); + double predicted_doppler = 0.0; + if (this->System == 'E') // Galileo + { + if (band == 1) + { + predicted_doppler = predicted_doppler_normalized * FREQ1; + } + else if (band == 5) + { + predicted_doppler = predicted_doppler_normalized * FREQ5; + } + else if (band == 6) + { + predicted_doppler = predicted_doppler_normalized * FREQ6; + } + else if (band == 7) + { + predicted_doppler = predicted_doppler_normalized * FREQ7; + } + else if (band == 8) + { + predicted_doppler = predicted_doppler_normalized * FREQ8; + } + else + { + predicted_doppler = 0.0; + } + } + else if (this->System == 'G') // GPS + { + if (band == 1) + { + predicted_doppler = predicted_doppler_normalized * FREQ1; + } + else if (band == 2) + { + predicted_doppler = predicted_doppler_normalized * FREQ2; + } + else if (band == 5) + { + predicted_doppler = predicted_doppler_normalized * FREQ5; + } + else + { + predicted_doppler = 0.0; + } + } + else if (this->System == 'B') // Beidou + { + if (band == 1) + { + predicted_doppler = predicted_doppler_normalized * FREQ1_BDS; + } + else if (band == 2) + { + predicted_doppler = predicted_doppler_normalized * FREQ2_BDS; + } + else if (band == 3) + { + predicted_doppler = predicted_doppler_normalized * FREQ3_BDS; + } + else + { + predicted_doppler = 0.0; + } + } + else + { + predicted_doppler = 0.0; + } + return predicted_doppler; +} + + +void Gnss_Almanac::satellitePosVelComputation(double transmitTime, std::array& pos_vel_dtr) const +{ + // Restore semi-major axis + const double a = this->sqrtA * this->sqrtA; + + // Computed mean motion + double n; + if (this->System == 'E') + { + n = sqrt(GALILEO_GM / (a * a * a)); + } + else if (this->System == 'B') + { + n = sqrt(BEIDOU_GM / (a * a * a)); + } + else + { + n = sqrt(GPS_GM / (a * a * a)); + } + + // Time from ephemeris reference epoch + const double tk = check_t(transmitTime - static_cast(this->toa)); + + // Mean anomaly + const double M = this->M_0 * GNSS_PI + n * tk; + + // Initial guess of eccentric anomaly + double E = M; + double E_old; + double dE; + + // --- Iteratively compute eccentric anomaly ------------------------------- + for (int32_t ii = 1; ii < 20; ii++) + { + E_old = E; + E = M + this->ecc * sin(E); + dE = fmod(E - E_old, 2.0 * GNSS_PI); + if (fabs(dE) < 1e-12) + { + // Necessary precision is reached, exit from the loop + break; + } + } + + const double sek = sin(E); + const double cek = cos(E); + const double OneMinusecosE = 1.0 - this->ecc * cek; + const double sq1e2 = sqrt(1.0 - this->ecc * this->ecc); + const double ekdot = n / OneMinusecosE; + + // Compute the true anomaly + const double tmp_Y = sq1e2 * sek; + const double tmp_X = cek - this->ecc; + const double nu = atan2(tmp_Y, tmp_X); + + // Compute angle phi (argument of Latitude) + const double phi = nu + this->omega * GNSS_PI; + + const double pkdot = sq1e2 * ekdot / OneMinusecosE; + + // Correct argument of latitude + const double suk = sin(phi); + const double cuk = cos(phi); + + // Correct radius + const double r = a * OneMinusecosE; + const double rkdot = a * this->ecc * sek * ekdot; + + // Correct inclination + double i; + if (this->System == 'E') + { + i = ((56.0 / 180.0) + this->delta_i) * GNSS_PI; + } + else + { + i = (0.3 + this->delta_i) * GNSS_PI; + } + + const double sik = sin(i); + const double cik = cos(i); + + // Compute the angle between the ascending node and the Greenwich meridian + double Omega; + double Omega_dot; + if (this->System == 'B') + { + Omega_dot = this->OMEGAdot * GNSS_PI - BEIDOU_OMEGA_EARTH_DOT; + Omega = this->OMEGA_0 * GNSS_PI + Omega_dot * tk - BEIDOU_OMEGA_EARTH_DOT * static_cast(this->toa); + } + else + { + Omega_dot = this->OMEGAdot * GNSS_PI - GNSS_OMEGA_EARTH_DOT; + Omega = this->OMEGA_0 * GNSS_PI + Omega_dot * tk - GNSS_OMEGA_EARTH_DOT * static_cast(this->toa); + } + + const double sok = sin(Omega); + const double cok = cos(Omega); + + // --- Compute satellite coordinates in Earth-fixed coordinates + const double xprime = r * cuk; + const double yprime = r * suk; + + pos_vel_dtr[0] = xprime * cok - yprime * cik * sok; + pos_vel_dtr[1] = xprime * sok + yprime * cik * cok; + pos_vel_dtr[2] = yprime * sik; + + // Satellite's velocity + const double xpkdot = rkdot * cuk - yprime * pkdot; + const double ypkdot = rkdot * suk + xprime * pkdot; + const double tmp = ypkdot * cik; + + pos_vel_dtr[3] = -Omega_dot * pos_vel_dtr[1] + xpkdot * cok - tmp * sok; + pos_vel_dtr[4] = Omega_dot * pos_vel_dtr[0] + xpkdot * sok + tmp * cok; + pos_vel_dtr[5] = ypkdot * sik; + pos_vel_dtr[6] = 0; +} diff --git a/src/core/system_parameters/gnss_almanac.h b/src/core/system_parameters/gnss_almanac.h index 2206f4acc..a8e28aeeb 100644 --- a/src/core/system_parameters/gnss_almanac.h +++ b/src/core/system_parameters/gnss_almanac.h @@ -18,6 +18,7 @@ #ifndef GNSS_SDR_GNSS_ALMANAC_H #define GNSS_SDR_GNSS_ALMANAC_H +#include #include /** \addtogroup Core @@ -37,6 +38,42 @@ public: */ Gnss_Almanac() = default; + /* \brief Computes prediction of the Doppler shift for a given time and receiver's position and velocity. + * \f[ + * f_{d} = - \mathbf{v} \frac{\mathbf{x}^{T}}{\left| \mathbf{x} \right| } \frac{f_{L}}{c} + * \f] + * where: + * \f[ + * \mathbf{v} = \mathbf{v}_{sat} - \mathbf{v}_{rx} + * \f] + * \f[ + * \mathbf{x} = \mathbf{x}_{sat} - \mathbf{x}_{rx} + * \f] + * \f[ + * \left| \mathbf{x} \right| = \sqrt{\mathbf{x}\mathbf{x}^{T}} + * \f] + * + * @param[in] rx_time_s Time of Week in seconds + * @param[in] lat Receiver's latitude in degrees + * @param[in] lon Receiver's longitude in degrees + * @param[in] h Receiver's height in meters + * @param[in] ve Receiver's velocity in the East direction [m/s] + * @param[in] vn Receiver's velocity in the North direction [m/s] + * @param[in] vu Receiver's velocity in the Up direction [m/s] + * @param[in] band Signal band for which the Doppler will be computed + * (1: L1 C/A, E1B, BI1; 2: L2C, BI2; 3: BI3; 5: L5/E5a; 6: E6B; 7: E5b; 8: E5a+E5b) + */ + double predicted_doppler(double rx_time_s, + double lat, + double lon, + double h, + double ve, + double vn, + double vu, + int band) const; + + void satellitePosVelComputation(double transmitTime, std::array& pos_vel_dtr) const; + uint32_t PRN{}; //!< SV PRN NUMBER double delta_i{}; //!< Inclination Angle at Reference Time (relative to i_0 = 0.30 semi-circles) int32_t toa{}; //!< Almanac data reference time of week [s] @@ -49,6 +86,11 @@ public: double OMEGAdot{}; //!< Rate of Right Ascension [semi-circles/s] double af0{}; //!< Coefficient 0 of code phase offset model [s] double af1{}; //!< Coefficient 1 of code phase offset model [s/s] + +protected: + char System{}; //!< Character ID of the GNSS system. 'G': GPS. 'E': Galileo. 'B': BeiDou +private: + double check_t(double time) const; }; diff --git a/src/core/system_parameters/gnss_ephemeris.cc b/src/core/system_parameters/gnss_ephemeris.cc index 6e33f231b..9505242ca 100644 --- a/src/core/system_parameters/gnss_ephemeris.cc +++ b/src/core/system_parameters/gnss_ephemeris.cc @@ -193,10 +193,7 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< const double n = n0 + this->delta_n; // Mean anomaly - double M = this->M_0 + n * tk; - - // Reduce mean anomaly to between 0 and 2pi - M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); + const double M = this->M_0 + n * tk; // Initial guess of eccentric anomaly double E = M; @@ -228,10 +225,9 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< const double nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) - double phi = nu + this->omega; + const double phi = nu + this->omega; // Reduce phi to between 0 and 2*pi rad - phi = fmod((phi), (2.0 * GNSS_PI)); const double s2pk = sin(2.0 * phi); const double c2pk = cos(2.0 * phi); const double pkdot = sq1e2 * ekdot / OneMinusecosE; @@ -266,8 +262,6 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< Omega = this->OMEGA_0 + Omega_dot * tk - GNSS_OMEGA_EARTH_DOT * static_cast(this->toe); } - // Reduce to between 0 and 2*pi rad - Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); const double sok = sin(Omega); const double cok = cos(Omega); @@ -352,9 +346,6 @@ double Gnss_Ephemeris::sv_clock_relativistic_term(double transmitTime) const // Mean anomaly const double M = this->M_0 + n * tk; - // Reduce mean anomaly to between 0 and 2pi - // M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); - // Initial guess of eccentric anomaly double E = M; double E_old; diff --git a/src/core/system_parameters/gnss_ephemeris.h b/src/core/system_parameters/gnss_ephemeris.h index a5549bff6..c3f9a8e78 100644 --- a/src/core/system_parameters/gnss_ephemeris.h +++ b/src/core/system_parameters/gnss_ephemeris.h @@ -39,6 +39,20 @@ public: /*! * \brief Computes prediction of the Doppler shift for a given time and receiver's position and velocity. + * \f[ + * f_{d} = - \mathbf{v} \frac{\mathbf{x}^{T}}{\left| \mathbf{x} \right| } \frac{f_{L}}{c} + * \f] + * where: + * \f[ + * \mathbf{v} = \mathbf{v}_{sat} - \mathbf{v}_{rx} + * \f] + * \f[ + * \mathbf{x} = \mathbf{x}_{sat} - \mathbf{x}_{rx} + * \f] + * \f[ + * \left| \mathbf{x} \right| = \sqrt{\mathbf{x}\mathbf{x}^{T}} + * \f] + * * @param[in] rx_time_s Time of Week in seconds * @param[in] lat Receiver's latitude in degrees * @param[in] lon Receiver's longitude in degrees diff --git a/src/core/system_parameters/gps_almanac.h b/src/core/system_parameters/gps_almanac.h index c03278841..9c0cce600 100644 --- a/src/core/system_parameters/gps_almanac.h +++ b/src/core/system_parameters/gps_almanac.h @@ -38,7 +38,10 @@ public: /*! * Default constructor */ - Gps_Almanac() = default; + Gps_Almanac() + { + this->System = 'G'; + }; int32_t SV_health{}; //!< SV Health int32_t AS_status{}; //!< Anti-Spoofing Flags and SV Configuration