diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index 8373d4c70..2590af434 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -46,6 +46,11 @@ All notable changes to GNSS-SDR will be documented in this file. and 1.79 for L5/E5a), and its variance by the square of that factor. Previously the unscaled L1 delay was applied to those measurements, undercorrecting the ionosphere by the same factor. +- Improved PVT processing of GPS L2C, GPS L5, and QZSS signals using CNAV + navigation data: satellite positions now include the CNAV semi-major axis and + mean-motion rate terms, and group-delay / inter-signal corrections follow + IS-GPS-200 / IS-GPS-705 in both single-band and L1+L5 dual-band + configurations. Contributed by @vladisslav2011. ### Improvements in Availability: diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f9ca62c6b..fb33810f8 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1653,6 +1653,13 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if (eph_data[i].sat == sat) { // 2. If found, attach the L2/L5 observation to the existing observation in RTKLIB structure + // The existing entry carries the LNAV ephemeris, which has no + // inter-signal corrections: take the ISCs from CNAV so the + // dual-frequency correction in prange() can apply them. + eph_data[i].isc[0] = gps_cnav_ephemeris_iter->second.ISCL1; + eph_data[i].isc[1] = gps_cnav_ephemeris_iter->second.ISCL2; + eph_data[i].isc[2] = gps_cnav_ephemeris_iter->second.ISCL5I; + eph_data[i].isc[3] = gps_cnav_ephemeris_iter->second.ISCL5Q; if (eph_data[i].apply_has_corrections) { const HAS_obs_corrections *applied_has_correction = nullptr; diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 30941edd6..8ba8cc7e7 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -350,6 +350,9 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, double *var) { double tk; + double Ak; + double na; + double delta_na; double M; double E; double Ek; @@ -402,7 +405,10 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, omge = GNSS_OMEGA_EARTH_DOT; break; } - M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk; + Ak = eph->A + eph->Adot * tk; + delta_na = eph->deln + 0.5 * eph->ndot * tk; + na = sqrt(mu / (eph->A * eph->A * eph->A)) + delta_na; + M = eph->M0 + na * tk; for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++) { @@ -420,7 +426,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, trace(4, "kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n", eph->sat, eph->e, n, E - Ek); u = atan2(sqrt(1.0 - eph->e * eph->e) * sinE, cosE - eph->e) + eph->omg; - r = eph->A * (1.0 - eph->e * cosE); + r = Ak * (1.0 - eph->e * cosE); i = eph->i0 + eph->idot * tk; sin2u = sin(2.0 * u); cos2u = cos(2.0 * u); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 072ceb332..3fc238024 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -336,11 +336,11 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P2 += P2_C2; // C2->P2 if (obs->code[j] == CODE_L2S) // L2 single freq. { - PC = P2 + P1_P2 - ISCl2; + PC = P2 - P1_P2 + ISCl2; } else if (obs->code[j] == CODE_L5X) // L5 single freq. { - PC = P2 + P1_P2 - ISCl5i; + PC = P2 - P1_P2 + ISCl5i; } } if (sys == SYS_BDS) diff --git a/src/core/system_parameters/gnss_ephemeris.cc b/src/core/system_parameters/gnss_ephemeris.cc index b1c8ec2fe..249021d88 100644 --- a/src/core/system_parameters/gnss_ephemeris.cc +++ b/src/core/system_parameters/gnss_ephemeris.cc @@ -190,11 +190,14 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< // Time from ephemeris reference epoch double tk = check_t(transmitTime - static_cast(this->toe)); + // Semi-major axis correction (CNAV) + const double Ak = a + this->Adot * tk; + // Corrected mean motion const double n = n0 + this->delta_n; // Mean anomaly - const double M = this->M_0 + n * tk; + const double M = this->M_0 + (n + 0.5 * this->delta_ndot * tk) * tk; // Initial guess of eccentric anomaly double E = M; @@ -218,7 +221,7 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< 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; + const double ekdot = (n + this->delta_ndot * tk) / OneMinusecosE; // Compute the true anomaly const double tmp_Y = sq1e2 * sek; @@ -240,8 +243,8 @@ void Gnss_Ephemeris::satellitePosVelComputation(double transmitTime, std::array< const double ukdot = pkdot * (1.0 + 2.0 * (this->Cus * c2pk - this->Cuc * s2pk)); // Correct radius - const double r = a * OneMinusecosE + this->Crc * c2pk + this->Crs * s2pk; - const double rkdot = a * this->ecc * sek * ekdot + 2.0 * pkdot * (this->Crs * c2pk - this->Crc * s2pk); + const double r = Ak * OneMinusecosE + this->Crc * c2pk + this->Crs * s2pk; + const double rkdot = this->Adot * (1. - this->ecc * cek) + Ak * this->ecc * sek * ekdot + 2.0 * pkdot * (this->Crs * c2pk - this->Crc * s2pk); // Correct inclination const double i = this->i_0 + this->idot * tk + this->Cic * c2pk + this->Cis * s2pk; @@ -345,7 +348,7 @@ double Gnss_Ephemeris::sv_clock_relativistic_term(double transmitTime) const const double n = n0 + this->delta_n; // Mean anomaly - const double M = this->M_0 + n * tk; + const double M = this->M_0 + (n + 0.5 * this->delta_ndot * tk) * tk; // Initial guess of eccentric anomaly double E = M; diff --git a/src/core/system_parameters/gnss_ephemeris.h b/src/core/system_parameters/gnss_ephemeris.h index 8616ea4c8..795d050e4 100644 --- a/src/core/system_parameters/gnss_ephemeris.h +++ b/src/core/system_parameters/gnss_ephemeris.h @@ -67,23 +67,25 @@ public: void satellitePosition(double transmitTime); //!< Computes the ECEF SV coordinates and ECEF velocity - uint32_t PRN{}; //!< SV ID - double M_0{}; //!< Mean anomaly at reference time [rad] - double delta_n{}; //!< Mean motion difference from computed value [rad/sec] - double ecc{}; //!< Eccentricity - double sqrtA{}; //!< Square root of the semi-major axis [meters^1/2] - double OMEGA_0{}; //!< Longitude of ascending node of orbital plane at weekly epoch [rad] - double i_0{}; //!< Inclination angle at reference time [rad] - double omega{}; //!< Argument of perigee [rad] - double OMEGAdot{}; //!< Rate of right ascension [rad/sec] - double idot{}; //!< Rate of inclination angle [rad/sec] - double Cuc{}; //!< Amplitude of the cosine harmonic correction term to the argument of latitude [rad] - double Cus{}; //!< Amplitude of the sine harmonic correction term to the argument of latitude [rad] - double Crc{}; //!< Amplitude of the cosine harmonic correction term to the orbit radius [meters] - double Crs{}; //!< Amplitude of the sine harmonic correction term to the orbit radius [meters] - double Cic{}; //!< Amplitude of the cosine harmonic correction term to the angle of inclination [rad] - double Cis{}; //!< Amplitude of the sine harmonic correction term to the angle of inclination [rad] - int32_t toe{}; //!< Ephemeris reference time [s] + uint32_t PRN{}; //!< SV ID + double M_0{}; //!< Mean anomaly at reference time [rad] + double Adot{}; //!< Change rate in semi-major axis (CNAV) + double delta_ndot{}; //!< Rate of mean motion difference from computed value (CNAV) + double delta_n{}; //!< Mean motion difference from computed value [rad/sec] + double ecc{}; //!< Eccentricity + double sqrtA{}; //!< Square root of the semi-major axis [meters^1/2] + double OMEGA_0{}; //!< Longitude of ascending node of orbital plane at weekly epoch [rad] + double i_0{}; //!< Inclination angle at reference time [rad] + double omega{}; //!< Argument of perigee [rad] + double OMEGAdot{}; //!< Rate of right ascension [rad/sec] + double idot{}; //!< Rate of inclination angle [rad/sec] + double Cuc{}; //!< Amplitude of the cosine harmonic correction term to the argument of latitude [rad] + double Cus{}; //!< Amplitude of the sine harmonic correction term to the argument of latitude [rad] + double Crc{}; //!< Amplitude of the cosine harmonic correction term to the orbit radius [meters] + double Crs{}; //!< Amplitude of the sine harmonic correction term to the orbit radius [meters] + double Cic{}; //!< Amplitude of the cosine harmonic correction term to the angle of inclination [rad] + double Cis{}; //!< Amplitude of the sine harmonic correction term to the angle of inclination [rad] + int32_t toe{}; //!< Ephemeris reference time [s] // Clock correction parameters int32_t toc{}; //!< Clock correction data reference Time of Week [sec] diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index 3bf156cca..8da03cd28 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -47,8 +47,6 @@ public: } double delta_A{}; //!< Semi-major axis difference at reference time - double Adot{}; //!< Change rate in semi-major axis - double delta_ndot{}; //!< Rate of mean motion difference from computed value double delta_OMEGAdot{}; //!< Rate of Right Ascension difference [semi-circles/s] int32_t toe1{}; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200M) [s] int32_t toe2{}; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200M) [s] @@ -140,6 +138,10 @@ public: archive& BOOST_SERIALIZATION_NVP(URA1); archive& BOOST_SERIALIZATION_NVP(URA2); } + if (version > 1) + { + archive& BOOST_SERIALIZATION_NVP(delta_ndot); + } archive& BOOST_SERIALIZATION_NVP(TGD); archive& BOOST_SERIALIZATION_NVP(ISCL1); archive& BOOST_SERIALIZATION_NVP(ISCL2); @@ -155,7 +157,7 @@ public: } }; -BOOST_CLASS_VERSION(Gps_CNAV_Ephemeris, 1) +BOOST_CLASS_VERSION(Gps_CNAV_Ephemeris, 2) /** \} */ diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 6d95acc53..164546c72 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -303,6 +303,7 @@ bool Gps_CNAV_Navigation_Message::have_new_ephemeris() // Check if we have a ne d_ephemeris_top == d_clock_top) { // MT10, MT11, and a clock page belong to the same CNAV CEI set. + ephemeris_record.toe = ephemeris_record.toe1; b_flag_ephemeris_1 = false; // clear the flags b_flag_ephemeris_2 = false; b_flag_clock_valid = false; diff --git a/tests/test_main.cc b/tests/test_main.cc index f1a674668..492fb6c59 100644 --- a/tests/test_main.cc +++ b/tests/test_main.cc @@ -188,6 +188,7 @@ private: #include "unit-tests/system-parameters/galileo_ism_test.cc" #include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" #include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc" +#include "unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc" #include "unit-tests/system-parameters/gps_cnav_navigation_message_test.cc" #include "unit-tests/system-parameters/has_decoding_test.cc" #include "unit-tests/system-parameters/qzss_code_generation_test.cc" diff --git a/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc new file mode 100644 index 000000000..311ada172 --- /dev/null +++ b/tests/unit-tests/system-parameters/gnss_ephemeris_posvel_test.cc @@ -0,0 +1,317 @@ +/*! + * \file gnss_ephemeris_posvel_test.cc.cc + * \author Vladislav P, 2026. vladisslav2011(at)gmail.com + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "beidou_dnav_ephemeris.h" +#include "galileo_ephemeris.h" +#include "gnss_ephemeris.h" +#include "gps_cnav_ephemeris.h" +#include "gps_ephemeris.h" + + +TEST(GnssEphemerisPosVelTest, NAV) +{ + Gps_Ephemeris e{}; + e.PRN = 15; + e.M_0 = 1.35464234665685956e+00; + e.delta_n = 5.10199823299105835e-09; + e.ecc = 1.70680778101086582e-02; + e.sqrtA = 5.15376512145996094e+03; + e.OMEGA_0 = 2.35100387398234556e+00; + e.i_0 = 9.44768674001770559e-01; + e.omega = 1.54160131395114042e+00; + e.OMEGAdot = -8.57285709390860145e-09; + e.idot = -2.30366738556474118e-10; + e.Cuc = -6.73159956932067871e-06; + e.Cus = 3.66382300853729248e-06; + e.Crc = 3.05843750000000000e+02; + e.Crs = -1.23937500000000000e+02; + e.Cic = -1.17346644401550293e-07; + e.Cis = -3.91155481338500977e-07; + e.toe = 331200; + e.toc = 331200; + e.af0 = 4.32340428233146667e-04; + e.af1 = 3.06954461848363200e-12; + e.af2 = 0.00000000000000000e+00; + e.WN = 381; + e.tow = 326166; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.IODE_SF2 = 30; + e.IODE_SF3 = 30; + e.code_on_L2 = 1; + e.L2_P_data_flag = 0; + e.SV_accuracy = 0; + e.SV_health = 0; + e.TGD = -1.07102096080780029e-08; + e.IODC = 30; + e.AODO = 16200; + e.fit_interval_flag = 0; + e.spare1 = 0.00000000000000000e+00; + e.spare2 = 0.00000000000000000e+00; + e.integrity_status_flag = 0; + e.alert_flag = 0; + e.antispoofing_flag = 1; + auto dopplerL1 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 1); + e.satellitePosition(590328.); + EXPECT_NEAR(98.676019, dopplerL1, 1e-4); + EXPECT_NEAR(26325247.164428, e.satpos_X, 1e-4); + EXPECT_NEAR(1943900.228289, e.satpos_Y, 1e-4); + EXPECT_NEAR(2494201.363945, e.satpos_Z, 1e-4); + EXPECT_NEAR(333.268032, e.satvel_X, 1e-4); + EXPECT_NEAR(384.414587, e.satvel_Y, 1e-4); + EXPECT_NEAR(-3116.921783, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(590327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(590328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} + + +TEST(GnssEphemerisPosVelTest, CNAV) +{ + Gps_CNAV_Ephemeris e{}; + e.PRN = 1; + e.M_0 = 8.45797918146817151e-01; + e.delta_n = 4.55893989814592299e-09; + e.ecc = 1.83155917329713681e-03; + e.sqrtA = 5.15360200490880015e+03; + e.OMEGA_0 = 6.26039239835774475e-01; + e.i_0 = 9.57107201573828559e-01; + e.omega = 1.91825002944197659e-01; + e.OMEGAdot = -8.21850013985512873e-09; + e.idot = 1.84293390845179697e-10; + e.Cuc = 3.64799052476882935e-06; + e.Cus = 3.35834920406341553e-06; + e.Crc = 3.11757812500000000e+02; + e.Crs = 6.96562500000000000e+01; + e.Cic = 4.00468707084655762e-08; + e.Cis = 4.00468707084655762e-08; + e.toe = 588600; + e.toc = 588600; + e.af0 = 2.13293504202738377e-04; + e.af1 = -9.59232693276135251e-12; + e.af2 = 0.00000000000000000e+00; + e.WN = 2428; + e.tow = 590328; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.toe1 = 588600; + e.toe2 = 588600; + e.WNop = 124; + e.top = 519300; + e.URAED = -1; + e.URANED0 = -2; + e.URANED1 = 3; + e.URANED2 = 7; + e.URA = -1; + e.URA0 = -2.00000000000000000e+00; + e.URA1 = 3.00000000000000000e+00; + e.URA2 = 7.00000000000000000e+00; + e.delta_ndot = -8.06351598383372435e-14; + e.TGD = -8.84756445884704424e-09; + e.ISCL1 = -2.91038304567336984e-10; + e.ISCL2 = 5.64614310860633767e-09; + e.ISCL5I = -5.52972778677940265e-10; + e.ISCL5Q = -6.69388100504875080e-10; + e.delta_A = -9.63750000000000000e+01; + e.Adot = 7.12776184082031250e-03; + e.delta_OMEGAdot = -5.03592405216479423e-11; + e.integrity_status_flag = 0; + e.l2c_phasing_flag = 0; + e.alert_flag = 0; + e.antispoofing_flag = 0; + auto dopplerL2 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 2); + auto dopplerL5 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 5); + e.satellitePosition(590328.); + EXPECT_NEAR(-117.548462, dopplerL2, 1e-4); + EXPECT_NEAR(-112.650609, dopplerL5, 1e-4); + EXPECT_NEAR(-14623772.544799, e.satpos_X, 1e-4); + EXPECT_NEAR(7423769.471881, e.satpos_Y, 1e-4); + EXPECT_NEAR(20862927.586354, e.satpos_Z, 1e-4); + EXPECT_NEAR(-111.278839, e.satvel_X, 1e-4); + EXPECT_NEAR(-2654.059316, e.satvel_Y, 1e-4); + EXPECT_NEAR(874.382189, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(590327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(590328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} + + +TEST(GnssEphemerisPosVelTest, Galileo) +{ + Galileo_Ephemeris e{}; + e.PRN = 13; + e.M_0 = 2.41163546760016079e+00; + e.delta_n = 2.66832543217894277e-09; + e.ecc = 4.77104913443326882e-05; + e.sqrtA = 5.44060108375549316e+03; + e.OMEGA_0 = 6.47008588356165260e-01; + e.i_0 = 9.97650131401018214e-01; + e.omega = -7.14945752017439456e-02; + e.OMEGAdot = -5.50165773755647669e-09; + e.idot = 3.22870591713259829e-10; + e.Cuc = 6.63474202156066895e-06; + e.Cus = 2.07684934139251709e-06; + e.Crc = 3.11656250000000000e+02; + e.Crs = 1.40062500000000000e+02; + e.Cic = -2.79396772384643555e-08; + e.Cis = 4.47034835815429688e-08; + e.toe = 57000; + e.toc = 57000; + e.af0 = -6.03639055043458871e-05; + e.af1 = -9.37916411203332043e-13; + e.af2 = 0.00000000000000000e+00; + e.WN = 2429; + e.tow = 59670; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.IOD_ephemeris = 95; + e.IOD_nav = 0; + e.SISA = 107; + e.E5a_HS = 0; + e.E5b_HS = 0; + e.E1B_HS = 0; + e.E5a_DVS = 0; + e.E5b_DVS = 0; + e.E1B_DVS = 0; + e.BGD_E1E5a = 4.42378222942352212e-09; + e.BGD_E1E5b = 0.00000000000000000e+00; + e.flag_all_ephemeris = 1; + e.nav_message_type = static_cast(2); + auto dopplerL1 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 1); + auto dopplerL5 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 5); + auto dopplerL6 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 6); + auto dopplerL7 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 7); + auto dopplerL8 = e.predicted_doppler(59328., 0., 0., 0., 0., 0., 0., 8); + e.satellitePosition(59328.); + EXPECT_NEAR(1250.566176, dopplerL1, 1e-4); + EXPECT_NEAR(933.864352, dopplerL5, 1e-4); + EXPECT_NEAR(1015.069948, dopplerL6, 1e-4); + EXPECT_NEAR(958.226030, dopplerL7, 1e-4); + EXPECT_NEAR(946.045191, dopplerL8, 1e-4); + EXPECT_NEAR(18121174.847871, e.satpos_X, 1e-4); + EXPECT_NEAR(-19974436.330324, e.satpos_Y, 1e-4); + EXPECT_NEAR(12202101.756204, e.satpos_Z, 1e-4); + EXPECT_NEAR(977.807753, e.satvel_X, 1e-4); + EXPECT_NEAR(-754.332969, e.satvel_Y, 1e-4); + EXPECT_NEAR(-2686.561660, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(59327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(59328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} + + +TEST(GnssEphemerisPosVelTest, DNAV) +{ + Beidou_Dnav_Ephemeris e{}; + e.PRN = 41; + e.M_0 = -2.86495395534618202e+00; + e.delta_n = 3.27227916070452066e-09; + e.ecc = 1.44056859426200368e-03; + e.sqrtA = 5.28263833618164062e+03; + e.OMEGA_0 = 2.17724435277226158e+00; + e.i_0 = 9.87684843163906256e-01; + e.omega = -1.04237488131494449e+00; + e.OMEGAdot = -6.49884213143194274e-09; + e.idot = -1.00004165574903490e-11; + e.Cuc = -7.40075483918190002e-06; + e.Cus = 5.91995194554328918e-06; + e.Crc = 2.55390625000000000e+02; + e.Crs = -1.52921875000000000e+02; + e.Cic = -1.07102096080780029e-08; + e.Cis = 3.44589352607727051e-08; + e.toe = 586800; + e.toc = 586800; + e.af0 = -9.49730747379362475e-04; + e.af1 = -2.21955787083061296e-12; + e.af2 = 0.00000000000000000e+00; + e.WN = 1072; + e.tow = 590232; + e.satClkDrift = 0.00000000000000000e+00; + e.dtr = 0.00000000000000000e+00; + e.AODE = 1.00000000000000000e+00; + e.SV_accuracy = 0; + e.SV_health = 0; + e.AODC = 1.00000000000000000e+00; + e.TGD1 = -2.39999999999999998e-08; + e.TGD2 = -2.39999999999999998e-08; + e.sig_type = 5; + e.nav_type = 1; + e.AODO = 0; + e.fit_interval_flag = 0; + e.spare1 = 0.00000000000000000e+00; + e.spare2 = 0.00000000000000000e+00; + e.integrity_status_flag = 0; + e.alert_flag = 0; + e.antispoofing_flag = 0; + auto dopplerL1 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 1); + auto dopplerL3 = e.predicted_doppler(590328., 0., 0., 0., 0., 0., 0., 3); + e.satellitePosition(590328.); + EXPECT_NEAR(1168.881533, dopplerL1, 1e-4); + EXPECT_NEAR(949.811992, dopplerL3, 1e-4); + EXPECT_NEAR(26628945.935993, e.satpos_X, 1e-4); + EXPECT_NEAR(-5184093.916996, e.satpos_Y, 1e-4); + EXPECT_NEAR(6666187.976574, e.satpos_Z, 1e-4); + EXPECT_NEAR(755.905578, e.satvel_X, 1e-4); + EXPECT_NEAR(18.179881, e.satvel_Y, 1e-4); + EXPECT_NEAR(-3021.004498, e.satvel_Z, 1e-4); + double satvel_X = e.satvel_X; + double satvel_Y = e.satvel_Y; + double satvel_Z = e.satvel_Z; + e.satellitePosition(590327.995); + double satpos_X = e.satpos_X; + double satpos_Y = e.satpos_Y; + double satpos_Z = e.satpos_Z; + e.satellitePosition(590328.005); + double d_X = (e.satpos_X - satpos_X) * 100.; + double d_Y = (e.satpos_Y - satpos_Y) * 100.; + double d_Z = (e.satpos_Z - satpos_Z) * 100.; + EXPECT_NEAR(d_X, satvel_X, 1e-4); + EXPECT_NEAR(d_Y, satvel_Y, 1e-4); + EXPECT_NEAR(d_Z, satvel_Z, 1e-4); +} diff --git a/tests/unit-tests/system-parameters/gps_cnav_navigation_message_test.cc b/tests/unit-tests/system-parameters/gps_cnav_navigation_message_test.cc index b1495db77..3ec478f36 100644 --- a/tests/unit-tests/system-parameters/gps_cnav_navigation_message_test.cc +++ b/tests/unit-tests/system-parameters/gps_cnav_navigation_message_test.cc @@ -81,6 +81,7 @@ TEST(GpsCnavNavigationMessageTest, EphemerisRequiresMatchingClockPage) set_gps_cnav_unsigned_field(mt30_matching, CNAV_TOC, 200); nav_message.decode_page(mt30_matching); EXPECT_TRUE(nav_message.have_new_ephemeris()); + EXPECT_EQ(200 * CNAV_TOE1_LSB, nav_message.get_ephemeris().toe); EXPECT_FALSE(nav_message.have_new_ephemeris()); }