From c0f2a03dd0f1de10c7b1bd35787a41a715d8d0d5 Mon Sep 17 00:00:00 2001 From: joebre Date: Wed, 15 Jan 2025 14:02:04 +0100 Subject: [PATCH] gps almanac decoding Signed-off-by: joebre --- .../gps_l1_ca_telemetry_decoder_gs.cc | 13 ++- src/core/system_parameters/GPS_L1_CA.h | 23 ++++++ .../gps_navigation_message.cc | 79 ++++++++++++++++++- .../gps_navigation_message.h | 23 +++++- 4 files changed, 131 insertions(+), 7 deletions(-) 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 62bbf6bfa..ba4c6d688 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 @@ -396,10 +396,19 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe(double cn0, bool flag_inver 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()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } break; case 5: - // get almanac (if available) - // TODO: implement almanac reader in navigation_message + if (d_nav.almanac_validation() == true) + { + const std::shared_ptr tmp_obj = std::make_shared(d_nav.get_almanac()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; default: break; } diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 842dbbf3b..8d4415d51 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -226,6 +226,29 @@ const std::vector> HEALTH_SV23({{253, 6}}); const std::vector> HEALTH_SV24({{259, 6}}); +// Almanac +const std::vector> ALM_ECC({{69, 16}}); +constexpr double ALM_ECC_LSB = TWO_N21; +const std::vector> ALM_TOA({{91, 8}}); +constexpr int32_t ALM_TOA_LSB = static_cast(TWO_P12); +const std::vector> ALM_DELTAI({{99, 16}}); +constexpr double ALM_DELTAI_LSB = TWO_N19; +const std::vector> ALM_OMEGADOT({{121, 16}}); +constexpr double ALM_OMEGADOT_LSB = TWO_N38; +const std::vector> ALM_SVHEALTH({{137, 8}}); +const std::vector> ALM_SQUAREA({{151, 24}}); +constexpr double ALM_SQUAREA_LSB = TWO_N11; +const std::vector> ALM_OMEGAZERO({{181, 24}}); +constexpr double ALM_OMEGAZERO_LSB = TWO_N23; +const std::vector> ALM_OMEGA({{211, 24}}); +constexpr double ALM_OMEGA_LSB = TWO_N23; +const std::vector> ALM_MZERO({{241, 24}}); +constexpr double ALM_MZERO_LSB = TWO_N23; + +const std::vector> ALM_AF0({{271, 8}, {290, 3}}); +constexpr double ALM_AF0_LSB = TWO_N20; +const std::vector> ALM_AF1({{279, 11}}); +constexpr double ALM_AF1_LSB = TWO_N38; /** \} */ /** \} */ #endif // GNSS_SDR_GPS_L1_CA_H diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index b17458da7..622b14c26 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -201,9 +201,31 @@ int32_t Gps_Navigation_Message::subframe_decoder(const char* subframe) 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) { - //! \TODO read almanac if (SV_data_ID != 0) { + 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 ; } } @@ -276,11 +298,34 @@ 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 < 25) + if ((SV_page_5 > 0) && (SV_page_5 < 25)) { - //! \TODO read almanac if (SV_data_ID_5 != 0) { + 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; + } } if (SV_page_5 == 51) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200M) @@ -288,6 +333,7 @@ int32_t Gps_Navigation_Message::subframe_decoder(const char* subframe) 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)); @@ -435,6 +481,27 @@ Gps_Ephemeris Gps_Navigation_Message::get_ephemeris() const } +Gps_Almanac Gps_Navigation_Message::get_almanac() +{ + Gps_Almanac almanac; + almanac.SV_health = SV_Health; + almanac.PRN = a_PRN; + almanac.delta_i = a_delta_i; + almanac.toa = i_Toa; + almanac.WNa = i_WN_A; + almanac.M_0 = a_M_0; + almanac.ecc = a_ecc; + almanac.sqrtA = a_sqrtA; + almanac.OMEGA_0 = a_OMEGA_0; + almanac.omega = a_omega; + almanac.OMEGAdot = a_OMEGAdot; + almanac.af0 = a_af0; + almanac.af1 = a_af1; + flag_almanac_valid = false; + return almanac; +} + + Gps_Iono Gps_Navigation_Message::get_iono() { Gps_Iono iono; @@ -490,3 +557,9 @@ bool Gps_Navigation_Message::satellite_validation() } return flag_data_valid; } + + +bool Gps_Navigation_Message::almanac_validation() +{ + return flag_almanac_valid && (i_WN_A > 0); +} diff --git a/src/core/system_parameters/gps_navigation_message.h b/src/core/system_parameters/gps_navigation_message.h index 16e8b2a9f..22f09c7e5 100644 --- a/src/core/system_parameters/gps_navigation_message.h +++ b/src/core/system_parameters/gps_navigation_message.h @@ -22,6 +22,7 @@ #include "GPS_L1_CA.h" #include "gps_ephemeris.h" +#include "gps_almanac.h" #include "gps_iono.h" #include "gps_utc_model.h" #include @@ -61,7 +62,12 @@ public: Gps_Iono get_iono(); /*! - * \brief Obtain a GPS UTC model parameters class filled with current SV data + * \brief Obtain a GPS almanac class filled with current SV data + */ + Gps_Almanac get_almanac(); + + /*! + * \brief Obtain a GPS Almanac model parameters class filled with current SV data */ Gps_Utc_Model get_utc_model(); @@ -133,6 +139,7 @@ public: } bool satellite_validation(); + bool almanac_validation(); private: uint64_t read_navigation_unsigned(const std::bitset& bits, const std::vector>& parameter) const; @@ -194,6 +201,17 @@ private: // Almanac int32_t i_Toa{}; // Almanac reference time [s] int32_t i_WN_A{}; // Modulo 256 of the GPS week number to which the almanac reference time (i_Toa) is referenced + int32_t SV_Health{}; // Almanac SV healt + uint32_t a_PRN; // Almanac PRN + double a_delta_i{}; // Inclination Angle at Reference Time (relative to i_0 = 0.30 semi-circles) + double a_M_0{}; // Mean Anomaly at Reference Time [semi-circles] + double a_ecc{}; // Eccentricity [dimensionless] + double a_sqrtA{}; // Square Root of the Semi-Major Axis [sqrt(m)] + double a_OMEGA_0{}; // Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] + double a_omega{}; // Argument of Perigee [semi-cicles] + double a_OMEGAdot{}; // Rate of Right Ascension [semi-circles/s] + double a_af0{}; // Coefficient 0 of code phase offset model [s] + double a_af1{}; // Coefficient 1 of code phase offset model [s/s] // satellite identification info int32_t i_channel_ID{}; @@ -224,7 +242,8 @@ private: bool b_valid_ephemeris_set_flag{}; // flag indicating that this ephemeris set have passed the validation check bool flag_iono_valid{}; // If set, it indicates that the ionospheric parameters are filled (page 18 has arrived and decoded) bool flag_utc_model_valid{}; // If set, it indicates that the UTC model parameters are filled - + bool flag_almanac_valid{}; // If set, it indicates that the almanac is filled + bool flag_almanac_week_valid{}; // If set, it indicates that the almanac week is valid /* If true, enhanced level of integrity assurance. * * If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.