From 344cd79af17366ebf9028cd651c99b04781c2013 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 Jun 2026 13:10:39 +0200 Subject: [PATCH] Implement QZSS LNAV almanac/auxiliary pages decoding, add unit tests --- .../libs/rtklib/rtklib_conversions.cc | 6 +- .../gps_l1_ca_telemetry_decoder_gs.cc | 10 + src/core/system_parameters/gnss_almanac.cc | 4 +- src/core/system_parameters/gnss_almanac.h | 2 +- src/core/system_parameters/gps_almanac.h | 10 + .../gps_navigation_message.cc | 386 ++++++++++++------ .../gps_navigation_message.h | 11 + src/core/system_parameters/qzss.h | 8 + tests/test_main.cc | 1 + .../qzss_lnav_navigation_message_test.cc | 237 +++++++++++ 10 files changed, 538 insertions(+), 137 deletions(-) create mode 100644 tests/unit-tests/system-parameters/qzss_lnav_navigation_message_test.cc diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 01ec235e5..ec2cbad51 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -781,7 +781,9 @@ alm_t alm_to_rtklib(const Gps_Almanac& gps_alm) rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - rtklib_alm.sat = gps_alm.PRN; + const int gps_sys = (MINPRNQZS <= gps_alm.PRN && gps_alm.PRN <= MAXPRNQZS) ? SYS_QZS : SYS_GPS; + + rtklib_alm.sat = satno(gps_sys, gps_alm.PRN); rtklib_alm.svh = gps_alm.SV_health; rtklib_alm.svconf = gps_alm.AS_status; rtklib_alm.week = gps_alm.WNa; @@ -791,7 +793,7 @@ alm_t alm_to_rtklib(const Gps_Almanac& gps_alm) rtklib_alm.toa = toa; rtklib_alm.A = gps_alm.sqrtA * gps_alm.sqrtA; rtklib_alm.e = gps_alm.ecc; - rtklib_alm.i0 = (gps_alm.delta_i + 0.3) * GNSS_PI; + rtklib_alm.i0 = ((gps_alm.get_system() == 'J') ? gps_alm.delta_i : (gps_alm.delta_i + 0.3)) * GNSS_PI; rtklib_alm.OMG0 = gps_alm.OMEGA_0 * GNSS_PI; rtklib_alm.OMGd = gps_alm.OMEGAdot * GNSS_PI; rtklib_alm.omg = gps_alm.omega * GNSS_PI; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 64221b0ae..a4eb4066c 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -387,6 +387,16 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe(double cn0, bool flag_inver } break; case 5: + if (d_nav->get_flag_iono_valid() == true) + { + const std::shared_ptr tmp_obj = std::make_shared(d_nav->get_iono()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + if (d_nav->get_flag_utc_model_valid() == true) + { + const std::shared_ptr tmp_obj = std::make_shared(d_nav->get_utc_model()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } if (d_nav->almanac_validation() == true) { const std::shared_ptr tmp_obj = std::make_shared(d_nav->get_almanac()); diff --git a/src/core/system_parameters/gnss_almanac.cc b/src/core/system_parameters/gnss_almanac.cc index 0450e03af..29d05081b 100644 --- a/src/core/system_parameters/gnss_almanac.cc +++ b/src/core/system_parameters/gnss_almanac.cc @@ -111,7 +111,7 @@ double Gnss_Almanac::predicted_doppler(double rx_time_s, predicted_doppler = 0.0; } } - else if (this->System == 'G') // GPS + else if (this->System == 'G' || this->System == 'J') // GPS/QZSS { if (band == 1) { @@ -233,7 +233,7 @@ void Gnss_Almanac::satellitePosVelComputation(double transmitTime, std::arraydelta_i) * GNSS_PI; + i = ((this->System == 'J') ? this->delta_i : (0.3 + this->delta_i)) * GNSS_PI; } const double sik = sin(i); diff --git a/src/core/system_parameters/gnss_almanac.h b/src/core/system_parameters/gnss_almanac.h index d8da862d6..f550eabfe 100644 --- a/src/core/system_parameters/gnss_almanac.h +++ b/src/core/system_parameters/gnss_almanac.h @@ -92,7 +92,7 @@ public: double af1{}; //!< Coefficient 1 of code phase offset model [s/s] protected: - char System{}; //!< Character ID of the GNSS system. 'G': GPS. 'E': Galileo. 'C': BeiDou + char System{}; //!< Character ID of the GNSS system. 'G': GPS. 'E': Galileo. 'C': BeiDou. 'J': QZSS private: double check_t(double time) const; }; diff --git a/src/core/system_parameters/gps_almanac.h b/src/core/system_parameters/gps_almanac.h index 65e0bd97c..f8db79263 100644 --- a/src/core/system_parameters/gps_almanac.h +++ b/src/core/system_parameters/gps_almanac.h @@ -43,6 +43,16 @@ public: this->System = 'G'; }; + void set_system(char system) + { + this->System = system; + } + + char get_system() const + { + return this->System; + } + int32_t SV_health{}; //!< SV Health int32_t AS_status{}; //!< Anti-Spoofing Flags and SV Configuration diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index 854957aaf..1d0331236 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -19,11 +19,14 @@ #include "gps_navigation_message.h" #include "gnss_satellite.h" +#include #include // for fmod, abs, floor #include // for memcpy #include // for operator<<, cout #include // for std::numeric_limits +using LnavParameter = std::vector>; + Gps_Navigation_Message::Gps_Navigation_Message(LnavSystem system) : d_system(system) @@ -49,6 +52,173 @@ void Gps_Navigation_Message::print_gps_word_bytes(uint32_t GPS_word) const } +static uint32_t qzss_prn_from_lnav_sv_id(int32_t sv_id) +{ + if (sv_id < 1 || sv_id > 10) + { + return 0U; + } + + return static_cast(sv_id) + QZSS_PRN_OFFSET; +} + + +static bool qzss_is_qzo(uint32_t prn) +{ + return prn >= 194U && prn <= 197U; +} + + +static double qzss_almanac_eccentricity_ref(uint32_t prn) +{ + return qzss_is_qzo(prn) ? QZSS_QZO_ECCENTRICITY_REF : 0.0; +} + + +static double qzss_almanac_inclination_ref(uint32_t prn) +{ + return qzss_is_qzo(prn) ? QZSS_QZO_INCLINATION_REF : 0.0; +} + + +static const std::array& gps_sf5_health_fields() +{ + static const std::array fields = { + &HEALTH_SV1, + &HEALTH_SV2, + &HEALTH_SV3, + &HEALTH_SV4, + &HEALTH_SV5, + &HEALTH_SV6, + &HEALTH_SV7, + &HEALTH_SV8, + &HEALTH_SV9, + &HEALTH_SV10, + &HEALTH_SV11, + &HEALTH_SV12, + &HEALTH_SV13, + &HEALTH_SV14, + &HEALTH_SV15, + &HEALTH_SV16, + &HEALTH_SV17, + &HEALTH_SV18, + &HEALTH_SV19, + &HEALTH_SV20, + &HEALTH_SV21, + &HEALTH_SV22, + &HEALTH_SV23, + &HEALTH_SV24}; + + return fields; +} + + +void Gps_Navigation_Message::decode_lnav_almanac(const std::bitset& subframe_bits, uint32_t prn, double eccentricity_ref, double inclination_ref) +{ + a_M_0 = static_cast(read_navigation_signed(subframe_bits, ALM_MZERO)); + a_M_0 = a_M_0 * ALM_MZERO_LSB; + a_ecc = static_cast(read_navigation_unsigned(subframe_bits, ALM_ECC)); + a_ecc = a_ecc * ALM_ECC_LSB + eccentricity_ref; + a_sqrtA = static_cast(read_navigation_unsigned(subframe_bits, ALM_SQUAREA)); + a_sqrtA = a_sqrtA * ALM_SQUAREA_LSB; + a_OMEGA_0 = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGAZERO)); + a_OMEGA_0 = a_OMEGA_0 * ALM_OMEGAZERO_LSB; + a_omega = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGA)); + a_omega = a_omega * ALM_OMEGA_LSB; + a_OMEGAdot = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGADOT)); + a_OMEGAdot = a_OMEGAdot * ALM_OMEGADOT_LSB; + a_delta_i = static_cast(read_navigation_signed(subframe_bits, ALM_DELTAI)); + a_delta_i = a_delta_i * ALM_DELTAI_LSB + inclination_ref; + a_af0 = static_cast(read_navigation_signed(subframe_bits, ALM_AF0)); + a_af0 = a_af0 * ALM_AF0_LSB; + a_af1 = static_cast(read_navigation_signed(subframe_bits, ALM_AF1)); + a_af1 = a_af1 * ALM_AF1_LSB; + a_PRN = prn; + i_Toa = static_cast(read_navigation_unsigned(subframe_bits, ALM_TOA)); + i_Toa = i_Toa * ALM_TOA_LSB; + SV_Health = static_cast(read_navigation_unsigned(subframe_bits, ALM_SVHEALTH)); + + flag_almanac_valid = true; +} + + +void Gps_Navigation_Message::decode_lnav_iono_utc(const std::bitset& subframe_bits) +{ + d_alpha0 = static_cast(read_navigation_signed(subframe_bits, ALPHA_0)); + d_alpha0 = d_alpha0 * ALPHA_0_LSB; + d_alpha1 = static_cast(read_navigation_signed(subframe_bits, ALPHA_1)); + d_alpha1 = d_alpha1 * ALPHA_1_LSB; + d_alpha2 = static_cast(read_navigation_signed(subframe_bits, ALPHA_2)); + d_alpha2 = d_alpha2 * ALPHA_2_LSB; + d_alpha3 = static_cast(read_navigation_signed(subframe_bits, ALPHA_3)); + d_alpha3 = d_alpha3 * ALPHA_3_LSB; + d_beta0 = static_cast(read_navigation_signed(subframe_bits, BETA_0)); + d_beta0 = d_beta0 * BETA_0_LSB; + d_beta1 = static_cast(read_navigation_signed(subframe_bits, BETA_1)); + d_beta1 = d_beta1 * BETA_1_LSB; + d_beta2 = static_cast(read_navigation_signed(subframe_bits, BETA_2)); + d_beta2 = d_beta2 * BETA_2_LSB; + d_beta3 = static_cast(read_navigation_signed(subframe_bits, BETA_3)); + d_beta3 = d_beta3 * BETA_3_LSB; + d_A1 = static_cast(read_navigation_signed(subframe_bits, A_1)); + d_A1 = d_A1 * A_1_LSB; + d_A0 = static_cast(read_navigation_signed(subframe_bits, A_0)); + d_A0 = d_A0 * A_0_LSB; + d_t_OT = static_cast(read_navigation_unsigned(subframe_bits, T_OT)); + d_t_OT = d_t_OT * T_OT_LSB; + i_WN_T = static_cast(read_navigation_unsigned(subframe_bits, WN_T)); + d_DeltaT_LS = static_cast(read_navigation_signed(subframe_bits, DELTAT_LS)); + i_WN_LSF = static_cast(read_navigation_unsigned(subframe_bits, WN_LSF)); + i_DN = static_cast(read_navigation_unsigned(subframe_bits, DN)); + d_DeltaT_LSF = static_cast(read_navigation_signed(subframe_bits, DELTAT_LSF)); + flag_iono_valid = true; + flag_utc_model_valid = true; +} + + +void Gps_Navigation_Message::decode_gps_almanac_health_sf4(const std::bitset& subframe_bits) +{ + almanacHealth[25] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV25)); + almanacHealth[26] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV26)); + almanacHealth[27] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV27)); + almanacHealth[28] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV28)); + almanacHealth[29] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV29)); + almanacHealth[30] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV30)); + almanacHealth[31] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV31)); + almanacHealth[32] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV32)); +} + + +void Gps_Navigation_Message::decode_gps_almanac_health_sf5(const std::bitset& subframe_bits) +{ + i_Toa = static_cast(read_navigation_unsigned(subframe_bits, T_OA)); + i_Toa = i_Toa * T_OA_LSB; + i_WN_A = static_cast(read_navigation_unsigned(subframe_bits, WN_A)); + flag_almanac_week_valid = true; + + const auto& health_fields = gps_sf5_health_fields(); + for (uint32_t prn = 1; prn <= health_fields.size(); ++prn) + { + almanacHealth[prn] = static_cast(read_navigation_unsigned(subframe_bits, *health_fields[prn - 1])); + } +} + + +void Gps_Navigation_Message::decode_qzss_almanac_epoch_health(const std::bitset& subframe_bits) +{ + i_Toa = static_cast(read_navigation_unsigned(subframe_bits, T_OA)); + i_Toa = i_Toa * T_OA_LSB; + i_WN_A = static_cast(read_navigation_unsigned(subframe_bits, WN_A)); + flag_almanac_week_valid = true; + + const auto& health_fields = gps_sf5_health_fields(); + for (int32_t sv_id = 1; sv_id <= 10; ++sv_id) + { + almanacHealth[qzss_prn_from_lnav_sv_id(sv_id)] = static_cast(read_navigation_unsigned(subframe_bits, *health_fields[sv_id - 1])); + } +} + + bool Gps_Navigation_Message::read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const { bool value = bits[GPS_SUBFRAME_BITS - parameter[0].first]; @@ -212,91 +382,55 @@ int32_t Gps_Navigation_Message::subframe_decoder(const char* subframe) b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); SV_data_ID = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); SV_page = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); - if (SV_page > 24 && SV_page < 33) // Page 4 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) + if (d_system == LnavSystem::QZSS) { - if (SV_data_ID != 0) + if (SV_data_ID == QZSS_LNAV_DATA_ID) { - a_M_0 = static_cast(read_navigation_signed(subframe_bits, ALM_MZERO)); - a_M_0 = a_M_0 * ALM_MZERO_LSB; - a_ecc = static_cast(read_navigation_unsigned(subframe_bits, ALM_ECC)); - a_ecc = a_ecc * ALM_ECC_LSB; - a_sqrtA = static_cast(read_navigation_unsigned(subframe_bits, ALM_SQUAREA)); - a_sqrtA = a_sqrtA * ALM_SQUAREA_LSB; - a_OMEGA_0 = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGAZERO)); - a_OMEGA_0 = a_OMEGA_0 * ALM_OMEGAZERO_LSB; - a_omega = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGA)); - a_omega = a_omega * ALM_OMEGA_LSB; - a_OMEGAdot = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGADOT)); - a_OMEGAdot = a_OMEGAdot * ALM_OMEGADOT_LSB; - a_delta_i = static_cast(read_navigation_signed(subframe_bits, ALM_DELTAI)); - a_delta_i = a_delta_i * ALM_DELTAI_LSB; - a_af0 = static_cast(read_navigation_signed(subframe_bits, ALM_AF0)); - a_af0 = a_af0 * ALM_AF0_LSB; - a_af1 = static_cast(read_navigation_signed(subframe_bits, ALM_AF1)); - a_af1 = a_af1 * ALM_AF1_LSB; - a_PRN = SV_page; - i_Toa = static_cast(read_navigation_unsigned(subframe_bits, ALM_TOA)); - i_Toa = i_Toa * ALM_TOA_LSB; - - flag_almanac_valid = true; + const uint32_t qzss_prn = qzss_prn_from_lnav_sv_id(SV_page); + if (qzss_prn != 0U) + { + decode_lnav_almanac(subframe_bits, qzss_prn, qzss_almanac_eccentricity_ref(qzss_prn), qzss_almanac_inclination_ref(qzss_prn)); + } + else if (SV_page == QZSS_ALMANAC_EPOCH_HEALTH_SV_ID) + { + decode_qzss_almanac_epoch_health(subframe_bits); + } + else if (SV_page == QZSS_IONO_UTC_WIDE_AREA_SV_ID || SV_page == QZSS_IONO_UTC_JAPAN_AREA_SV_ID) + { + decode_lnav_iono_utc(subframe_bits); + } } } + else + { + if (SV_page > 24 && SV_page < 33) // Page 4 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) + { + if (SV_data_ID != 0) + { + decode_lnav_almanac(subframe_bits, static_cast(SV_page), 0.0, 0.0); + } + } - if (SV_page == 52) // Page 13 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) - { - //! \TODO read Estimated Range Deviation (ERD) values - } + if (SV_page == 52) // Page 13 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) + { + //! \TODO read Estimated Range Deviation (ERD) values + } - if (SV_page == 56) // Page 18 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) - { - // Page 18 - Ionospheric and UTC data - d_alpha0 = static_cast(read_navigation_signed(subframe_bits, ALPHA_0)); - d_alpha0 = d_alpha0 * ALPHA_0_LSB; - d_alpha1 = static_cast(read_navigation_signed(subframe_bits, ALPHA_1)); - d_alpha1 = d_alpha1 * ALPHA_1_LSB; - d_alpha2 = static_cast(read_navigation_signed(subframe_bits, ALPHA_2)); - d_alpha2 = d_alpha2 * ALPHA_2_LSB; - d_alpha3 = static_cast(read_navigation_signed(subframe_bits, ALPHA_3)); - d_alpha3 = d_alpha3 * ALPHA_3_LSB; - d_beta0 = static_cast(read_navigation_signed(subframe_bits, BETA_0)); - d_beta0 = d_beta0 * BETA_0_LSB; - d_beta1 = static_cast(read_navigation_signed(subframe_bits, BETA_1)); - d_beta1 = d_beta1 * BETA_1_LSB; - d_beta2 = static_cast(read_navigation_signed(subframe_bits, BETA_2)); - d_beta2 = d_beta2 * BETA_2_LSB; - d_beta3 = static_cast(read_navigation_signed(subframe_bits, BETA_3)); - d_beta3 = d_beta3 * BETA_3_LSB; - d_A1 = static_cast(read_navigation_signed(subframe_bits, A_1)); - d_A1 = d_A1 * A_1_LSB; - d_A0 = static_cast(read_navigation_signed(subframe_bits, A_0)); - d_A0 = d_A0 * A_0_LSB; - d_t_OT = static_cast(read_navigation_unsigned(subframe_bits, T_OT)); - d_t_OT = d_t_OT * T_OT_LSB; - i_WN_T = static_cast(read_navigation_unsigned(subframe_bits, WN_T)); - d_DeltaT_LS = static_cast(read_navigation_signed(subframe_bits, DELTAT_LS)); - i_WN_LSF = static_cast(read_navigation_unsigned(subframe_bits, WN_LSF)); - i_DN = static_cast(read_navigation_unsigned(subframe_bits, DN)); // Right-justified ? - d_DeltaT_LSF = static_cast(read_navigation_signed(subframe_bits, DELTAT_LSF)); - flag_iono_valid = true; - flag_utc_model_valid = true; - } - if (SV_page == 57) - { - // Reserved - } + if (SV_page == 56) // Page 18 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) + { + decode_lnav_iono_utc(subframe_bits); + } + if (SV_page == 57) + { + // Reserved + } - if (SV_page == 63) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) - { - // Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32) - //! \TODO Read Anti-Spoofing, SV config - almanacHealth[25] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV25)); - almanacHealth[26] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV26)); - almanacHealth[27] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV27)); - almanacHealth[28] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV28)); - almanacHealth[29] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV29)); - almanacHealth[30] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV30)); - almanacHealth[31] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV31)); - almanacHealth[32] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV32)); + if (SV_page == 63) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) + { + // Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32) + //! \TODO Read Anti-Spoofing, SV config + decode_gps_almanac_health_sf4(subframe_bits); + } } break; @@ -311,65 +445,38 @@ int32_t Gps_Navigation_Message::subframe_decoder(const char* subframe) b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); SV_data_ID_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); SV_page_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); - if ((SV_page_5 > 0) && (SV_page_5 < 25)) + if (d_system == LnavSystem::QZSS) { - if (SV_data_ID_5 != 0) + if (SV_data_ID_5 == QZSS_LNAV_DATA_ID) { - a_M_0 = static_cast(read_navigation_signed(subframe_bits, ALM_MZERO)); - a_M_0 = a_M_0 * ALM_MZERO_LSB; - a_ecc = static_cast(read_navigation_unsigned(subframe_bits, ALM_ECC)); - a_ecc = a_ecc * ALM_ECC_LSB; - a_sqrtA = static_cast(read_navigation_unsigned(subframe_bits, ALM_SQUAREA)); - a_sqrtA = a_sqrtA * ALM_SQUAREA_LSB; - a_OMEGA_0 = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGAZERO)); - a_OMEGA_0 = a_OMEGA_0 * ALM_OMEGAZERO_LSB; - a_omega = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGA)); - a_omega = a_omega * ALM_OMEGA_LSB; - a_OMEGAdot = static_cast(read_navigation_signed(subframe_bits, ALM_OMEGADOT)); - a_OMEGAdot = a_OMEGAdot * ALM_OMEGADOT_LSB; - a_delta_i = static_cast(read_navigation_signed(subframe_bits, ALM_DELTAI)); - a_delta_i = a_delta_i * ALM_DELTAI_LSB; - a_af0 = static_cast(read_navigation_signed(subframe_bits, ALM_AF0)); - a_af0 = a_af0 * ALM_AF0_LSB; - a_af1 = static_cast(read_navigation_signed(subframe_bits, ALM_AF1)); - a_af1 = a_af1 * ALM_AF1_LSB; - a_PRN = SV_page_5; - i_Toa = static_cast(read_navigation_unsigned(subframe_bits, ALM_TOA)); - i_Toa = i_Toa * ALM_TOA_LSB; - SV_Health = static_cast(read_navigation_unsigned(subframe_bits, ALM_SVHEALTH)); - flag_almanac_valid = true; + const uint32_t qzss_prn = qzss_prn_from_lnav_sv_id(SV_page_5); + if (qzss_prn != 0U) + { + decode_lnav_almanac(subframe_bits, qzss_prn, qzss_almanac_eccentricity_ref(qzss_prn), qzss_almanac_inclination_ref(qzss_prn)); + } + else if (SV_page_5 == QZSS_ALMANAC_EPOCH_HEALTH_SV_ID) + { + decode_qzss_almanac_epoch_health(subframe_bits); + } + else if (SV_page_5 == QZSS_IONO_UTC_WIDE_AREA_SV_ID || SV_page_5 == QZSS_IONO_UTC_JAPAN_AREA_SV_ID) + { + decode_lnav_iono_utc(subframe_bits); + } } } - if (SV_page_5 == 51) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) + else { - i_Toa = static_cast(read_navigation_unsigned(subframe_bits, T_OA)); - i_Toa = i_Toa * T_OA_LSB; - i_WN_A = static_cast(read_navigation_unsigned(subframe_bits, WN_A)); - flag_almanac_week_valid = true; - almanacHealth[1] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV1)); - almanacHealth[2] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV2)); - almanacHealth[3] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV3)); - almanacHealth[4] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV4)); - almanacHealth[5] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV5)); - almanacHealth[6] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV6)); - almanacHealth[7] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV7)); - almanacHealth[8] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV8)); - almanacHealth[9] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV9)); - almanacHealth[10] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV10)); - almanacHealth[11] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV11)); - almanacHealth[12] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV12)); - almanacHealth[13] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV13)); - almanacHealth[14] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV14)); - almanacHealth[15] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV15)); - almanacHealth[16] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV16)); - almanacHealth[17] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV17)); - almanacHealth[18] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV18)); - almanacHealth[19] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV19)); - almanacHealth[20] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV20)); - almanacHealth[21] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV21)); - almanacHealth[22] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV22)); - almanacHealth[23] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV23)); - almanacHealth[24] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV24)); + if ((SV_page_5 > 0) && (SV_page_5 < 25)) + { + if (SV_data_ID_5 != 0) + { + decode_lnav_almanac(subframe_bits, static_cast(SV_page_5), 0.0, 0.0); + } + } + if (SV_page_5 == 51) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) + { + decode_gps_almanac_health_sf5(subframe_bits); + } } break; @@ -503,6 +610,10 @@ Gps_Ephemeris Gps_Navigation_Message::get_ephemeris() const Gps_Almanac Gps_Navigation_Message::get_almanac() { Gps_Almanac almanac; + if (d_system == LnavSystem::QZSS) + { + almanac.set_system('J'); + } almanac.SV_health = SV_Health; almanac.PRN = a_PRN; almanac.delta_i = a_delta_i; @@ -558,6 +669,17 @@ Gps_Utc_Model Gps_Navigation_Message::get_utc_model() } +int32_t Gps_Navigation_Message::get_almanac_health(uint32_t prn) const +{ + const auto almanac_health = almanacHealth.find(prn); + if (almanac_health == almanacHealth.cend()) + { + return 0; + } + return almanac_health->second; +} + + bool Gps_Navigation_Message::satellite_validation() { bool flag_data_valid = false; diff --git a/src/core/system_parameters/gps_navigation_message.h b/src/core/system_parameters/gps_navigation_message.h index 9a60adb29..0441e3b4c 100644 --- a/src/core/system_parameters/gps_navigation_message.h +++ b/src/core/system_parameters/gps_navigation_message.h @@ -25,6 +25,7 @@ #include "gps_ephemeris.h" #include "gps_iono.h" #include "gps_utc_model.h" +#include "qzss.h" #include #include #include @@ -143,6 +144,11 @@ public: return flag_utc_model_valid; } + /*! + * \brief Gets the almanac health field for a satellite PRN + */ + int32_t get_almanac_health(uint32_t prn) const; + bool satellite_validation(); bool almanac_validation() const; @@ -151,6 +157,11 @@ private: int64_t read_navigation_signed(const std::bitset& bits, const std::vector>& parameter) const; bool read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const; void print_gps_word_bytes(uint32_t GPS_word) const; + void decode_lnav_almanac(const std::bitset& subframe_bits, uint32_t prn, double eccentricity_ref, double inclination_ref); + void decode_lnav_iono_utc(const std::bitset& subframe_bits); + void decode_gps_almanac_health_sf4(const std::bitset& subframe_bits); + void decode_gps_almanac_health_sf5(const std::bitset& subframe_bits); + void decode_qzss_almanac_epoch_health(const std::bitset& subframe_bits); std::map almanacHealth; //!< Map that stores the health information stored in the almanac diff --git a/src/core/system_parameters/qzss.h b/src/core/system_parameters/qzss.h index adb376f67..0a0ce9de5 100644 --- a/src/core/system_parameters/qzss.h +++ b/src/core/system_parameters/qzss.h @@ -52,6 +52,14 @@ constexpr const char QZSS_CA_PREAMBLE_SYMBOLS_STR[161] = "1111111111111111111100 constexpr const char QZSS_L5Q_NH_CODE_STR[21] = "00000100110101001110"; constexpr const char QZSS_L5I_NH_CODE_STR[11] = "0000110101"; +static constexpr int32_t QZSS_LNAV_DATA_ID = 3; +static constexpr int32_t QZSS_ALMANAC_EPOCH_HEALTH_SV_ID = 51; +static constexpr int32_t QZSS_IONO_UTC_WIDE_AREA_SV_ID = 56; +static constexpr int32_t QZSS_IONO_UTC_JAPAN_AREA_SV_ID = 61; +static constexpr uint32_t QZSS_PRN_OFFSET = 192U; +static constexpr double QZSS_QZO_ECCENTRICITY_REF = 0.06; +static constexpr double QZSS_QZO_INCLINATION_REF = 0.25; + /** \} */ /** \} */ diff --git a/tests/test_main.cc b/tests/test_main.cc index 6524305a9..933558f22 100644 --- a/tests/test_main.cc +++ b/tests/test_main.cc @@ -189,6 +189,7 @@ private: #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" +#include "unit-tests/system-parameters/qzss_lnav_navigation_message_test.cc" #ifndef EXCLUDE_TESTS_REQUIRING_BINARIES #include "unit-tests/control-plane/control_thread_test.cc" diff --git a/tests/unit-tests/system-parameters/qzss_lnav_navigation_message_test.cc b/tests/unit-tests/system-parameters/qzss_lnav_navigation_message_test.cc new file mode 100644 index 000000000..bc7e8beb3 --- /dev/null +++ b/tests/unit-tests/system-parameters/qzss_lnav_navigation_message_test.cc @@ -0,0 +1,237 @@ +/*! + * \file qzss_lnav_navigation_message_test.cc + * \brief Tests for QZSS LNAV navigation message decoding + * \author Carles Fernandez-Prades, 2026. 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-2026 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "GPS_L1_CA.h" +#include "gps_navigation_message.h" +#include "rtklib_conversions.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +int32_t qzss_lnav_field_width(const std::vector>& parameter) +{ + int32_t width = 0; + for (const auto& p : parameter) + { + width += p.second; + } + return width; +} + + +void set_qzss_lnav_unsigned_field(std::bitset& bits, + const std::vector>& parameter, + uint64_t value) +{ + int32_t pending_bits = qzss_lnav_field_width(parameter); + + for (const auto& p : parameter) + { + for (int32_t j = 0; j < p.second; ++j) + { + --pending_bits; + bits[GPS_SUBFRAME_BITS - p.first - j] = ((value >> pending_bits) & 1ULL) != 0ULL; + } + } +} + + +void set_qzss_lnav_signed_field(std::bitset& bits, + const std::vector>& parameter, + int64_t value) +{ + const int32_t width = qzss_lnav_field_width(parameter); + const uint64_t mask = (width == 64) ? std::numeric_limits::max() : ((1ULL << width) - 1ULL); + set_qzss_lnav_unsigned_field(bits, parameter, static_cast(value) & mask); +} + + +std::array qzss_lnav_subframe_bytes(const std::bitset& bits) +{ + std::array subframe{}; + + for (int32_t i = 0; i < 10; ++i) + { + uint32_t word = 0; + for (int32_t j = 0; j < GPS_WORD_BITS; ++j) + { + if (bits[GPS_WORD_BITS * (9 - i) + j]) + { + word |= (1U << j); + } + } + std::memcpy(&subframe[i * GPS_WORD_LENGTH], &word, sizeof(word)); + } + + return subframe; +} + + +std::bitset qzss_lnav_page(int32_t subframe_id, int32_t sv_id) +{ + std::bitset bits; + + set_qzss_lnav_unsigned_field(bits, TOW, 1); + set_qzss_lnav_unsigned_field(bits, SUBFRAME_ID, static_cast(subframe_id)); + set_qzss_lnav_unsigned_field(bits, SV_DATA_ID, 3); + set_qzss_lnav_unsigned_field(bits, SV_PAGE, static_cast(sv_id)); + + return bits; +} +} // namespace + + +TEST(QzssLnavNavigationMessageTest, DecodesQzoAlmanacFromSubframe4) +{ + Gps_Navigation_Message nav_message(LnavSystem::QZSS); + + auto bits = qzss_lnav_page(4, 2); + set_qzss_lnav_unsigned_field(bits, ALM_ECC, 100); + set_qzss_lnav_unsigned_field(bits, ALM_TOA, 7); + set_qzss_lnav_signed_field(bits, ALM_DELTAI, -25); + set_qzss_lnav_signed_field(bits, ALM_OMEGADOT, -30); + set_qzss_lnav_unsigned_field(bits, ALM_SVHEALTH, 0xAD); + set_qzss_lnav_unsigned_field(bits, ALM_SQUAREA, 5440000); + set_qzss_lnav_signed_field(bits, ALM_OMEGAZERO, -3000); + set_qzss_lnav_signed_field(bits, ALM_OMEGA, 2000); + set_qzss_lnav_signed_field(bits, ALM_MZERO, -1000); + set_qzss_lnav_signed_field(bits, ALM_AF0, -12); + set_qzss_lnav_signed_field(bits, ALM_AF1, 13); + + const auto subframe = qzss_lnav_subframe_bytes(bits); + EXPECT_EQ(4, nav_message.subframe_decoder(subframe.data())); + + ASSERT_FALSE(nav_message.almanac_validation()); + const auto almanac = nav_message.get_almanac(); + + EXPECT_EQ('J', almanac.get_system()); + EXPECT_EQ(194U, almanac.PRN); + EXPECT_EQ(7 * ALM_TOA_LSB, almanac.toa); + EXPECT_EQ(0xAD, almanac.SV_health); + EXPECT_NEAR(0.06 + 100.0 * ALM_ECC_LSB, almanac.ecc, 1e-14); + EXPECT_NEAR(0.25 - 25.0 * ALM_DELTAI_LSB, almanac.delta_i, 1e-14); + EXPECT_NEAR(-30.0 * ALM_OMEGADOT_LSB, almanac.OMEGAdot, 1e-18); + + const auto rtklib_almanac = alm_to_rtklib(almanac); + EXPECT_EQ(satno(SYS_QZS, 194), rtklib_almanac.sat); + EXPECT_NEAR(almanac.delta_i * GNSS_PI, rtklib_almanac.i0, 1e-12); +} + + +TEST(GpsLnavNavigationMessageTest, KeepsGpsAlmanacReferenceConvention) +{ + Gps_Navigation_Message nav_message; + + auto bits = qzss_lnav_page(5, 2); + set_qzss_lnav_unsigned_field(bits, ALM_ECC, 100); + set_qzss_lnav_unsigned_field(bits, ALM_TOA, 7); + set_qzss_lnav_signed_field(bits, ALM_DELTAI, -25); + set_qzss_lnav_unsigned_field(bits, ALM_SVHEALTH, 0x12); + + const auto subframe = qzss_lnav_subframe_bytes(bits); + EXPECT_EQ(5, nav_message.subframe_decoder(subframe.data())); + + const auto almanac = nav_message.get_almanac(); + EXPECT_EQ('G', almanac.get_system()); + EXPECT_EQ(2U, almanac.PRN); + EXPECT_EQ(7 * ALM_TOA_LSB, almanac.toa); + EXPECT_EQ(0x12, almanac.SV_health); + EXPECT_NEAR(100.0 * ALM_ECC_LSB, almanac.ecc, 1e-14); + EXPECT_NEAR(-25.0 * ALM_DELTAI_LSB, almanac.delta_i, 1e-14); + + const auto rtklib_almanac = alm_to_rtklib(almanac); + EXPECT_EQ(satno(SYS_GPS, 2), rtklib_almanac.sat); + EXPECT_NEAR((0.3 + almanac.delta_i) * GNSS_PI, rtklib_almanac.i0, 1e-12); +} + + +TEST(QzssLnavNavigationMessageTest, DecodesQzssAlmanacEpochAndHealthFromSubframe4) +{ + Gps_Navigation_Message nav_message(LnavSystem::QZSS); + + auto bits = qzss_lnav_page(4, 51); + set_qzss_lnav_unsigned_field(bits, T_OA, 9); + set_qzss_lnav_unsigned_field(bits, WN_A, 77); + set_qzss_lnav_unsigned_field(bits, HEALTH_SV1, 1); + set_qzss_lnav_unsigned_field(bits, HEALTH_SV4, 4); + set_qzss_lnav_unsigned_field(bits, HEALTH_SV8, 8); + set_qzss_lnav_unsigned_field(bits, HEALTH_SV10, 10); + + const auto subframe = qzss_lnav_subframe_bytes(bits); + EXPECT_EQ(4, nav_message.subframe_decoder(subframe.data())); + + EXPECT_EQ(1, nav_message.get_almanac_health(193)); + EXPECT_EQ(4, nav_message.get_almanac_health(196)); + EXPECT_EQ(8, nav_message.get_almanac_health(200)); + EXPECT_EQ(10, nav_message.get_almanac_health(202)); +} + + +TEST(QzssLnavNavigationMessageTest, DecodesJapanAreaIonoUtcFromSubframe5) +{ + Gps_Navigation_Message nav_message(LnavSystem::QZSS); + + auto bits = qzss_lnav_page(5, 61); + set_qzss_lnav_signed_field(bits, ALPHA_0, -1); + set_qzss_lnav_signed_field(bits, ALPHA_1, 2); + set_qzss_lnav_signed_field(bits, ALPHA_2, -3); + set_qzss_lnav_signed_field(bits, ALPHA_3, 4); + set_qzss_lnav_signed_field(bits, BETA_0, -5); + set_qzss_lnav_signed_field(bits, BETA_1, 6); + set_qzss_lnav_signed_field(bits, BETA_2, -7); + set_qzss_lnav_signed_field(bits, BETA_3, 8); + set_qzss_lnav_signed_field(bits, A_1, -9); + set_qzss_lnav_signed_field(bits, A_0, 10); + set_qzss_lnav_unsigned_field(bits, T_OT, 11); + set_qzss_lnav_unsigned_field(bits, WN_T, 12); + set_qzss_lnav_signed_field(bits, DELTAT_LS, -18); + set_qzss_lnav_unsigned_field(bits, WN_LSF, 13); + set_qzss_lnav_unsigned_field(bits, DN, 4); + set_qzss_lnav_signed_field(bits, DELTAT_LSF, -17); + + const auto subframe = qzss_lnav_subframe_bytes(bits); + EXPECT_EQ(5, nav_message.subframe_decoder(subframe.data())); + + ASSERT_TRUE(nav_message.get_flag_iono_valid()); + ASSERT_TRUE(nav_message.get_flag_utc_model_valid()); + + const auto iono = nav_message.get_iono(); + EXPECT_TRUE(iono.valid); + EXPECT_NEAR(-1.0 * ALPHA_0_LSB, iono.alpha0, 1e-18); + EXPECT_NEAR(2.0 * ALPHA_1_LSB, iono.alpha1, 1e-18); + EXPECT_NEAR(-3.0 * ALPHA_2_LSB, iono.alpha2, 1e-18); + EXPECT_NEAR(4.0 * ALPHA_3_LSB, iono.alpha3, 1e-18); + EXPECT_NEAR(-5.0 * BETA_0_LSB, iono.beta0, 1e-12); + EXPECT_NEAR(6.0 * BETA_1_LSB, iono.beta1, 1e-12); + + const auto utc = nav_message.get_utc_model(); + EXPECT_TRUE(utc.valid); + EXPECT_NEAR(-9.0 * A_1_LSB, utc.A1, 1e-18); + EXPECT_NEAR(10.0 * A_0_LSB, utc.A0, 1e-18); + EXPECT_EQ(11 * T_OT_LSB, utc.tot); + EXPECT_EQ(12, utc.WN_T); + EXPECT_EQ(-18, utc.DeltaT_LS); + EXPECT_EQ(13, utc.WN_LSF); + EXPECT_EQ(4, utc.DN); + EXPECT_EQ(-17, utc.DeltaT_LSF); +}