diff --git a/CMakeLists.txt b/CMakeLists.txt index 8289159cf..0baefcf8b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -321,7 +321,7 @@ set(GNSSSDR_PROTOBUF_MIN_VERSION "3.0.0") ################################################################################ set(GNSSSDR_GFLAGS_LOCAL_VERSION "2.2.2") set(GNSSSDR_GLOG_LOCAL_VERSION "0.6.0") -set(GNSSSDR_ARMADILLO_LOCAL_VERSION "11.0.x") +set(GNSSSDR_ARMADILLO_LOCAL_VERSION "11.1.x") if((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS 5.0) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS 5.0)) set(GNSSSDR_GTEST_LOCAL_VERSION "1.10.x") diff --git a/README.md b/README.md index eb3d62fff..4eebe1323 100644 --- a/README.md +++ b/README.md @@ -381,9 +381,9 @@ $ sudo apt-get install libblas-dev liblapack-dev # For Debian/Ubuntu/Linux $ sudo yum install lapack-devel blas-devel # For Fedora/CentOS/RHEL $ sudo zypper install lapack-devel blas-devel # For OpenSUSE $ sudo pacman -S blas lapack # For Arch Linux -$ wget https://sourceforge.net/projects/arma/files/armadillo-11.0.1.tar.xz -$ tar xvfz armadillo-11.0.1.tar.xz -$ cd armadillo-11.0.1 +$ wget https://sourceforge.net/projects/arma/files/armadillo-11.1.1.tar.xz +$ tar xvfz armadillo-11.1.1.tar.xz +$ cd armadillo-11.1.1 $ cmake . $ make $ sudo make install diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index 6a1c599ff..b1567b209 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -26,9 +26,16 @@ All notable changes to GNSS-SDR will be documented in this file. ### Improvements in Usability: -- Fixed large GLONASS velocity errors when using the +- Fixed large GLONASS velocity errors and the extended correlator when using the `GLONASS_L1_CA_DLL_PLL_C_Aid_Tracking` and `GLONASS_L2_CA_DLL_PLL_C_Aid_Tracking` implementations. +- Added a over-the-wire sample format (that is, the format used between the + device and the UHD) configuration parameter for the `UHD_Signal_Source`, thus + allowing to select the `sc8` format instead of the default `sc16`. This would + reduce the dynamic range and increase quantization noise, but also reduce the + load on the data link and thus allow more bandwidth. +- Added gain setting and reading for the XTRX board when using the + `Osmosdr_Signal_Source` implementation of a `SignalSource`. See the definitions of concepts and metrics at https://gnss-sdr.org/design-forces/ diff --git a/src/algorithms/PVT/libs/has_simple_printer.cc b/src/algorithms/PVT/libs/has_simple_printer.cc index 4665e7454..e29df9809 100644 --- a/src/algorithms/PVT/libs/has_simple_printer.cc +++ b/src/algorithms/PVT/libs/has_simple_printer.cc @@ -183,7 +183,7 @@ bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data) d_has_file << indent << indent << "Code Bias Flag: " << static_cast(has_data->header.code_bias_flag) << '\n'; d_has_file << indent << indent << "Phase Bias Flag: " << static_cast(has_data->header.phase_bias_flag) << '\n'; d_has_file << indent << indent << "Mask ID: " << static_cast(has_data->header.mask_id) << '\n'; - d_has_file << indent << indent << "IOD Set ID: " << static_cast(has_data->header.iod_id) << "\n\n"; + d_has_file << indent << indent << "IOD Set ID: " << static_cast(has_data->header.iod_set_id) << "\n\n"; d_has_file << indent << "MT1 Body\n"; d_has_file << indent << "--------\n"; @@ -240,7 +240,7 @@ bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data) d_has_file << indent << indent << "Validity interval: " << static_cast(has_data->validity_interval_index_orbit_corrections) << '\n'; d_has_file << indent << indent << "GNSS IOD: " << print_vector(has_data->gnss_iod) << '\n'; d_has_file << indent << indent << "Delta Radial [m]: " << print_vector(has_data->delta_radial, HAS_MSG_DELTA_RADIAL_SCALE_FACTOR) << '\n'; - d_has_file << indent << indent << "Delta Along Track [m]: " << print_vector(has_data->delta_along_track, HAS_MSG_DELTA_ALONG_TRACK_SCALE_FACTOR) << '\n'; + d_has_file << indent << indent << "Delta In-Track [m]: " << print_vector(has_data->delta_in_track, HAS_MSG_DELTA_IN_TRACK_SCALE_FACTOR) << '\n'; d_has_file << indent << indent << "Delta Cross Track [m]: " << print_vector(has_data->delta_cross_track, HAS_MSG_DELTA_CROSS_TRACK_SCALE_FACTOR) << '\n'; } @@ -249,9 +249,9 @@ bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data) d_has_file << '\n'; d_has_file << indent << indent << "Clock Full-set Corrections Block\n"; d_has_file << indent << indent << "--------------------------------\n"; - d_has_file << indent << indent << "Validity interval: " << static_cast(has_data->validity_interval_index_clock_fullset_corrections) << '\n'; - d_has_file << indent << indent << "Delta Clock C0 Multiplier: " << print_vector(has_data->delta_clock_c0_multiplier) << '\n'; - d_has_file << indent << indent << "Delta Clock C0 [m]: " << print_vector(has_data->delta_clock_c0, HAS_MSG_DELTA_CLOCK_SCALE_FACTOR) << '\n'; + d_has_file << indent << indent << "Validity interval: " << static_cast(has_data->validity_interval_index_clock_fullset_corrections) << '\n'; + d_has_file << indent << indent << "Delta Clock Multiplier: " << print_vector(has_data->delta_clock_multiplier) << '\n'; + d_has_file << indent << indent << "Delta Clock Correction [m]: " << print_vector(has_data->delta_clock_correction, HAS_MSG_DELTA_CLOCK_SCALE_FACTOR) << '\n'; } if (has_data->header.clock_subset_flag == true) @@ -260,12 +260,12 @@ bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data) d_has_file << indent << indent << "Clock Subset Corrections Block\n"; d_has_file << indent << indent << "------------------------------\n"; d_has_file << indent << indent << "Validity interval: " << static_cast(has_data->validity_interval_index_clock_subset_corrections) << '\n'; - d_has_file << indent << indent << "Nsysprime: " << static_cast(has_data->Nsysprime) << '\n'; + d_has_file << indent << indent << "Nsys_sub: " << static_cast(has_data->Nsys_sub) << '\n'; d_has_file << indent << indent << "GNSS ID: " << print_vector(has_data->gnss_id_clock_subset) << '\n'; - d_has_file << indent << indent << "Delta Clock C0 Multiplier: " << print_vector(has_data->delta_clock_c0_multiplier_clock_subset) << '\n'; + d_has_file << indent << indent << "Delta Clock Multiplier: " << print_vector(has_data->delta_clock_multiplier_clock_subset) << '\n'; d_has_file << indent << indent << "Satellite sub-mask: "; - int Nsatprime = 0; - for (uint8_t k = 0; k < has_data->Nsysprime; k++) + int Nsat_sub = 0; + for (uint8_t k = 0; k < has_data->Nsys_sub; k++) { auto it = std::find(has_data->gnss_id_mask.begin(), has_data->gnss_id_mask.end(), has_data->gnss_id_clock_subset[k]); if (it != has_data->gnss_id_mask.end()) @@ -290,14 +290,14 @@ bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data) mask <<= 1; } d_has_file << binary << " "; - Nsatprime += std::count(binary.begin(), binary.end(), '1'); + Nsat_sub += std::count(binary.begin(), binary.end(), '1'); } } d_has_file << '\n'; - d_has_file << " Nsat in subset = " << Nsatprime << '\n'; - const std::string text("Delta Clock C0 [m]: "); + d_has_file << " Nsat in subset = " << Nsat_sub << '\n'; + const std::string text("Delta Clock Correction [m]: "); const std::string filler(indent.length() * 2 + text.length(), ' '); - d_has_file << indent << indent << text << print_matrix(has_data->delta_clock_c0_clock_subset, filler, HAS_MSG_DELTA_CLOCK_SCALE_FACTOR); + d_has_file << indent << indent << text << print_matrix(has_data->delta_clock_correction_clock_subset, filler, HAS_MSG_DELTA_CLOCK_SCALE_FACTOR); } if (has_data->header.code_bias_flag == true) diff --git a/src/algorithms/PVT/libs/rtcm.cc b/src/algorithms/PVT/libs/rtcm.cc index b4860fe27..5beb3d9ea 100644 --- a/src/algorithms/PVT/libs/rtcm.cc +++ b/src/algorithms/PVT/libs/rtcm.cc @@ -31,7 +31,7 @@ #include // for std::reverse #include // for std::fmod, std::lround #include // for strtol -#include // for cout +#include // for std::cout #include // for std::stringstream @@ -3249,6 +3249,619 @@ std::string Rtcm::get_MSM_7_content_signal_data(const Gps_Ephemeris& ephNAV, return signal_data; } +// SSR + +uint8_t Rtcm::ssr_update_interval(uint16_t validity_seconds) const +{ + uint8_t ssr_update_interval = 0; + if (validity_seconds > 0) + { + if (validity_seconds < 2) + { + ssr_update_interval = 0; + } + else if (validity_seconds < 5) + { + ssr_update_interval = 1; + } + else if (validity_seconds < 10) + { + ssr_update_interval = 2; + } + else if (validity_seconds < 15) + { + ssr_update_interval = 3; + } + else if (validity_seconds < 30) + { + ssr_update_interval = 4; + } + else if (validity_seconds < 60) + { + ssr_update_interval = 5; + } + else if (validity_seconds < 120) + { + ssr_update_interval = 6; + } + else if (validity_seconds < 240) + { + ssr_update_interval = 7; + } + else if (validity_seconds < 300) + { + ssr_update_interval = 8; + } + else if (validity_seconds < 600) + { + ssr_update_interval = 9; + } + else if (validity_seconds < 900) + { + ssr_update_interval = 10; + } + else if (validity_seconds < 1800) + { + ssr_update_interval = 11; + } + else if (validity_seconds < 3600) + { + ssr_update_interval = 12; + } + else if (validity_seconds < 7200) + { + ssr_update_interval = 13; + } + else if (validity_seconds < 10800) + { + ssr_update_interval = 14; + } + else + { + ssr_update_interval = 15; + } + } + return ssr_update_interval; +} + + +std::vector Rtcm::print_IGM01(const Galileo_HAS_data& has_data) +{ + std::vector msgs; + const uint8_t nsys = has_data.Nsys; + bool ssr_multiple_msg_indicator = true; + for (uint8_t sys = 0; sys < nsys; sys++) + { + if (sys == nsys - 1) + { + ssr_multiple_msg_indicator = false; // last message of a sequence + } + const std::string header = Rtcm::get_IGM01_header(has_data, sys, ssr_multiple_msg_indicator); + const std::string sat_data = Rtcm::get_IGM01_content_sat(has_data, sys); + std::string message = build_message(header + sat_data); + if (server_is_running) + { + rtcm_message_queue->push(message); + } + msgs.push_back(message); + } + return msgs; +} + + +std::vector Rtcm::print_IGM02(const Galileo_HAS_data& has_data) +{ + std::vector msgs; + const uint8_t nsys = has_data.Nsys; + bool ssr_multiple_msg_indicator = true; + for (uint8_t sys = 0; sys < nsys; sys++) + { + if (sys == nsys - 1) + { + ssr_multiple_msg_indicator = false; // last message of a sequence + } + const std::string header = Rtcm::get_IGM02_header(has_data, sys, ssr_multiple_msg_indicator); + const std::string sat_data = Rtcm::get_IGM02_content_sat(has_data, sys); + std::string message = build_message(header + sat_data); + if (server_is_running) + { + rtcm_message_queue->push(message); + } + msgs.push_back(message); + } + return msgs; +} + + +std::vector Rtcm::print_IGM03(const Galileo_HAS_data& has_data) +{ + std::vector msgs; + const uint8_t nsys = has_data.Nsys; + bool ssr_multiple_msg_indicator = true; + for (uint8_t sys = 0; sys < nsys; sys++) + { + if (sys == nsys - 1) + { + ssr_multiple_msg_indicator = false; // last message of a sequence + } + const std::string header = Rtcm::get_IGM03_header(has_data, sys, ssr_multiple_msg_indicator); + const std::string sat_data = Rtcm::get_IGM03_content_sat(has_data, sys); + std::string message = build_message(header + sat_data); + if (server_is_running) + { + rtcm_message_queue->push(message); + } + msgs.push_back(message); + } + return msgs; +} + + +std::vector Rtcm::print_IGM05(const Galileo_HAS_data& has_data) +{ + std::vector msgs; + const uint8_t nsys = has_data.Nsys; + bool ssr_multiple_msg_indicator = true; + for (uint8_t sys = 0; sys < nsys; sys++) + { + if (sys == nsys - 1) + { + ssr_multiple_msg_indicator = false; // last message of a sequence + } + const std::string header = Rtcm::get_IGM05_header(has_data, sys, ssr_multiple_msg_indicator); + const std::string sat_data = Rtcm::get_IGM05_content_sat(has_data, sys); + if (!sat_data.empty()) + { + std::string message = build_message(header + sat_data); + if (server_is_running) + { + rtcm_message_queue->push(message); + } + msgs.push_back(message); + } + } + return msgs; +} + + +std::string Rtcm::get_IGM01_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator) +{ + std::string header; + + uint32_t tow = 0; // TODO + uint16_t ssr_provider_id = 0; // ? + uint8_t igm_version = 0; // ? + uint8_t ssr_solution_id = 0; // ? + auto iod_ssr = has_data.header.iod_set_id % 15; // ?? HAS IOD is 0-31 + bool regional_indicator = false; // ? + + uint8_t subtype_msg_number = 0; + if (has_data.gnss_id_mask[nsys] == 0) // GPS + { + subtype_msg_number = 21; + } + else if (has_data.gnss_id_mask[nsys] == 2) // Galileo + { + subtype_msg_number = 61; + } + + uint8_t validity_index = has_data.validity_interval_index_orbit_corrections; + uint16_t validity_seconds = has_data.get_validity_interval_s(validity_index); + uint8_t ssr_update_interval_ = ssr_update_interval(validity_seconds); + uint8_t Nsat = has_data.get_num_satellites()[nsys]; + + Rtcm::set_DF002(4076); // Always “4076” for IGS Proprietary Messages + Rtcm::set_IDF001(igm_version); + Rtcm::set_IDF002(subtype_msg_number); + Rtcm::set_IDF003(tow); + Rtcm::set_IDF004(ssr_update_interval_); + Rtcm::set_IDF005(ssr_multiple_msg_indicator); + Rtcm::set_IDF007(iod_ssr); + Rtcm::set_IDF008(ssr_provider_id); + Rtcm::set_IDF009(ssr_solution_id); + Rtcm::set_IDF006(regional_indicator); + Rtcm::set_IDF010(Nsat); + + header += DF002.to_string() + IDF001.to_string() + IDF002.to_string() + + IDF003.to_string() + IDF004.to_string() + IDF005.to_string() + + IDF007.to_string() + IDF008.to_string() + IDF009.to_string() + + IDF006.to_string() + IDF010.to_string(); + return header; +} + + +std::string Rtcm::get_IGM01_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index) +{ + std::string content; + + std::vector prn = has_data.get_PRNs_in_mask(nsys_index); + std::vector gnss_iod = has_data.get_gnss_iod(nsys_index); + std::vector delta_orbit_radial_m = has_data.get_delta_radial_m(nsys_index); + std::vector delta_orbit_in_track_m = has_data.get_delta_in_track_m(nsys_index); + std::vector delta_orbit_cross_track_m = has_data.get_delta_cross_track_m(nsys_index); + + const uint8_t num_sats_in_this_system = has_data.get_num_satellites()[nsys_index]; + for (uint8_t sat = 0; sat < num_sats_in_this_system; sat++) + { + Rtcm::set_IDF011(static_cast(prn[sat])); + Rtcm::set_IDF012(static_cast(gnss_iod[sat] % 255)); // 8 LSBs + Rtcm::set_IDF013(delta_orbit_radial_m[sat]); + Rtcm::set_IDF014(delta_orbit_in_track_m[sat]); + Rtcm::set_IDF016(0.0); // dot_orbit_delta_track_m_s + Rtcm::set_IDF015(delta_orbit_cross_track_m[sat]); + Rtcm::set_IDF017(0.0); // dot_orbit_delta_in_track_m_s + Rtcm::set_IDF018(0.0); // dot_orbit_delta_cross_track_m_s + + content += IDF011.to_string() + IDF012.to_string() + IDF013.to_string() + + IDF014.to_string() + IDF016.to_string() + IDF015.to_string() + + IDF017.to_string() + IDF018.to_string(); + } + + return content; +} + + +std::string Rtcm::get_IGM02_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator) +{ + std::string header; + + uint32_t tow = 0; // TODO + uint16_t ssr_provider_id = 0; // ? + uint8_t igm_version = 0; // ? + uint8_t ssr_solution_id = 0; // ? + auto iod_ssr = has_data.header.iod_set_id % 15; // ?? HAS IOD is 0-31 + + uint8_t subtype_msg_number = 0; + if (has_data.gnss_id_mask[nsys] == 0) // GPS + { + subtype_msg_number = 22; + } + else if (has_data.gnss_id_mask[nsys] == 2) // Galileo + { + subtype_msg_number = 62; + } + + uint8_t validity_index = has_data.validity_interval_index_orbit_corrections; + uint16_t validity_seconds = has_data.get_validity_interval_s(validity_index); + uint8_t ssr_update_interval_ = ssr_update_interval(validity_seconds); + uint8_t Nsat = has_data.get_num_satellites()[nsys]; + + Rtcm::set_DF002(4076); // Always “4076” for IGS Proprietary Messages + Rtcm::set_IDF001(igm_version); + Rtcm::set_IDF002(subtype_msg_number); + Rtcm::set_IDF003(tow); + Rtcm::set_IDF004(ssr_update_interval_); + Rtcm::set_IDF005(ssr_multiple_msg_indicator); + Rtcm::set_IDF007(iod_ssr); + Rtcm::set_IDF008(ssr_provider_id); + Rtcm::set_IDF009(ssr_solution_id); + Rtcm::set_IDF010(Nsat); + + header += DF002.to_string() + IDF001.to_string() + IDF002.to_string() + + IDF003.to_string() + IDF004.to_string() + IDF005.to_string() + + IDF007.to_string() + IDF008.to_string() + IDF009.to_string() + + IDF010.to_string(); + return header; +} + + +std::string Rtcm::get_IGM02_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index) +{ + std::string content; + + const uint8_t num_sats_in_this_system = has_data.get_num_satellites()[nsys_index]; + + std::vector prn = has_data.get_PRNs_in_mask(nsys_index); + + std::vector delta_clock_c0 = has_data.get_delta_clock_correction_m(nsys_index); + std::vector delta_clock_c1(num_sats_in_this_system); + std::vector delta_clock_c2(num_sats_in_this_system); + + for (uint8_t sat = 0; sat < num_sats_in_this_system; sat++) + { + Rtcm::set_IDF011(static_cast(prn[sat])); + Rtcm::set_IDF019(delta_clock_c0[sat]); + Rtcm::set_IDF020(delta_clock_c1[sat]); + Rtcm::set_IDF021(delta_clock_c2[sat]); + + content += IDF011.to_string() + IDF019.to_string() + IDF020.to_string() + + IDF021.to_string(); + } + + return content; +} + + +std::string Rtcm::get_IGM03_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator) +{ + std::string header; + + uint32_t tow = 0; // TODO + uint16_t ssr_provider_id = 0; // ? + uint8_t igm_version = 0; // ? + uint8_t ssr_solution_id = 0; // ? + auto iod_ssr = has_data.header.iod_set_id % 15; // ?? HAS IOD is 0-31 + bool regional_indicator = false; // ? + + uint8_t subtype_msg_number = 0; + if (has_data.gnss_id_mask[nsys] == 0) // GPS + { + subtype_msg_number = 23; + } + else if (has_data.gnss_id_mask[nsys] == 2) // Galileo + { + subtype_msg_number = 63; + } + + uint8_t validity_index = has_data.validity_interval_index_orbit_corrections; + uint16_t validity_seconds = has_data.get_validity_interval_s(validity_index); + uint8_t ssr_update_interval_ = ssr_update_interval(validity_seconds); + uint8_t Nsat = has_data.get_num_satellites()[nsys]; + + Rtcm::set_DF002(4076); // Always “4076” for IGS Proprietary Messages + Rtcm::set_IDF001(igm_version); + Rtcm::set_IDF002(subtype_msg_number); + Rtcm::set_IDF003(tow); + Rtcm::set_IDF004(ssr_update_interval_); + Rtcm::set_IDF005(ssr_multiple_msg_indicator); + Rtcm::set_IDF007(iod_ssr); + Rtcm::set_IDF008(ssr_provider_id); + Rtcm::set_IDF009(ssr_solution_id); + Rtcm::set_IDF006(regional_indicator); + Rtcm::set_IDF010(Nsat); + + header += DF002.to_string() + IDF001.to_string() + IDF002.to_string() + + IDF003.to_string() + IDF004.to_string() + IDF005.to_string() + + IDF007.to_string() + IDF008.to_string() + IDF009.to_string() + + IDF006.to_string() + IDF010.to_string(); + return header; +} + + +std::string Rtcm::get_IGM03_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index) +{ + std::string content; + + const uint8_t num_sats_in_this_system = has_data.get_num_satellites()[nsys_index]; + + std::vector prn = has_data.get_PRNs_in_mask(nsys_index); + std::vector gnss_iod = has_data.get_gnss_iod(nsys_index); + std::vector delta_orbit_radial_m = has_data.get_delta_radial_m(nsys_index); + std::vector delta_orbit_in_track_m = has_data.get_delta_in_track_m(nsys_index); + std::vector delta_orbit_cross_track_m = has_data.get_delta_cross_track_m(nsys_index); + std::vector delta_clock_c0 = has_data.get_delta_clock_correction_m(nsys_index); + std::vector delta_clock_c1(num_sats_in_this_system); + std::vector delta_clock_c2(num_sats_in_this_system); + + for (uint8_t sat = 0; sat < num_sats_in_this_system; sat++) + { + Rtcm::set_IDF011(static_cast(prn[sat])); + Rtcm::set_IDF012(static_cast(gnss_iod[sat] % 255)); // 8 LSBs + Rtcm::set_IDF013(delta_orbit_radial_m[sat]); + Rtcm::set_IDF014(delta_orbit_in_track_m[sat]); + Rtcm::set_IDF015(delta_orbit_cross_track_m[sat]); + Rtcm::set_IDF016(0.0); // dot_orbit_delta_track_m_s + Rtcm::set_IDF017(0.0); // dot_orbit_delta_in_track_m_s + Rtcm::set_IDF018(0.0); // dot_orbit_delta_cross_track_m_s + Rtcm::set_IDF019(delta_clock_c0[sat]); + Rtcm::set_IDF020(delta_clock_c1[sat]); + Rtcm::set_IDF021(delta_clock_c2[sat]); + + content += IDF011.to_string() + IDF012.to_string() + IDF013.to_string() + + IDF014.to_string() + IDF015.to_string() + IDF016.to_string() + + IDF017.to_string() + IDF018.to_string() + DF019.to_string() + + IDF020.to_string() + IDF021.to_string(); + } + + return content; +} + + +std::string Rtcm::get_IGM05_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator) +{ + std::string header; + + uint32_t tow = 0; // TODO + uint16_t ssr_provider_id = 0; // ? + uint8_t igm_version = 0; // ? + uint8_t ssr_solution_id = 0; // ? + auto iod_ssr = has_data.header.iod_set_id % 15; // ?? HAS IOD is 0-31 + + uint8_t subtype_msg_number = 0; + if (has_data.gnss_id_mask[nsys] == 0) // GPS + { + subtype_msg_number = 25; + } + else if (has_data.gnss_id_mask[nsys] == 2) // Galileo + { + subtype_msg_number = 65; + } + + uint8_t validity_index = has_data.validity_interval_index_orbit_corrections; + uint16_t validity_seconds = has_data.get_validity_interval_s(validity_index); + uint8_t ssr_update_interval_ = ssr_update_interval(validity_seconds); + uint8_t Nsat = has_data.get_num_satellites()[nsys]; + + Rtcm::set_DF002(4076); // Always “4076” for IGS Proprietary Messages + Rtcm::set_IDF001(igm_version); + Rtcm::set_IDF002(subtype_msg_number); + Rtcm::set_IDF003(tow); + Rtcm::set_IDF004(ssr_update_interval_); + Rtcm::set_IDF005(ssr_multiple_msg_indicator); + Rtcm::set_IDF007(iod_ssr); + Rtcm::set_IDF008(ssr_provider_id); + Rtcm::set_IDF009(ssr_solution_id); + Rtcm::set_IDF010(Nsat); + + header += DF002.to_string() + IDF001.to_string() + IDF002.to_string() + + IDF003.to_string() + IDF004.to_string() + IDF005.to_string() + + IDF007.to_string() + IDF008.to_string() + IDF009.to_string() + + IDF010.to_string(); + return header; +} + + +std::string Rtcm::get_IGM05_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index) +{ + std::string content; + + const uint8_t num_sats_in_this_system = has_data.get_num_satellites()[nsys_index]; + std::vector prn = has_data.get_PRNs_in_mask(nsys_index); + std::vector> code_bias_m = has_data.get_code_bias_m(); + + for (uint8_t sat = 0; sat < num_sats_in_this_system; sat++) + { + uint8_t num_bias_processed = has_data.get_signals_in_mask(nsys_index).size(); + + uint8_t valid_num_bias_processed = 0; + std::vector gnss_signal_tracking_mode_id_v; + std::vector valid_bias_v; + + for (uint8_t code = 0; code < num_bias_processed; code++) + { + std::string code_string = has_data.get_signals_in_mask(nsys_index)[code]; + if (has_data.gnss_id_mask[nsys_index] == 0) // GPS + { + if (code_string == "L1 C/A") + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L1C(D)") + { + gnss_signal_tracking_mode_id_v.push_back(3); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L1C(P)") + { + gnss_signal_tracking_mode_id_v.push_back(4); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L2 CM") + { + gnss_signal_tracking_mode_id_v.push_back(7); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L2 CL") + { + gnss_signal_tracking_mode_id_v.push_back(8); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L5 I") + { + gnss_signal_tracking_mode_id_v.push_back(14); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "L5 Q") + { + gnss_signal_tracking_mode_id_v.push_back(15); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(false); + } + } + else if (has_data.gnss_id_mask[nsys_index] == 2) // Galileo + { + if (code_string == "E1-B I/NAV OS") + { + gnss_signal_tracking_mode_id_v.push_back(1); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E1-C") + { + gnss_signal_tracking_mode_id_v.push_back(2); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5a-I F/NAV OS") + { + gnss_signal_tracking_mode_id_v.push_back(5); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5a-Q") + { + gnss_signal_tracking_mode_id_v.push_back(6); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5b-I I/NAV OS") + { + gnss_signal_tracking_mode_id_v.push_back(8); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E5b-Q") + { + gnss_signal_tracking_mode_id_v.push_back(9); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E6-B C/NAV HAS") + { + gnss_signal_tracking_mode_id_v.push_back(15); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else if (code_string == "E6-C") + { + gnss_signal_tracking_mode_id_v.push_back(16); + valid_bias_v.push_back(true); + valid_num_bias_processed++; + } + else + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(false); + } + } + else + { + gnss_signal_tracking_mode_id_v.push_back(0); + valid_bias_v.push_back(false); + } + } + + if (valid_num_bias_processed > 0) + { + Rtcm::set_IDF011(static_cast(prn[sat])); + Rtcm::set_IDF023(valid_num_bias_processed); + + content += IDF011.to_string() + IDF023.to_string(); + + uint8_t num_sats_in_previous_systems = 0; + for (uint8_t nsys = 0; nsys < nsys_index; nsys++) + { + num_sats_in_previous_systems += has_data.get_num_satellites()[nsys]; + } + uint8_t sat_index = sat + num_sats_in_previous_systems; + + for (uint8_t code = 0; code < num_bias_processed; code++) + { + if (valid_bias_v[code] == true) + { + Rtcm::set_IDF024(gnss_signal_tracking_mode_id_v[code]); + Rtcm::set_IDF025(code_bias_m[sat_index][code]); + content += DF024.to_string() + IDF025.to_string(); + } + } + } + } + + return content; +} + // ***************************************************************************************************** // Some utilities @@ -5733,3 +6346,323 @@ int32_t Rtcm::set_DF420(const Gnss_Synchro& gnss_synchro __attribute__((unused)) DF420 = std::bitset<1>(half_cycle_ambiguity_indicator); return 0; } + + +// IGS State Space Representation (SSR) data fields +// see https://files.igs.org/pub/data/format/igs_ssr_v1.pdf + +void Rtcm::set_IDF001(uint8_t version) +{ + const uint8_t max_value = 7; + uint8_t igm_version = version; + if (igm_version > max_value) // not defined + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad IGM/IM Version"; + igm_version = 0; + } + IDF001 = std::bitset<3>(igm_version); +} + + +void Rtcm::set_IDF002(uint8_t igs_message_number) +{ + IDF002 = std::bitset<8>(igs_message_number); +} + + +void Rtcm::set_IDF003(uint32_t tow) +{ + const uint32_t max_value = 604799; + uint32_t ssr_epoch_time = tow; + if (ssr_epoch_time > max_value) + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad SSR Epoch Time"; + ssr_epoch_time = 0; + } + IDF003 = std::bitset<20>(ssr_epoch_time); +} + + +void Rtcm::set_IDF004(uint8_t ssr_update_interval) +{ + const uint8_t max_value = 15; + uint8_t update_interval = ssr_update_interval; + if (update_interval > max_value) + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad SSR Update Interval"; + update_interval = 0; + } + IDF004 = std::bitset<4>(update_interval); +} + + +void Rtcm::set_IDF005(bool ssr_multiple_message_indicator) +{ + IDF005 = std::bitset<1>(ssr_multiple_message_indicator); +} + + +void Rtcm::set_IDF006(bool regional_indicator) +{ + IDF006 = std::bitset<1>(regional_indicator); +} + + +void Rtcm::set_IDF007(uint8_t ssr_iod) +{ + const uint8_t max_value = 15; + uint8_t iod = ssr_iod; + if (iod > max_value) + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad IOD SSR"; + iod = 0; + } + IDF007 = std::bitset<4>(iod); +} + + +void Rtcm::set_IDF008(uint16_t ssr_provider_id) +{ + IDF008 = std::bitset<16>(ssr_provider_id); +} + + +void Rtcm::set_IDF009(uint8_t ssr_solution_id) +{ + const uint8_t max_value = 15; + uint8_t sol_id = ssr_solution_id; + if (sol_id > max_value) + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad SSR Solution ID"; + sol_id = 0; + } + IDF009 = std::bitset<4>(sol_id); +} + + +void Rtcm::set_IDF010(uint8_t num_satellites) +{ + const uint8_t max_value = 63; + uint8_t num_sats = num_satellites; + if (num_sats > max_value) + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad number of satellites"; + num_sats = 0; + } + IDF010 = std::bitset<6>(num_sats); +} + + +void Rtcm::set_IDF011(uint8_t gnss_satellite_id) +{ + const uint8_t max_value = 63; + uint8_t sat_id = gnss_satellite_id; + if (sat_id > max_value) + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad GNSS Satellite ID"; + sat_id = 0; + } + IDF011 = std::bitset<6>(sat_id); +} + + +void Rtcm::set_IDF012(uint8_t gnss_iod) +{ + IDF012 = std::bitset<8>(gnss_iod); +} + + +void Rtcm::set_IDF013(float delta_orbit_radial_m) +{ + const float scale = 1.0e4; + const int32_t max_value = 2097151; + auto delta_orbit_radial = static_cast((delta_orbit_radial_m * scale)); + if (delta_orbit_radial > max_value) + { + delta_orbit_radial = max_value; + } + if (delta_orbit_radial < -max_value) + { + delta_orbit_radial = -max_value; + } + IDF013 = std::bitset<22>(delta_orbit_radial); +} + + +void Rtcm::set_IDF014(float delta_orbit_in_track_m) +{ + const float scale = 2500.0; + const int32_t max_value = 524287; + auto delta_orbit_in_track = static_cast((delta_orbit_in_track_m * scale)); + if (delta_orbit_in_track > max_value) + { + delta_orbit_in_track = max_value; + } + if (delta_orbit_in_track < -max_value) + { + delta_orbit_in_track = -max_value; + } + IDF014 = std::bitset<20>(delta_orbit_in_track); +} + + +void Rtcm::set_IDF015(float delta_orbit_cross_track_m) +{ + const float scale = 2500.0; + const int32_t max_value = 524287; + auto delta_orbit_cross_track = static_cast((delta_orbit_cross_track_m * scale)); + if (delta_orbit_cross_track > max_value) + { + delta_orbit_cross_track = max_value; + } + if (delta_orbit_cross_track < -max_value) + { + delta_orbit_cross_track = -max_value; + } + IDF015 = std::bitset<20>(delta_orbit_cross_track); +} + + +void Rtcm::set_IDF016(float dot_orbit_delta_track_m_s) +{ + const float scale = 1.0e6; + const int32_t max_value = 1048575; + auto dot_orbit_delta_track = static_cast((dot_orbit_delta_track_m_s * scale)); + if (dot_orbit_delta_track > max_value) + { + dot_orbit_delta_track = max_value; + } + if (dot_orbit_delta_track < -max_value) + { + dot_orbit_delta_track = -max_value; + } + IDF016 = std::bitset<21>(dot_orbit_delta_track); +} + + +void Rtcm::set_IDF017(float dot_orbit_delta_in_track_m_s) +{ + const float scale = 250000.0; + const int32_t max_value = 262143; + auto dot_orbit_delta_in_track = static_cast((dot_orbit_delta_in_track_m_s * scale)); + if (dot_orbit_delta_in_track > max_value) + { + dot_orbit_delta_in_track = max_value; + } + if (dot_orbit_delta_in_track < -max_value) + { + dot_orbit_delta_in_track = -max_value; + } + IDF017 = std::bitset<19>(dot_orbit_delta_in_track); +} + + +void Rtcm::set_IDF018(float dot_orbit_delta_cross_track_m_s) +{ + const float scale = 250000.0; + const int32_t max_value = 262143; + auto dot_orbit_delta_cross_track = static_cast((dot_orbit_delta_cross_track_m_s * scale)); + if (dot_orbit_delta_cross_track > max_value) + { + dot_orbit_delta_cross_track = max_value; + } + if (dot_orbit_delta_cross_track < -max_value) + { + dot_orbit_delta_cross_track = -max_value; + } + IDF018 = std::bitset<19>(dot_orbit_delta_cross_track); +} + + +void Rtcm::set_IDF019(float delta_clock_c0_m) +{ + const float scale = 1.0e4; + const int32_t max_value = 2097151; + auto delta_clock_c0 = static_cast((delta_clock_c0_m * scale)); + if (delta_clock_c0 > max_value) + { + delta_clock_c0 = max_value; + } + if (delta_clock_c0 < -max_value) + { + delta_clock_c0 = -max_value; + } + IDF019 = std::bitset<22>(delta_clock_c0); +} + + +void Rtcm::set_IDF020(float delta_clock_c1_m_s) +{ + const float scale = 1.0e6; + const int32_t max_value = 1048575; + auto delta_clock_c1 = static_cast((delta_clock_c1_m_s * scale)); + if (delta_clock_c1 > max_value) + { + delta_clock_c1 = max_value; + } + if (delta_clock_c1 < -max_value) + { + delta_clock_c1 = -max_value; + } + IDF020 = std::bitset<21>(delta_clock_c1); +} + + +void Rtcm::set_IDF021(float delta_clock_c2_m_s2) +{ + const float scale = 5.0e8; + const int32_t max_value = 67108863; + auto delta_clock_c2 = static_cast((delta_clock_c2_m_s2 * scale)); + if (delta_clock_c2 > max_value) + { + delta_clock_c2 = max_value; + } + if (delta_clock_c2 < -max_value) + { + delta_clock_c2 = -max_value; + } + IDF021 = std::bitset<27>(delta_clock_c2); +} + + +void Rtcm::set_IDF023(uint8_t num_bias_processed) +{ + const uint8_t max_value = 31; + uint8_t num_bias = num_bias_processed; + if (num_bias > max_value) + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad number of biases processed"; + num_bias = 0; + } + IDF023 = std::bitset<5>(num_bias); +} + + +void Rtcm::set_IDF024(uint8_t gnss_signal_tracking_mode_id) +{ + const uint8_t max_value = 31; + uint8_t tracking_mode = gnss_signal_tracking_mode_id; + if (tracking_mode > max_value) // error + { + LOG(WARNING) << "RTCM SSR messages are probably wrong: bad GNSS Signal and Tracking Mode Identifier"; + tracking_mode = 0; + } + IDF024 = std::bitset<5>(tracking_mode); +} + + +void Rtcm::set_IDF025(float code_bias_m) +{ + const float scale = 100.0; + const int16_t max_value = 8191; + auto code_bias = static_cast((code_bias_m * scale)); + if (code_bias > max_value) + { + code_bias = max_value; + } + if (code_bias < -max_value) + { + code_bias = -max_value; + } + IDF025 = std::bitset<14>(code_bias); +} diff --git a/src/algorithms/PVT/libs/rtcm.h b/src/algorithms/PVT/libs/rtcm.h index d85e40fbc..91b7ac275 100644 --- a/src/algorithms/PVT/libs/rtcm.h +++ b/src/algorithms/PVT/libs/rtcm.h @@ -21,6 +21,7 @@ #include "concurrent_queue.h" #include "galileo_ephemeris.h" +#include "galileo_has_data.h" #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_utc_model.h" #include "gnss_synchro.h" @@ -336,6 +337,26 @@ public: bool divergence_free, bool more_messages); + /*! + * \brief Prints messages of type IGM01 (SSR Orbit Correction) + */ + std::vector print_IGM01(const Galileo_HAS_data& has_data); + + /*! + * \brief Prints messages of type IGM02 (SSR Clock Correction) + */ + std::vector print_IGM02(const Galileo_HAS_data& has_data); + + /*! + * \brief Prints messages of type IGM03 (SSR Combined Orbit and Clock Correction) + */ + std::vector print_IGM03(const Galileo_HAS_data& has_data); + + /*! + * \brief Prints messages of type IGM05 (SSR Bias Correction) + */ + std::vector print_IGM05(const Galileo_HAS_data& has_data); + uint32_t lock_time(const Gps_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro); //!< Returns the time period in which GPS L1 signals have been continually tracked. uint32_t lock_time(const Gps_CNAV_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro); //!< Returns the time period in which GPS L2 signals have been continually tracked. uint32_t lock_time(const Galileo_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro); //!< Returns the time period in which Galileo signals have been continually tracked. @@ -476,6 +497,15 @@ private: std::string get_MSM_6_content_signal_data(const Gps_Ephemeris& ephNAV, const Gps_CNAV_Ephemeris& ephCNAV, const Galileo_Ephemeris& ephFNAV, const Glonass_Gnav_Ephemeris& ephGNAV, double obs_time, const std::map& observables); std::string get_MSM_7_content_signal_data(const Gps_Ephemeris& ephNAV, const Gps_CNAV_Ephemeris& ephCNAV, const Galileo_Ephemeris& ephFNAV, const Glonass_Gnav_Ephemeris& ephGNAV, double obs_time, const std::map& observables); + std::string get_IGM01_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator); + std::string get_IGM01_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index); + std::string get_IGM02_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator); + std::string get_IGM02_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index); + std::string get_IGM03_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator); + std::string get_IGM03_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index); + std::string get_IGM05_header(const Galileo_HAS_data& has_data, uint8_t nsys, bool ssr_multiple_msg_indicator); + std::string get_IGM05_content_sat(const Galileo_HAS_data& has_data, uint8_t nsys_index); + // // Utilities // @@ -496,6 +526,8 @@ private: uint32_t lock_time_indicator(uint32_t lock_time_period_s); uint32_t msm_lock_time_indicator(uint32_t lock_time_period_s); uint32_t msm_extended_lock_time_indicator(uint32_t lock_time_period_s); + // SSR utilities + uint8_t ssr_update_interval(uint16_t validity_seconds) const; // // Classes for TCP communication @@ -1467,6 +1499,80 @@ private: std::bitset<1> DF420; int32_t set_DF420(const Gnss_Synchro& gnss_synchro); + + // IGS State Space Representation (SSR) data fields + // see https://files.igs.org/pub/data/format/igs_ssr_v1.pdf + std::bitset<3> IDF001; + void set_IDF001(uint8_t version); + + std::bitset<8> IDF002; + void set_IDF002(uint8_t igs_message_number); + + std::bitset<20> IDF003; + void set_IDF003(uint32_t tow); + + std::bitset<4> IDF004; + void set_IDF004(uint8_t ssr_update_interval); + + std::bitset<1> IDF005; + void set_IDF005(bool ssr_multiple_message_indicator); + + std::bitset<1> IDF006; + void set_IDF006(bool regional_indicator); + + std::bitset<4> IDF007; + void set_IDF007(uint8_t ssr_iod); + + std::bitset<16> IDF008; + void set_IDF008(uint16_t ssr_provider_id); + + std::bitset<4> IDF009; + void set_IDF009(uint8_t ssr_solution_id); + + std::bitset<6> IDF010; + void set_IDF010(uint8_t num_satellites); + + std::bitset<6> IDF011; + void set_IDF011(uint8_t gnss_satellite_id); + + std::bitset<8> IDF012; + void set_IDF012(uint8_t gnss_iod); + + std::bitset<22> IDF013; + void set_IDF013(float delta_orbit_radial_m); + + std::bitset<20> IDF014; + void set_IDF014(float delta_orbit_in_track_m); + + std::bitset<20> IDF015; + void set_IDF015(float delta_orbit_cross_track_m); + + std::bitset<21> IDF016; + void set_IDF016(float dot_orbit_delta_track_m_s); + + std::bitset<19> IDF017; + void set_IDF017(float dot_orbit_delta_in_track_m_s); + + std::bitset<19> IDF018; + void set_IDF018(float dot_orbit_delta_cross_track_m_s); + + std::bitset<22> IDF019; + void set_IDF019(float delta_clock_c0_m); + + std::bitset<21> IDF020; + void set_IDF020(float delta_clock_c1_m_s); + + std::bitset<27> IDF021; + void set_IDF021(float delta_clock_c2_m_s2); + + std::bitset<5> IDF023; + void set_IDF023(uint8_t num_bias_processed); + + std::bitset<5> IDF024; + void set_IDF024(uint8_t gnss_signal_tracking_mode_id); + + std::bitset<14> IDF025; + void set_IDF025(float code_bias_m); }; diff --git a/src/algorithms/PVT/libs/rtcm_printer.cc b/src/algorithms/PVT/libs/rtcm_printer.cc index 1b84bc7fc..a562c14a9 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.cc +++ b/src/algorithms/PVT/libs/rtcm_printer.cc @@ -19,6 +19,7 @@ #include "rtcm_printer.h" #include "galileo_ephemeris.h" +#include "galileo_has_data.h" #include "glonass_gnav_ephemeris.h" #include "glonass_gnav_utc_model.h" #include "gnss_sdr_filesystem.h" @@ -36,6 +37,7 @@ #include // for cout, cerr #include // for tcgetattr #include // for close, write +#include // for std::vector Rtcm_Printer::Rtcm_Printer(const std::string& filename, @@ -1460,6 +1462,40 @@ void Rtcm_Printer::Print_Rtcm_Messages(const Rtklib_Solver* pvt_solver, } +void Rtcm_Printer::Print_IGM_Messages(const Galileo_HAS_data& has_data) +{ + try + { + if (has_data.header.orbit_correction_flag && has_data.header.clock_fullset_flag) + { + Print_IGM03(has_data); + } + if (has_data.header.orbit_correction_flag && !has_data.header.clock_fullset_flag) + { + Print_IGM01(has_data); + } + if (!has_data.header.orbit_correction_flag && has_data.header.clock_fullset_flag) + { + Print_IGM02(has_data); + } + if (has_data.header.code_bias_flag) + { + Print_IGM05(has_data); + } + } + catch (const boost::exception& ex) + { + std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << '\n'; + LOG(ERROR) << "RTCM boost exception: " << boost::diagnostic_information(ex); + } + catch (const std::exception& ex) + { + std::cout << "RTCM std exception: " << ex.what() << '\n'; + LOG(ERROR) << "RTCM std exception: " << ex.what(); + } +} + + bool Rtcm_Printer::Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map& observables) { const std::string m1001 = rtcm->print_MT1001(gps_eph, obs_time, observables, station_id); @@ -1599,6 +1635,66 @@ bool Rtcm_Printer::Print_Rtcm_MSM(uint32_t msm_number, const Gps_Ephemeris& gps_ } +bool Rtcm_Printer::Print_IGM01(const Galileo_HAS_data& has_data) +{ + const std::vector msgs = rtcm->print_IGM01(has_data); + if (msgs.empty()) + { + return false; + } + for (const auto& s : msgs) + { + Rtcm_Printer::Print_Message(s); + } + return true; +} + + +bool Rtcm_Printer::Print_IGM02(const Galileo_HAS_data& has_data) +{ + const std::vector msgs = rtcm->print_IGM02(has_data); + if (msgs.empty()) + { + return false; + } + for (const auto& s : msgs) + { + Rtcm_Printer::Print_Message(s); + } + return true; +} + + +bool Rtcm_Printer::Print_IGM03(const Galileo_HAS_data& has_data) +{ + const std::vector msgs = rtcm->print_IGM03(has_data); + if (msgs.empty()) + { + return false; + } + for (const auto& s : msgs) + { + Rtcm_Printer::Print_Message(s); + } + return true; +} + + +bool Rtcm_Printer::Print_IGM05(const Galileo_HAS_data& has_data) +{ + const std::vector msgs = rtcm->print_IGM05(has_data); + if (msgs.empty()) + { + return false; + } + for (const auto& s : msgs) + { + Rtcm_Printer::Print_Message(s); + } + return true; +} + + int Rtcm_Printer::init_serial(const std::string& serial_device) { /* diff --git a/src/algorithms/PVT/libs/rtcm_printer.h b/src/algorithms/PVT/libs/rtcm_printer.h index f710fca98..66a5959b4 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.h +++ b/src/algorithms/PVT/libs/rtcm_printer.h @@ -40,6 +40,7 @@ class Gps_CNAV_Ephemeris; class Gps_Ephemeris; class Rtcm; class Rtklib_Solver; +class Galileo_HAS_data; /*! * \brief This class provides a implementation of a subset of the RTCM Standard 10403.2 messages @@ -98,6 +99,8 @@ public: */ uint32_t lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro); + void Print_IGM_Messages(const Galileo_HAS_data& has_data); + std::string print_MT1005_test(); //!< For testing purposes private: @@ -178,6 +181,11 @@ private: bool divergence_free, bool more_messages); + bool Print_IGM01(const Galileo_HAS_data& has_data); // SSR Orbit Corrections + bool Print_IGM02(const Galileo_HAS_data& has_data); // SSR Clock Corrections + bool Print_IGM03(const Galileo_HAS_data& has_data); // SSR Combined Orbit & Clock Corrections + bool Print_IGM05(const Galileo_HAS_data& has_data); // SSR Bias Corrections + int32_t init_serial(const std::string& serial_device); // serial port control void close_serial() const; bool Print_Message(const std::string& message); diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index feac903cd..28376dfd8 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -128,6 +128,10 @@ void Channel::connect(gr::top_block_sptr top_block) // Message ports top_block->msg_connect(nav_->get_left_block(), pmt::mp("telemetry_to_trk"), trk_->get_right_block(), pmt::mp("telemetry_to_trk")); + if ((trk_->get_right_block()->name() == "glonass_l1_ca_dll_pll_c_aid_tracking_cc") || (trk_->get_right_block()->name() == "glonass_l2_ca_dll_pll_c_aid_tracking_cc")) + { + top_block->msg_connect(nav_->get_left_block(), pmt::mp("preamble_timestamp_samples"), trk_->get_right_block(), pmt::mp("preamble_timestamp_samples")); + } DLOG(INFO) << "tracking -> telemetry_decoder"; // Message ports @@ -158,6 +162,10 @@ void Channel::disconnect(gr::top_block_sptr top_block) nav_->disconnect(top_block); top_block->msg_disconnect(nav_->get_left_block(), pmt::mp("telemetry_to_trk"), trk_->get_right_block(), pmt::mp("telemetry_to_trk")); + if ((trk_->get_right_block()->name() == "glonass_l1_ca_dll_pll_c_aid_tracking_cc") || (trk_->get_right_block()->name() == "glonass_l2_ca_dll_pll_c_aid_tracking_cc")) + { + top_block->msg_disconnect(nav_->get_left_block(), pmt::mp("preamble_timestamp_samples"), trk_->get_right_block(), pmt::mp("preamble_timestamp_samples")); + } if (!flag_enable_fpga_) { top_block->msg_disconnect(acq_->get_right_block(), pmt::mp("events"), channel_msg_rx_, pmt::mp("events")); diff --git a/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc b/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc index e8532d624..0d952d6e7 100644 --- a/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc +++ b/src/algorithms/signal_source/adapters/osmosdr_signal_source.cc @@ -101,8 +101,20 @@ OsmosdrSignalSource::OsmosdrSignalSource(const ConfigurationInterface* configura } else { - std::cout << "Actual RX Gain: " << osmosdr_source_->get_gain() << " dB...\n"; - LOG(INFO) << "Actual RX Gain: " << osmosdr_source_->get_gain() << " dB..."; + if (!osmosdr_args_.empty() && (osmosdr_args_.find("xtrx") != std::string::npos)) + { + osmosdr_source_->set_gain(gain_, "LNA", 0); + osmosdr_source_->set_gain(rf_gain_, "TIA", 0); + osmosdr_source_->set_gain(if_gain_, "PGA", 0); + std::cout << "Actual XTRX LNA Gain: " << osmosdr_source_->get_gain("LNA", 0) << " dB...\n"; + std::cout << "Actual XTRX TIA Gain: " << osmosdr_source_->get_gain("TIA", 0) << " dB...\n"; + std::cout << "Actual XTRX PGA Gain: " << osmosdr_source_->get_gain("PGA", 0) << " dB...\n"; + } + else + { + std::cout << "Actual RX Gain: " << osmosdr_source_->get_gain() << " dB...\n"; + LOG(INFO) << "Actual RX Gain: " << osmosdr_source_->get_gain() << " dB..."; + } } } diff --git a/src/algorithms/signal_source/adapters/uhd_signal_source.cc b/src/algorithms/signal_source/adapters/uhd_signal_source.cc index cbb151480..9b222bdb5 100644 --- a/src/algorithms/signal_source/adapters/uhd_signal_source.cc +++ b/src/algorithms/signal_source/adapters/uhd_signal_source.cc @@ -60,6 +60,7 @@ UhdSignalSource::UhdSignalSource(const ConfigurationInterface* configuration, } subdevice_ = configuration->property(role + ".subdevice", empty); clock_source_ = configuration->property(role + ".clock_source", std::string("internal")); + otw_format_ = configuration->property(role + ".otw_format", std::string("sc16")); RF_channels_ = configuration->property(role + ".RF_channels", 1); sample_rate_ = configuration->property(role + ".sampling_frequency", 4.0e6); item_type_ = configuration->property(role + ".item_type", default_item_type); @@ -128,26 +129,49 @@ UhdSignalSource::UhdSignalSource(const ConfigurationInterface* configuration, // fc32: Complex floating point (32-bit floats) range [-1.0, +1.0]. // sc16: Complex signed integer (16-bit integers) range [-32768, +32767]. // sc8: Complex signed integer (8-bit integers) range [-128, 127]. + //! Convenience constructor for streamer args + // stream_args_t(const std::string& cpu = "", const std::string& otw = "") + // { + // cpu_format = cpu; + // otw_format = otw; + // } + // + // + // * The CPU format is a string that describes the format of host memory. + // * Conversions for the following CPU formats have been implemented: + // * - fc64 - complex + // * - fc32 - complex + // * - sc16 - complex + // * - sc8 - complex + // * + // * The following are not implemented, but are listed to demonstrate naming convention: + // * - f32 - float + // * - f64 - double + // * - s16 - int16_t + // * - s8 - int8_t + // * + // * The CPU format can be chosen depending on what the application requires. + if (item_type_ == "cbyte") { item_size_ = sizeof(lv_8sc_t); - uhd_stream_args_ = uhd::stream_args_t("sc8"); + uhd_stream_args_ = uhd::stream_args_t("sc8", otw_format_); } else if (item_type_ == "cshort") { item_size_ = sizeof(lv_16sc_t); - uhd_stream_args_ = uhd::stream_args_t("sc16"); + uhd_stream_args_ = uhd::stream_args_t("sc16", otw_format_); } else if (item_type_ == "gr_complex") { item_size_ = sizeof(gr_complex); - uhd_stream_args_ = uhd::stream_args_t("fc32"); + uhd_stream_args_ = uhd::stream_args_t("fc32", otw_format_); } else { LOG(WARNING) << item_type_ << " unrecognized item type. Using cshort."; item_size_ = sizeof(lv_16sc_t); - uhd_stream_args_ = uhd::stream_args_t("sc16"); + uhd_stream_args_ = uhd::stream_args_t("sc16", otw_format_); } // select the number of channels and the subdevice specifications diff --git a/src/algorithms/signal_source/adapters/uhd_signal_source.h b/src/algorithms/signal_source/adapters/uhd_signal_source.h index 956e7fa2c..5a513b4c9 100644 --- a/src/algorithms/signal_source/adapters/uhd_signal_source.h +++ b/src/algorithms/signal_source/adapters/uhd_signal_source.h @@ -77,6 +77,13 @@ private: std::string subdevice_; std::string clock_source_; + // * The OTW format is a string that describes the format over-the-wire. + // * The following over-the-wire formats have been implemented: + // * - sc16 - Q16 I16 + // * - sc8 - Q8_1 I8_1 Q8_0 I8_0 + // * - sc12 (Only some devices) + std::string otw_format_; + double sample_rate_; size_t item_size_; int RF_channels_; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 8c259691f..db723cdf9 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -516,7 +516,7 @@ void galileo_telemetry_decoder_gs::decode_FNAV_word(float *page_symbols, int32_t } -void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t page_length) +void galileo_telemetry_decoder_gs::decode_CNAV_word(uint64_t time_stamp, float *page_symbols, int32_t page_length) { // 1. De-interleave std::vector page_symbols_soft_value(page_length); @@ -550,7 +550,7 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t } } d_cnav_nav.read_HAS_page(page_String); - + d_cnav_nav.set_time_stamp(time_stamp); // 4. If we have a new HAS page, read it if (d_cnav_nav.have_new_HAS_page() == true) { @@ -834,7 +834,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( decode_FNAV_word(d_page_part_symbols.data(), d_frame_length_symbols); break; case 3: // CNAV - decode_CNAV_word(d_page_part_symbols.data(), d_frame_length_symbols); + decode_CNAV_word(current_symbol.Tracking_sample_counter / static_cast(current_symbol.fs), d_page_part_symbols.data(), d_frame_length_symbols); break; default: return -1; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index 59f36b61a..59e5fcb01 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -83,7 +83,7 @@ private: void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out); void decode_INAV_word(float *page_part_symbols, int32_t frame_length); void decode_FNAV_word(float *page_symbols, int32_t frame_length); - void decode_CNAV_word(float *page_symbols, int32_t page_length); + void decode_CNAV_word(uint64_t time_stamp, float *page_symbols, int32_t page_length); std::unique_ptr d_viterbi; std::vector d_preamble_samples; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index 0ebb91bb4..ef9964a4a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -69,6 +69,7 @@ glonass_l1_ca_telemetry_decoder_gs::glonass_l1_ca_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); + this->message_port_register_out(pmt::mp("preamble_timestamp_samples")); if (d_enable_navdata_monitor) { @@ -367,6 +368,7 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu // try to decode frame LOG(INFO) << "Starting string decoder for GLONASS L1 C/A SAT " << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp + this->message_port_pub(pmt::mp("preamble_timestamp_samples"), pmt::mp(d_preamble_time_samples)); d_stat = 2; } else diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index aa1707e93..b0a6d8812 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -69,6 +69,7 @@ glonass_l2_ca_telemetry_decoder_gs::glonass_l2_ca_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); + this->message_port_register_out(pmt::mp("preamble_timestamp_samples")); if (d_enable_navdata_monitor) { @@ -366,6 +367,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu // try to decode frame LOG(INFO) << "Starting string decoder for GLONASS L2 C/A SAT " << this->d_satellite; d_preamble_index = d_sample_counter; // record the preamble sample stamp + this->message_port_pub(pmt::mp("preamble_timestamp_samples"), pmt::mp(d_preamble_time_samples)); d_stat = 2; } else diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc index ba6a7c0e9..05b8eb578 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -87,7 +87,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index(const p DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); if (d_enable_extended_integration == false) // avoid re-setting preamble indicator { - d_preamble_timestamp_s = pmt::to_double(msg); + d_preamble_timestamp_samples = pmt::to_double(msg); d_enable_extended_integration = true; d_preamble_synchronized = false; } @@ -138,7 +138,7 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc d_code_error_filt_chips_s(0.0), d_code_error_filt_chips_Ti(0.0), d_carr_phase_error_secs_Ti(0.0), - d_preamble_timestamp_s(0.0), + d_preamble_timestamp_samples(0.0), d_extend_correlation_ms(extend_correlation_ms), d_correlation_length_samples(static_cast(d_vector_length)), d_sample_counter(0ULL), @@ -156,9 +156,9 @@ glonass_l1_ca_dll_pll_c_aid_tracking_cc::glonass_l1_ca_dll_pll_c_aid_tracking_cc d_dump(dump) { // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_in(pmt::mp("preamble_timestamp_samples")); - this->set_msg_handler(pmt::mp("preamble_timestamp_s"), + this->set_msg_handler(pmt::mp("preamble_timestamp_samples"), #if HAS_GENERIC_LAMBDA [this](auto &&PH1) { msg_handler_preamble_index(PH1); }); #else @@ -602,7 +602,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at bool enable_dll_pll; if (d_enable_extended_integration == true) { - int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); + int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples - d_preamble_timestamp_samples) / static_cast(d_fs_in))); if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) { // compute coherent integration and enable tracking loop diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h index a11fb2a3f..97d05ee93 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.h @@ -171,7 +171,7 @@ private: double d_code_error_filt_chips_Ti; double d_carr_phase_error_secs_Ti; - double d_preamble_timestamp_s; + double d_preamble_timestamp_samples; int32_t d_extend_correlation_ms; // Integration period in samples diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc index 44a4bdcbd..74bc090ff 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc @@ -84,7 +84,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::msg_handler_preamble_index(const p DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); if (d_enable_extended_integration == false) // avoid re-setting preamble indicator { - d_preamble_timestamp_s = pmt::to_double(msg); + d_preamble_timestamp_samples = pmt::to_double(msg); d_enable_extended_integration = true; d_preamble_synchronized = false; } @@ -135,7 +135,7 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc d_code_error_filt_chips_s(0.0), d_code_error_filt_chips_Ti(0.0), d_carr_phase_error_secs_Ti(0.0), - d_preamble_timestamp_s(0.0), + d_preamble_timestamp_samples(0.0), d_extend_correlation_ms(extend_correlation_ms), d_correlation_length_samples(static_cast(d_vector_length)), d_sample_counter(0ULL), @@ -153,9 +153,9 @@ glonass_l2_ca_dll_pll_c_aid_tracking_cc::glonass_l2_ca_dll_pll_c_aid_tracking_cc d_dump(dump) { // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_in(pmt::mp("preamble_timestamp_samples")); - this->set_msg_handler(pmt::mp("preamble_timestamp_s"), + this->set_msg_handler(pmt::mp("preamble_timestamp_samples"), #if HAS_GENERIC_LAMBDA [this](auto &&PH1) { msg_handler_preamble_index(PH1); }); #else @@ -600,7 +600,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at bool enable_dll_pll; if (d_enable_extended_integration == true) { - const int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in) - d_preamble_timestamp_s)); + const int64_t symbol_diff = round(1000.0 * ((static_cast(d_sample_counter) + d_rem_code_phase_samples - d_preamble_timestamp_samples) / static_cast(d_fs_in))); if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0) { // compute coherent integration and enable tracking loop diff --git a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h index bebb30333..dd92858e5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.h @@ -168,7 +168,7 @@ private: double d_code_error_filt_chips_s; double d_code_error_filt_chips_Ti; double d_carr_phase_error_secs_Ti; - double d_preamble_timestamp_s; + double d_preamble_timestamp_samples; int32_t d_extend_correlation_ms; // Integration period in samples diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 62407724a..53fc3d6bc 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -28,6 +28,7 @@ #include // for std::find, std::count #include // for size_t #include // for std::back_inserter +#include // for std::numeric_limits #include // for std::stringstream #include // for std::out_of_range #include // for typeid @@ -78,6 +79,10 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ d_C_matrix = std::vector>>(GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE))); // 32 x 255 x 53 d_M_matrix = std::vector>(GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)); // HAS message matrix 32 x 53 d_received_pids = std::vector>(HAS_MSG_NUMBER_MESSAGE_IDS, std::vector()); + d_received_timestamps = std::vector>(HAS_MSG_NUMBER_MESSAGE_IDS, std::vector()); + d_printed_timestamps = std::vector(HAS_MSG_NUMBER_MESSAGE_IDS, std::numeric_limits::max()); + d_printed_mids = std::vector(HAS_MSG_NUMBER_MESSAGE_IDS); + // Reserve memory to store masks d_nsat_in_mask_id = std::vector(HAS_MSG_NUMBER_MASK_IDS); @@ -103,10 +108,9 @@ void galileo_e6_has_msg_receiver::set_enable_navdata_monitor(bool enable) } -void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& msg) +std::shared_ptr galileo_e6_has_msg_receiver::process_test_page(const pmt::pmt_t& msg) { - gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_galileo_e6_has function called by the scheduler - + int64_t timestamp = std::numeric_limits::max(); try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); @@ -121,7 +125,60 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m << "PID: " << static_cast(HAS_data_page->message_page_id); d_current_has_status = HAS_data_page->has_status; d_current_message_id = HAS_data_page->message_id; - process_HAS_page(*HAS_data_page.get()); + timestamp = HAS_data_page->time_stamp; + if (d_printed_mids[d_current_message_id] == false) + { + process_HAS_page(*HAS_data_page.get()); + } + } + else + { + LOG(WARNING) << "galileo_e6_has_msg_receiver received an unknown object type!"; + } + } + catch (const wht::bad_any_cast& e) + { + LOG(WARNING) << "galileo_e6_has_msg_receiver Bad any_cast: " << e.what(); + } + + // Return the resulting decoded HAS data (if available) + if (d_new_message == true) + { + d_HAS_data.has_status = d_current_has_status; + d_HAS_data.message_id = d_current_message_id; + auto has_data_ptr = std::make_shared(d_HAS_data); + d_new_message = false; + d_printed_mids[d_current_message_id] = true; + d_printed_timestamps[d_current_message_id] = timestamp; + return has_data_ptr; + } + return nullptr; +} + + +void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& msg) +{ + gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_galileo_e6_has function called by the scheduler + int64_t timestamp = std::numeric_limits::max(); + try + { + const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); + if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) + { + const auto HAS_data_page = wht::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "New HAS page received: " + << "Status: " << static_cast(HAS_data_page->has_status) << ", " + << "MT: " << static_cast(HAS_data_page->message_type) << ", " + << "MID: " << static_cast(HAS_data_page->message_id) << ", " + << "MS: " << static_cast(HAS_data_page->message_size) << ", " + << "PID: " << static_cast(HAS_data_page->message_page_id); + d_current_has_status = HAS_data_page->has_status; + d_current_message_id = HAS_data_page->message_id; + timestamp = HAS_data_page->time_stamp; + if (d_printed_mids[d_current_message_id] == false) + { + process_HAS_page(*HAS_data_page.get()); + } } else { @@ -138,6 +195,8 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m { d_HAS_data.has_status = d_current_has_status; d_HAS_data.message_id = d_current_message_id; + d_printed_mids[d_current_message_id] = true; + d_printed_timestamps[d_current_message_id] = timestamp; auto has_data_ptr = std::make_shared(d_HAS_data); this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(has_data_ptr)); d_new_message = false; @@ -155,12 +214,14 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p { if (has_page.message_type == 1) // contains satellite corrections { + delete_outdated_data(has_page); if (has_page.message_id < HAS_MSG_NUMBER_MESSAGE_IDS) // MID range is from 0 to 31 { if (std::find(d_received_pids[has_page.message_id].begin(), d_received_pids[has_page.message_id].end(), has_page.message_page_id) == d_received_pids[has_page.message_id].end()) { // New pid! Annotate it. d_received_pids[has_page.message_id].push_back(has_page.message_page_id); + d_received_timestamps[has_page.message_id].push_back(has_page.time_stamp); for (int k = 0; k < GALILEO_CNAV_OCTETS_IN_SUBPAGE; k++) { constexpr int bits_in_octet = 8; @@ -184,18 +245,51 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p if (res == 0) { // Successful decoding, we have a valid HAS message stored at d_HAS_data - std::cout << TEXT_MAGENTA << "New Galileo HAS message type " << std::to_string(has_page.message_id) - << " received and successfully decoded" << TEXT_RESET << '\n'; if (d_nsat_in_mask_id[d_HAS_data.header.mask_id] != 0) { // if we have the mask for that message, it's ready to be sent to PVT d_new_message = true; + std::cout << TEXT_MAGENTA << "New Galileo HAS message ID " << std::to_string(has_page.message_id) + << " received and successfully decoded" << TEXT_RESET << '\n'; } } } } +void galileo_e6_has_msg_receiver::delete_outdated_data(const Galileo_HAS_page& has_page) +{ + const uint64_t current_time_stamp = has_page.time_stamp; + for (size_t i = 0; i < d_received_pids.size(); i++) + { + uint64_t oldest_time_stamp = std::numeric_limits::max(); + for (size_t j = 0; j < d_received_pids[i].size(); j++) + { + uint64_t timestamp = d_received_timestamps[i][j]; + if (timestamp > 0 && timestamp < oldest_time_stamp) + { + oldest_time_stamp = timestamp; + } + } + if (current_time_stamp > oldest_time_stamp && current_time_stamp - oldest_time_stamp > MAX_SECONDS_REMEMBERING_MID) + { + DLOG(INFO) << "Deleting data for message ID " << i << " because it is too old: " << oldest_time_stamp << " vs " << current_time_stamp; + d_received_pids[i].clear(); + d_received_timestamps[i].clear(); + d_C_matrix[i] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; + } + } + for (size_t mid = 0; mid < HAS_MSG_NUMBER_MESSAGE_IDS; mid++) + { + if (d_printed_mids[mid] == true && current_time_stamp > d_printed_timestamps[mid] && current_time_stamp - d_printed_timestamps[mid] > MAX_SECONDS_REMEMBERING_MID) + { + d_printed_timestamps[mid] = std::numeric_limits::max(); + d_printed_mids[mid] = false; + } + } +} + + int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_t message_size) { DLOG(INFO) << "Start decoding of a HAS message"; @@ -338,36 +432,36 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ void galileo_e6_has_msg_receiver::read_MT1_header(const std::string& message_header) { - // ICD v1.2 Table 6: MT1 Message Header + // HAS SIS ICD v1.0 Table 13: MT1 Message Header const std::bitset has_mt1_header(message_header); d_HAS_data.header.toh = read_has_message_header_parameter_uint16(has_mt1_header, GALILEO_MT1_HEADER_TOH); - d_HAS_data.header.mask_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_MASK_ID); - d_HAS_data.header.iod_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_IOD_ID); d_HAS_data.header.mask_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_MASK_FLAG); d_HAS_data.header.orbit_correction_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG); d_HAS_data.header.clock_fullset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG); d_HAS_data.header.clock_subset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG); d_HAS_data.header.code_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CODE_BIAS_FLAG); d_HAS_data.header.phase_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_PHASE_BIAS_FLAG); - d_HAS_data.header.ura_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_URA_FLAG); + d_HAS_data.header.reserved = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_RESERVED); + d_HAS_data.header.mask_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_MASK_ID); + d_HAS_data.header.iod_set_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_IOD_SET_ID); DLOG(INFO) << "MT1 header " << message_header << ": " << "TOH: " << static_cast(d_HAS_data.header.toh) << ", " - << "mask ID: " << static_cast(d_HAS_data.header.mask_id) << ", " - << "iod ID: " << static_cast(d_HAS_data.header.iod_id) << ", " << "mask_flag: " << static_cast(d_HAS_data.header.mask_flag) << ", " << "orbit_correction_flag: " << static_cast(d_HAS_data.header.orbit_correction_flag) << ", " << "clock_fullset_flag: " << static_cast(d_HAS_data.header.clock_fullset_flag) << ", " << "clock_subset_flag: " << static_cast(d_HAS_data.header.clock_subset_flag) << ", " << "code_bias_flag: " << static_cast(d_HAS_data.header.code_bias_flag) << ", " << "phase_bias_flag: " << static_cast(d_HAS_data.header.phase_bias_flag) << ", " - << "ura_flag: " << static_cast(d_HAS_data.header.ura_flag); + << "reserved: " << static_cast(d_HAS_data.header.reserved) << ", " + << "mask ID: " << static_cast(d_HAS_data.header.mask_id) << ", " + << "iod ID: " << static_cast(d_HAS_data.header.iod_set_id); } void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) { - // ICD v1.2 Table 7: MT1 Message Body. + // HAS SIS ICD v1.0 Table 17: MT1 Message Body auto message = std::string(message_body); int Nsat = 0; bool have_mask = false; @@ -489,7 +583,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); d_HAS_data.gnss_iod = std::vector(Nsat); d_HAS_data.delta_radial = std::vector(Nsat); - d_HAS_data.delta_along_track = std::vector(Nsat); + d_HAS_data.delta_in_track = std::vector(Nsat); d_HAS_data.delta_cross_track = std::vector(Nsat); for (int i = 0; i < Nsat; i++) { @@ -520,8 +614,8 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) d_HAS_data.delta_radial[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_RADIAL_LENGTH)); message = std::string(message.begin() + HAS_MSG_DELTA_RADIAL_LENGTH, message.end()); - d_HAS_data.delta_along_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_ALONG_TRACK_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_ALONG_TRACK_LENGTH, message.end()); + d_HAS_data.delta_in_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_IN_TRACK_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_IN_TRACK_LENGTH, message.end()); d_HAS_data.delta_cross_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CROSS_TRACK_LENGTH)); message = std::string(message.begin() + HAS_MSG_DELTA_CROSS_TRACK_LENGTH, message.end()); @@ -530,7 +624,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) DLOG(INFO) << debug_print_vector("gnss_iod", d_HAS_data.gnss_iod); DLOG(INFO) << debug_print_vector("delta_radial", d_HAS_data.delta_radial); - DLOG(INFO) << debug_print_vector("delta_along_track", d_HAS_data.delta_along_track); + DLOG(INFO) << debug_print_vector("delta_in_track", d_HAS_data.delta_in_track); DLOG(INFO) << debug_print_vector("delta_cross_track", d_HAS_data.delta_cross_track); } @@ -540,25 +634,22 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) d_HAS_data.validity_interval_index_clock_fullset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - d_HAS_data.delta_clock_c0_multiplier = std::vector(d_HAS_data.Nsys); + d_HAS_data.delta_clock_multiplier = std::vector(d_HAS_data.Nsys); for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) { - d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH)) + 1; // b00 means x1, b01 means x2, etc - message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end()); + d_HAS_data.delta_clock_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_LENGTH)) + 1; // b00 means x1, b01 means x2, etc + message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_LENGTH, message.end()); } - d_HAS_data.iod_change_flag = std::vector(Nsat); - d_HAS_data.delta_clock_c0 = std::vector(Nsat); + d_HAS_data.delta_clock_correction = std::vector(Nsat); for (int i = 0; i < Nsat; i++) { - d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false); - message = std::string(message.begin() + 1, message.end()); - d_HAS_data.delta_clock_c0[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_LENGTH, message.end()); + d_HAS_data.delta_clock_correction[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_CORRECTION_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_CORRECTION_LENGTH, message.end()); } - DLOG(INFO) << debug_print_vector("delta_clock_c0_multiplier", d_HAS_data.delta_clock_c0_multiplier); - DLOG(INFO) << debug_print_vector("delta_clock_c0", d_HAS_data.delta_clock_c0); + DLOG(INFO) << debug_print_vector("delta_clock_multiplier", d_HAS_data.delta_clock_multiplier); + DLOG(INFO) << debug_print_vector("delta_clock_correction", d_HAS_data.delta_clock_correction); } if (d_HAS_data.header.clock_subset_flag && have_mask) @@ -567,29 +658,28 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) d_HAS_data.validity_interval_index_clock_subset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH)); - message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end()); + d_HAS_data.Nsys_sub = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_SUB_LENGTH)); + message = std::string(message.begin() + HAS_MSG_NSYS_SUB_LENGTH, message.end()); - if (d_HAS_data.Nsysprime == 0) + if (d_HAS_data.Nsys_sub == 0) { // wrong data format, aborting have_mask = false; d_nsat_in_mask_id[d_HAS_data.header.mask_id] = 0; } - d_HAS_data.gnss_id_clock_subset = std::vector(d_HAS_data.Nsysprime); - d_HAS_data.delta_clock_c0_multiplier_clock_subset = std::vector(d_HAS_data.Nsysprime); - d_HAS_data.satellite_submask = std::vector(d_HAS_data.Nsysprime); - d_HAS_data.iod_change_flag_clock_subset = std::vector>(d_HAS_data.Nsysprime, std::vector()); - d_HAS_data.delta_clock_c0_clock_subset = std::vector>(d_HAS_data.Nsysprime, std::vector()); + d_HAS_data.gnss_id_clock_subset = std::vector(d_HAS_data.Nsys_sub); + d_HAS_data.delta_clock_multiplier_clock_subset = std::vector(d_HAS_data.Nsys_sub); + d_HAS_data.satellite_submask = std::vector(d_HAS_data.Nsys_sub); + d_HAS_data.delta_clock_correction_clock_subset = std::vector>(d_HAS_data.Nsys_sub, std::vector()); - for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++) + for (uint8_t i = 0; i < d_HAS_data.Nsys_sub; i++) { d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH)); message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end()); uint8_t clock_multiplier = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH)); - d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = clock_multiplier + 1; // b00 means x1, b01 means x2, etc + d_HAS_data.delta_clock_multiplier_clock_subset[i] = clock_multiplier + 1; // b00 means x1, b01 means x2, etc message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end()); // find the satellite mask corresponding to this GNSS ID @@ -621,22 +711,22 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) } aux <<= 1; } - int Nsatprime = std::count(binary.begin(), binary.end(), '1'); - d_HAS_data.delta_clock_c0_clock_subset[i].reserve(Nsatprime); + int Nsat_sub = std::count(binary.begin(), binary.end(), '1'); + d_HAS_data.delta_clock_correction_clock_subset[i].reserve(Nsat_sub); - // Read Nsatprime values of delta_clock_c0_clock_subset - for (int j = 0; j < Nsatprime; j++) + // Read Nsat_sub values of delta_clock_correction_clock_subset + for (int j = 0; j < Nsat_sub; j++) { - d_HAS_data.delta_clock_c0_clock_subset[i][j] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH, message.end()); + d_HAS_data.delta_clock_correction_clock_subset[i][j] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_CORRECTION_SUBSET_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_CORRECTION_SUBSET_LENGTH, message.end()); } } - DLOG(INFO) << "Nsysprime: " << static_cast(d_HAS_data.Nsysprime); - DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("gnss_id_clock_subset", d_HAS_data.gnss_id_clock_subset)); - DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("delta_clock_c0_multiplier_clock_subset", d_HAS_data.delta_clock_c0_multiplier_clock_subset)); - DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_vector("satellite_submask", d_HAS_data.satellite_submask)); - DLOG(INFO) << (d_HAS_data.Nsysprime == 0 ? "" : debug_print_matrix("delta_clock_c0_clock_subset", d_HAS_data.delta_clock_c0_clock_subset)); + DLOG(INFO) << "Nsys_sub: " << static_cast(d_HAS_data.Nsys_sub); + DLOG(INFO) << (d_HAS_data.Nsys_sub == 0 ? "" : debug_print_vector("gnss_id_clock_subset", d_HAS_data.gnss_id_clock_subset)); + DLOG(INFO) << (d_HAS_data.Nsys_sub == 0 ? "" : debug_print_vector("delta_clock_multiplier_clock_subset", d_HAS_data.delta_clock_multiplier_clock_subset)); + DLOG(INFO) << (d_HAS_data.Nsys_sub == 0 ? "" : debug_print_vector("satellite_submask", d_HAS_data.satellite_submask)); + DLOG(INFO) << (d_HAS_data.Nsys_sub == 0 ? "" : debug_print_matrix("delta_clock_correction_clock_subset", d_HAS_data.delta_clock_correction_clock_subset)); } if (d_HAS_data.header.code_bias_flag && have_mask) @@ -785,19 +875,6 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) DLOG(INFO) << debug_print_matrix("phase bias", d_HAS_data.phase_bias); DLOG(INFO) << debug_print_matrix("phase discontinuity indicator", d_HAS_data.phase_discontinuity_indicator); } - - // if (d_HAS_data.header.ura_flag && have_mask) - // { - // // read URA - // d_HAS_data.validity_interval_index_ura_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); - // message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - // d_HAS_data.ura = std::vector(Nsat); - // for (int i = 0; i < Nsat; i++) - // { - // d_HAS_data.ura[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_URA_LENGTH)); - // message = std::string(message.begin() + HAS_MSG_URA_LENGTH, message.end()); - // } - // } } diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index dd26b1e34..e564ecdc7 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -57,6 +57,7 @@ class galileo_e6_has_msg_receiver : public gr::block public: ~galileo_e6_has_msg_receiver() = default; //!< Default destructor void set_enable_navdata_monitor(bool enable); + std::shared_ptr process_test_page(const pmt::pmt_t& msg); //!< For testing purposes only private: friend galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make(); @@ -66,6 +67,7 @@ private: void process_HAS_page(const Galileo_HAS_page& has_page); void read_MT1_header(const std::string& message_header); void read_MT1_body(const std::string& message_body); + void delete_outdated_data(const Galileo_HAS_page& has_page); int decode_message_type1(uint8_t message_id, uint8_t message_size); @@ -89,9 +91,12 @@ private: Nav_Message_Packet d_nav_msg_packet; // Store decoding matrices and received PIDs + std::vector> d_received_timestamps; std::vector>> d_C_matrix; std::vector> d_M_matrix; std::vector> d_received_pids; + std::vector d_printed_timestamps; + std::vector d_printed_mids; // Store masks std::vector d_nsat_in_mask_id; diff --git a/src/core/system_parameters/Galileo_CNAV.h b/src/core/system_parameters/Galileo_CNAV.h index 5a3ca8a31..226b1d888 100644 --- a/src/core/system_parameters/Galileo_CNAV.h +++ b/src/core/system_parameters/Galileo_CNAV.h @@ -1,9 +1,9 @@ /*! * \file Galileo_CNAV.h * \brief Galileo CNAV mesage constants. Data from: - * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 - * (April 2020). - * \author Carles Fernandez-Prades, 2020. cfernandez(at)cttc.es + * Galileo High Accuracy Service Signal-In-Space Interface Control Document + * (HAS SIS ICD) Issue 1.0, May 2022 + * \author Carles Fernandez-Prades, 2020-2022. cfernandez(at)cttc.es * * * ----------------------------------------------------------------------------- @@ -11,7 +11,7 @@ * 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) + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -29,71 +29,74 @@ /** \addtogroup System_Parameters * \{ */ -// Galileo CNAV message structure -constexpr int32_t GALILEO_CNAV_SYMBOLS_PER_PAGE = 1000; //!< Total numer of symbols per HAS page including the sync pattern -constexpr int32_t GALILEO_CNAV_PREAMBLE_PERIOD_SYMBOLS = 1000; -constexpr int32_t GALILEO_CNAV_PAGE_MS = 1; //!< Duration in ms of a CNAV page -constexpr int32_t GALILEO_CNAV_INTERLEAVER_ROWS = 8; -constexpr int32_t GALILEO_CNAV_INTERLEAVER_COLS = 123; -constexpr int32_t GALILEO_CNAV_TELEMETRY_RATE_BITS_SECOND = 1000; // bps -constexpr int32_t GALILEO_CNAV_HAS_PAGE_DATA_BITS = 448; -constexpr int32_t GALILEO_CNAV_PAGE_RESERVED_BITS = 14; -constexpr int32_t GALILEO_CNAV_BITS_FOR_CRC = GALILEO_CNAV_HAS_PAGE_DATA_BITS + GALILEO_CNAV_PAGE_RESERVED_BITS; // 462 -constexpr int32_t GALILEO_CNAV_BYTES_FOR_CRC = 58; -constexpr int32_t GALILEO_CNAV_CRC_LENGTH = 24; -constexpr int32_t GALILEO_CNAV_MESSAGE_BITS_PER_PAGE = 424; -constexpr int32_t GALILEO_CNAV_PAGE_HEADER_BITS = 24; -constexpr int32_t GALILEO_CNAV_PREAMBLE_LENGTH_BITS = 16; -constexpr int32_t GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK = 255; -constexpr int32_t GALILEO_CNAV_MT1_HEADER_BITS = 32; -constexpr int32_t GALILEO_CNAV_OCTETS_IN_SUBPAGE = 53; -constexpr int32_t GALILEO_CNAV_INFORMATION_VECTOR_LENGTH = 32; - -constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; - // Galileo HAS message field lengths -constexpr size_t HAS_MSG_NSYS_LENGTH = 4; -constexpr size_t HAS_MSG_ID_MASK_LENGTH = 4; -constexpr size_t HAS_MSG_SATELLITE_MASK_LENGTH = 40; -constexpr size_t HAS_MSG_SIGNAL_MASK_LENGTH = 16; -constexpr size_t HAS_MSG_NAV_MESSAGE_LENGTH = 3; -constexpr size_t HAS_MSG_MASK_RESERVED_LENGTH = 6; -constexpr size_t HAS_MSG_VALIDITY_INDEX_LENGTH = 4; -constexpr size_t HAS_MSG_IOD_GPS_LENGTH = 8; -constexpr size_t HAS_MSG_IOD_GAL_LENGTH = 10; -constexpr size_t HAS_MSG_DELTA_RADIAL_LENGTH = 14; -constexpr size_t HAS_MSG_DELTA_ALONG_TRACK_LENGTH = 12; -constexpr size_t HAS_MSG_DELTA_CROSS_TRACK_LENGTH = 12; -constexpr size_t HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH = 2; -constexpr size_t HAS_MSG_DELTA_CLOCK_C0_LENGTH = 14; -constexpr size_t HAS_MSG_NSYSPRIME_LENGTH = 4; -constexpr size_t HAS_MSG_ID_CLOCK_SUBSET_LENGTH = 4; -constexpr size_t HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH = 2; -constexpr size_t HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH = 13; -constexpr size_t HAS_MSG_CODE_BIAS_LENGTH = 11; -constexpr size_t HAS_MSG_PHASE_BIAS_LENGTH = 11; -constexpr size_t HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH = 2; -constexpr size_t HAS_MSG_URA_LENGTH = 2; +constexpr size_t HAS_MSG_NSYS_LENGTH = 4; // HAS SIS ICD 1.0 Table 15 +constexpr size_t HAS_MSG_ID_MASK_LENGTH = 4; // HAS SIS ICD 1.0 Table 16 +constexpr size_t HAS_MSG_SATELLITE_MASK_LENGTH = 40; // HAS SIS ICD 1.0 Table 16 +constexpr size_t HAS_MSG_SIGNAL_MASK_LENGTH = 16; // HAS SIS ICD 1.0 Table 16 +constexpr size_t HAS_MSG_NAV_MESSAGE_LENGTH = 3; // HAS SIS ICD 1.0 Table 16 +constexpr size_t HAS_MSG_MASK_RESERVED_LENGTH = 6; // HAS SIS ICD 1.0 Table 15 +constexpr size_t HAS_MSG_VALIDITY_INDEX_LENGTH = 4; // HAS SIS ICD 1.0 Table 22 +constexpr size_t HAS_MSG_IOD_GPS_LENGTH = 8; // HAS SIS ICD 1.0 Table 26 +constexpr size_t HAS_MSG_IOD_GAL_LENGTH = 10; // HAS SIS ICD 1.0 Table 26 +constexpr size_t HAS_MSG_DELTA_RADIAL_LENGTH = 13; // HAS SIS ICD 1.0 Table 25 +constexpr size_t HAS_MSG_DELTA_IN_TRACK_LENGTH = 12; // HAS SIS ICD 1.0 Table 25 +constexpr size_t HAS_MSG_DELTA_CROSS_TRACK_LENGTH = 12; // HAS SIS ICD 1.0 Table 25 +constexpr size_t HAS_MSG_DELTA_CLOCK_MULTIPLIER_LENGTH = 2; // HAS SIS ICD 1.0 Table 28 +constexpr size_t HAS_MSG_DELTA_CLOCK_CORRECTION_LENGTH = 13; // HAS SIS ICD 1.0 Table 31 +constexpr size_t HAS_MSG_NSYS_SUB_LENGTH = 4; // HAS SIS ICD 1.0 Table 32 +constexpr size_t HAS_MSG_ID_CLOCK_SUBSET_LENGTH = 4; // HAS SIS ICD 1.0 Table 32 +constexpr size_t HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH = 2; // HAS SIS ICD 1.0 Table 33 +constexpr size_t HAS_MSG_DELTA_CLOCK_CORRECTION_SUBSET_LENGTH = 13; // HAS SIS ICD 1.0 Table 34 +constexpr size_t HAS_MSG_CODE_BIAS_LENGTH = 11; // HAS SIS ICD 1.0 Table 37 +constexpr size_t HAS_MSG_PHASE_BIAS_LENGTH = 11; // HAS SIS ICD 1.0 Table 40 +constexpr size_t HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH = 2; // HAS SIS ICD 1.0 Table 40 -constexpr int32_t HAS_MSG_NUMBER_MASK_IDS = 32; -constexpr int32_t HAS_MSG_NUMBER_GNSS_IDS = 16; -constexpr int32_t HAS_MSG_NUMBER_MESSAGE_IDS = 32; -constexpr int32_t HAS_MSG_NUMBER_SATELLITE_IDS = 40; -constexpr int32_t HAS_MSG_NUMBER_SIGNAL_MASKS = 16; +constexpr uint64_t MAX_SECONDS_REMEMBERING_MID = 150; // HAS SIS ICD 1.0 Section 6.4.1 HAS Message Completion Time-out -constexpr float HAS_MSG_DELTA_RADIAL_SCALE_FACTOR = 0.0025; -constexpr float HAS_MSG_DELTA_ALONG_TRACK_SCALE_FACTOR = 0.008; -constexpr float HAS_MSG_DELTA_CROSS_TRACK_SCALE_FACTOR = 0.008; -constexpr float HAS_MSG_DELTA_CLOCK_SCALE_FACTOR = 0.0025; -constexpr float HAS_MSG_CODE_BIAS_SCALE_FACTOR = 0.02; -constexpr float HAS_MSG_PHASE_BIAS_SCALE_FACTOR = 0.01; +// Galileo CNAV message structure +constexpr int32_t GALILEO_CNAV_SYMBOLS_PER_PAGE = 1000; // Total numer of symbols per HAS page including the sync pattern +constexpr int32_t GALILEO_CNAV_PREAMBLE_PERIOD_SYMBOLS = 1000; +constexpr int32_t GALILEO_CNAV_PAGE_MS = 1; // Duration in ms of a CNAV page +constexpr int32_t GALILEO_CNAV_INTERLEAVER_ROWS = 8; // HAS SIS ICD 1.0 Table 4 +constexpr int32_t GALILEO_CNAV_INTERLEAVER_COLS = 123; // HAS SIS ICD 1.0 Table 4 +constexpr int32_t GALILEO_CNAV_TELEMETRY_RATE_BITS_SECOND = 1000; // bps +constexpr int32_t GALILEO_CNAV_HAS_PAGE_DATA_BITS = 448; // HAS SIS ICD 1.0 Table 5 +constexpr int32_t GALILEO_CNAV_PAGE_RESERVED_BITS = 14; // HAS SIS ICD 1.0 Table 5 +constexpr int32_t GALILEO_CNAV_BYTES_FOR_CRC = 58; // ceil(462 / 8) +constexpr int32_t GALILEO_CNAV_CRC_LENGTH = 24; // HAS SIS ICD 1.0 Table 5 +constexpr int32_t GALILEO_CNAV_MESSAGE_BITS_PER_PAGE = 424; // HAS SIS ICD 1.0 Table 6 +constexpr int32_t GALILEO_CNAV_PAGE_HEADER_BITS = 24; // HAS SIS ICD 1.0 Table 6 +constexpr int32_t GALILEO_CNAV_PREAMBLE_LENGTH_BITS = 16; // HAS SIS ICD 1.0 Table 5 +constexpr int32_t GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK = 255; // HAS SIS ICD 1.0 Section 6.2 Reed-Solomon Code +constexpr int32_t GALILEO_CNAV_MT1_HEADER_BITS = 32; // HAS SIS ICD 1.0 Table 11 +constexpr int32_t GALILEO_CNAV_OCTETS_IN_SUBPAGE = 53; // HAS SIS ICD 1.0 Section 6.3 HAS Encoding and Transmission +constexpr int32_t GALILEO_CNAV_INFORMATION_VECTOR_LENGTH = 32; // HAS SIS ICD 1.0 Section 6.2 Reed-Solomon Code -constexpr uint16_t HAS_MSG_NUMBER_MAX_TOH = 3599; +constexpr int32_t GALILEO_CNAV_BITS_FOR_CRC = GALILEO_CNAV_HAS_PAGE_DATA_BITS + GALILEO_CNAV_PAGE_RESERVED_BITS; // 462 -constexpr uint8_t HAS_MSG_GPS_SYSTEM = 0; // Table 8 ICD v1.2 -constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // Table 8 ICD v1.2 +constexpr int32_t HAS_MSG_NUMBER_MASK_IDS = 32; // HAS SIS ICD 1.0 Table 13 +constexpr int32_t HAS_MSG_NUMBER_GNSS_IDS = 16; // HAS SIS ICD 1.0 Table 18 +constexpr int32_t HAS_MSG_NUMBER_MESSAGE_IDS = 32; // HAS SIS ICD 1.0 Table 8 +constexpr int32_t HAS_MSG_NUMBER_SATELLITE_IDS = 40; // HAS SIS ICD 1.0 Table 19 +constexpr int32_t HAS_MSG_NUMBER_SIGNAL_MASKS = 16; // HAS SIS ICD 1.0 Table 20 + +constexpr float HAS_MSG_DELTA_RADIAL_SCALE_FACTOR = 0.0025; // HAS SIS ICD 1.0 Table 25 +constexpr float HAS_MSG_DELTA_IN_TRACK_SCALE_FACTOR = 0.008; // HAS SIS ICD 1.0 Table 25 +constexpr float HAS_MSG_DELTA_CROSS_TRACK_SCALE_FACTOR = 0.008; // HAS SIS ICD 1.0 Table 25 +constexpr float HAS_MSG_DELTA_CLOCK_SCALE_FACTOR = 0.0025; // HAS SIS ICD 1.0 Table 31 +constexpr float HAS_MSG_CODE_BIAS_SCALE_FACTOR = 0.02; // HAS SIS ICD 1.0 Table 37 +constexpr float HAS_MSG_PHASE_BIAS_SCALE_FACTOR = 0.01; // HAS SIS ICD 1.0 Table 40 + +constexpr uint16_t HAS_MSG_NUMBER_MAX_TOH = 3599; // HAS SIS ICD 1.0 Table 13 + +constexpr uint8_t HAS_MSG_GPS_SYSTEM = 0; // HAS SIS ICD 1.0 Table 18 +constexpr uint8_t HAS_MSG_GALILEO_SYSTEM = 2; // HAS SIS ICD 1.0 Table 18 constexpr uint8_t HAS_MSG_WRONG_SYSTEM = 255; +constexpr char GALILEO_CNAV_PREAMBLE[17] = "1011011101110000"; // HAS SIS ICD 1.0 Section 2.3.1 + +// HAS SIS ICD 1.0 Table 7 const std::pair GALILEO_HAS_STATUS({1, 2}); const std::pair GALILEO_HAS_RESERVED({3, 2}); const std::pair GALILEO_HAS_MESSAGE_TYPE({5, 2}); @@ -101,6 +104,7 @@ const std::pair GALILEO_HAS_MESSAGE_ID({7, 5}); const std::pair GALILEO_HAS_MESSAGE_SIZE({12, 5}); const std::pair GALILEO_HAS_MESSAGE_PAGE_ID({17, 8}); +// HAS SIS ICD 1.0 Table 12 const std::pair GALILEO_MT1_HEADER_TOH({1, 12}); const std::pair GALILEO_MT1_HEADER_MASK_FLAG({13, 1}); const std::pair GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG({14, 1}); @@ -108,9 +112,9 @@ const std::pair GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG({15, 1}) const std::pair GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG({16, 1}); const std::pair GALILEO_MT1_HEADER_CODE_BIAS_FLAG({17, 1}); const std::pair GALILEO_MT1_HEADER_PHASE_BIAS_FLAG({18, 1}); -const std::pair GALILEO_MT1_HEADER_URA_FLAG({19, 1}); +const std::pair GALILEO_MT1_HEADER_RESERVED({19, 4}); const std::pair GALILEO_MT1_HEADER_MASK_ID({23, 5}); -const std::pair GALILEO_MT1_HEADER_IOD_ID({28, 5}); +const std::pair GALILEO_MT1_HEADER_IOD_SET_ID({28, 5}); /** \} */ diff --git a/src/core/system_parameters/galileo_cnav_message.cc b/src/core/system_parameters/galileo_cnav_message.cc index 84a12c798..0b93f6bf0 100644 --- a/src/core/system_parameters/galileo_cnav_message.cc +++ b/src/core/system_parameters/galileo_cnav_message.cc @@ -1,16 +1,16 @@ /*! * \file galileo_cnav_message.cc * \brief Implementation of a Galileo CNAV Data message as described in - * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 - * (April 2020) - * \author Carles Fernandez-Prades, 2020-2021 cfernandez(at)cttc.es + * Galileo High Accuracy Service Signal-In-Space Interface Control Document + * (HAS SIS ICD) Issue 1.0, May 2022 + * \author Carles Fernandez-Prades, 2020-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-2021 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -67,7 +67,7 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string) read_HAS_page_header(page_string.substr(GALILEO_CNAV_PAGE_RESERVED_BITS, GALILEO_CNAV_PAGE_HEADER_BITS)); bool use_has = false; d_test_mode = false; - // HAS status as defined in ICD v1.2 Table 5 HAS Page Header + // HAS status as defined in HAS SIS ICD v1.0 Table 9 - HASS values and corresponding semantic if (!d_page_dummy) { switch (d_has_page_status) @@ -118,7 +118,7 @@ void Galileo_Cnav_Message::read_HAS_page_header(const std::string& page_string) } if (!d_page_dummy) { - // ICD v1.2 Table 5: HAS page header + // HAS SIS ICD v1.0 Table 7: HAS page header const std::bitset has_page_header(page_string); d_has_page_status = read_has_page_header_parameter(has_page_header, GALILEO_HAS_STATUS); d_has_reserved = read_has_page_header_parameter(has_page_header, GALILEO_HAS_RESERVED); diff --git a/src/core/system_parameters/galileo_cnav_message.h b/src/core/system_parameters/galileo_cnav_message.h index 7df0e4a0a..90444175b 100644 --- a/src/core/system_parameters/galileo_cnav_message.h +++ b/src/core/system_parameters/galileo_cnav_message.h @@ -1,16 +1,16 @@ /*! * \file galileo_cnav_message.h * \brief Implementation of a Galileo CNAV Data message as described in - * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 - * (April 2020) - * \author Carles Fernandez-Prades, 2020-2021 cfernandez(at)cttc.es + * Galileo High Accuracy Service Signal-In-Space Interface Control Document + * (HAS SIS ICD) Issue 1.0, May 2022 + * \author Carles Fernandez-Prades, 2020-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-2021 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -34,8 +34,8 @@ /*! * \brief This class handles the Galileo CNAV Data message, as described in the - * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 - * (April 2020) + * Galileo High Accuracy Service Signal-In-Space Interface Control Document + * (HAS SIS ICD) Issue 1.0, May 2022 */ class Galileo_Cnav_Message { @@ -69,6 +69,11 @@ public: return d_flag_CRC_test; } + inline void set_time_stamp(uint64_t time_stamp) + { + has_page.time_stamp = time_stamp; + } + private: uint8_t read_has_page_header_parameter(const std::bitset& bits, const std::pair& parameter) const; bool CRC_test(const std::bitset& bits, uint32_t checksum) const; diff --git a/src/core/system_parameters/galileo_has_data.cc b/src/core/system_parameters/galileo_has_data.cc index 5cc8c381b..f3bb628cd 100644 --- a/src/core/system_parameters/galileo_has_data.cc +++ b/src/core/system_parameters/galileo_has_data.cc @@ -1,14 +1,14 @@ /*! * \file galileo_has_data.cc * \brief Class for Galileo HAS message type 1 data storage - * \author Carles Fernandez-Prades, 2020-2021 cfernandez(at)cttc.es + * \author Carles Fernandez-Prades, 2020-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-2021 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -117,6 +117,7 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con { uint8_t system = gnss_id_mask[nsys]; std::string signal; + // See HAS SIS ICD v1.0 Table 20 switch (k) { case 0: @@ -171,7 +172,7 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con if (system == 0) { // GPS - signal = "L1 L1C(D)"; + signal = "L1C(D)"; } else if (system == 2) { @@ -187,7 +188,7 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con if (system == 0) { // GPS - signal = "L1 L1C(P)"; + signal = "L1C(P)"; } else if (system == 2) { @@ -203,7 +204,7 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con if (system == 0) { // GPS - signal = "L1 L1C(D+P)"; + signal = "L1C(D+P)"; } else if (system == 2) { @@ -219,7 +220,7 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con if (system == 0) { // GPS - signal = "L2 L2C(M)"; + signal = "L2 CM"; } else if (system == 2) { @@ -235,7 +236,7 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con if (system == 0) { // GPS - signal = "L2 L2C(L)"; + signal = "L2 CL"; } else if (system == 2) { @@ -251,7 +252,7 @@ std::vector Galileo_HAS_data::get_signals_in_mask(uint8_t nsys) con if (system == 0) { // GPS - signal = "L2 L2C(M+L)"; + signal = "L2 CM+CL"; } else if (system == 2) { @@ -405,6 +406,7 @@ uint8_t Galileo_HAS_data::get_gnss_id(int nsat) const uint16_t Galileo_HAS_data::get_validity_interval_s(uint8_t validity_interval_index) const { uint16_t validity_interval; + // See HAS SIS ICD v1.0 Table 23 switch (validity_interval_index) { case 0: @@ -503,28 +505,28 @@ std::vector Galileo_HAS_data::get_delta_radial_m(uint8_t nsys) const } -std::vector Galileo_HAS_data::get_delta_along_track_m() const +std::vector Galileo_HAS_data::get_delta_in_track_m() const { - std::vector delta_along_track_m; - delta_along_track_m.reserve(this->delta_along_track.size()); - for (const auto& d : this->delta_along_track) + std::vector delta_in_track_m; + delta_in_track_m.reserve(this->delta_in_track.size()); + for (const auto& d : this->delta_in_track) { - delta_along_track_m.push_back(static_cast(d) * HAS_MSG_DELTA_ALONG_TRACK_SCALE_FACTOR); + delta_in_track_m.push_back(static_cast(d) * HAS_MSG_DELTA_IN_TRACK_SCALE_FACTOR); } - return delta_along_track_m; + return delta_in_track_m; } -std::vector Galileo_HAS_data::get_delta_along_track_m(uint8_t nsys) const +std::vector Galileo_HAS_data::get_delta_in_track_m(uint8_t nsys) const { - std::vector delta_along_track_m = this->get_delta_along_track_m(); + std::vector delta_in_track_m = this->get_delta_in_track_m(); if (nsys >= this->Nsys) { - return delta_along_track_m; + return delta_in_track_m; } - std::vector delta_along_track_m_aux; + std::vector delta_in_track_m_aux; uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; - delta_along_track_m_aux.reserve(num_sats_in_this_system); + delta_in_track_m_aux.reserve(num_sats_in_this_system); size_t index = 0; for (uint8_t sys = 0; sys <= nsys; sys++) @@ -538,12 +540,12 @@ std::vector Galileo_HAS_data::get_delta_along_track_m(uint8_t nsys) const { for (uint8_t sat = 0; sat < num_sats_in_system; sat++) { - delta_along_track_m_aux.push_back(delta_along_track_m[index]); + delta_in_track_m_aux.push_back(delta_in_track_m[index]); index++; } } } - return delta_along_track_m_aux; + return delta_in_track_m_aux; } @@ -551,7 +553,7 @@ std::vector Galileo_HAS_data::get_delta_cross_track_m() const { std::vector delta_cross_track_m; delta_cross_track_m.reserve(this->delta_cross_track.size()); - for (const auto& d : this->delta_along_track) + for (const auto& d : this->delta_cross_track) { delta_cross_track_m.push_back(static_cast(d) * HAS_MSG_DELTA_CROSS_TRACK_SCALE_FACTOR); } @@ -591,28 +593,28 @@ std::vector Galileo_HAS_data::get_delta_cross_track_m(uint8_t nsys) const } -std::vector Galileo_HAS_data::get_delta_clock_c0_m() const +std::vector Galileo_HAS_data::get_delta_clock_correction_m() const { - std::vector delta_clock_c0_m; - delta_clock_c0_m.reserve(this->delta_clock_c0.size()); - for (const auto& d : this->delta_clock_c0) + std::vector delta_clock_correction_m; + delta_clock_correction_m.reserve(this->delta_clock_correction.size()); + for (const auto& d : this->delta_clock_correction) { - delta_clock_c0_m.push_back(static_cast(d) * HAS_MSG_DELTA_CLOCK_SCALE_FACTOR); + delta_clock_correction_m.push_back(static_cast(d) * HAS_MSG_DELTA_CLOCK_SCALE_FACTOR); } - return delta_clock_c0_m; + return delta_clock_correction_m; } -std::vector Galileo_HAS_data::get_delta_clock_c0_m(uint8_t nsys) const +std::vector Galileo_HAS_data::get_delta_clock_correction_m(uint8_t nsys) const { - std::vector delta_clock_c0_m = this->get_delta_clock_c0_m(); + std::vector delta_clock_correction_m = this->get_delta_clock_correction_m(); if (nsys >= this->Nsys) { - return delta_clock_c0_m; + return delta_clock_correction_m; } - std::vector delta_clock_c0_m_aux; + std::vector delta_clock_correction_m_aux; uint8_t num_sats_in_this_system = this->get_num_satellites()[nsys]; - delta_clock_c0_m_aux.reserve(num_sats_in_this_system); + delta_clock_correction_m_aux.reserve(num_sats_in_this_system); size_t index = 0; for (uint8_t sys = 0; sys <= nsys; sys++) @@ -626,12 +628,12 @@ std::vector Galileo_HAS_data::get_delta_clock_c0_m(uint8_t nsys) const { for (uint8_t sat = 0; sat < num_sats_in_system; sat++) { - delta_clock_c0_m_aux.push_back(delta_clock_c0_m[index]); + delta_clock_correction_m_aux.push_back(delta_clock_correction_m[index]); index++; } } } - return delta_clock_c0_m_aux; + return delta_clock_correction_m_aux; } @@ -760,8 +762,8 @@ std::vector Galileo_HAS_data::get_systems_string() const } -uint16_t Galileo_HAS_data::get_nsatprime() const +uint16_t Galileo_HAS_data::get_nsat_sub() const { - auto Nsatprime = static_cast(this->delta_clock_c0_clock_subset.size()); - return Nsatprime; + auto Nsat_sub = static_cast(this->delta_clock_correction_clock_subset.size()); + return Nsat_sub; } diff --git a/src/core/system_parameters/galileo_has_data.h b/src/core/system_parameters/galileo_has_data.h index 0785efb08..a4a3c289e 100644 --- a/src/core/system_parameters/galileo_has_data.h +++ b/src/core/system_parameters/galileo_has_data.h @@ -1,14 +1,14 @@ /*! * \file galileo_has_data.h * \brief Class for Galileo HAS message type 1 data storage - * \author Carles Fernandez-Prades, 2020-2021 cfernandez(at)cttc.es + * \author Carles Fernandez-Prades, 2020-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-2021 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -31,20 +31,21 @@ struct mt1_header { uint16_t toh; uint8_t mask_id; - uint8_t iod_id; + uint8_t iod_set_id; + uint8_t reserved; bool mask_flag; bool orbit_correction_flag; bool clock_fullset_flag; bool clock_subset_flag; bool code_bias_flag; bool phase_bias_flag; - bool ura_flag; }; /*! * \brief This class is a storage for Galileo HAS message type 1, as defined in - * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 - * (April 2020). + * Galileo High Accuracy Service Signal-In-Space Interface Control Document + * (HAS SIS ICD) Issue 1.0, May 2022. + * See https://www.gsc-europa.eu/sites/default/files/sites/all/files/Galileo_HAS_SIS_ICD_v1.0.pdf */ class Galileo_HAS_data { @@ -57,69 +58,64 @@ public: std::vector> get_phase_bias_cycle() const; //!< Get Nsat x Nphases phase biases in [cycles] std::vector get_delta_radial_m() const; //!< Get Nsat delta radial corrections in [m] std::vector get_delta_radial_m(uint8_t nsys) const; //!< Get delta radial corrections in [m] for system nsys, with 0 <= nsys < Nsys - std::vector get_delta_along_track_m() const; //!< Get Nsat delta along_track corrections in [m] - std::vector get_delta_along_track_m(uint8_t nsys) const; //!< Get alog-track corrections in [m] for system nsys, with 0 <= nsys < Nsys - std::vector get_delta_cross_track_m() const; //!< Get Nsat delta cross_track corrections in [m] - std::vector get_delta_cross_track_m(uint8_t nsys) const; //!< Get delta cross_track corrections in [m] for system nsys, with 0 <= nsys < Nsys - std::vector get_delta_clock_c0_m() const; //!< Get Nsat delta clock C0 corrections in [m] - std::vector get_delta_clock_c0_m(uint8_t nsys) const; //!< Get delta clock C0 corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_delta_in_track_m() const; //!< Get Nsat delta in-track corrections in [m] + std::vector get_delta_in_track_m(uint8_t nsys) const; //!< Get delta in-track corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_delta_cross_track_m() const; //!< Get Nsat delta cross-track corrections in [m] + std::vector get_delta_cross_track_m(uint8_t nsys) const; //!< Get delta cross-track corrections in [m] for system nsys, with 0 <= nsys < Nsys + std::vector get_delta_clock_correction_m() const; //!< Get Nsat delta clock C0 corrections in [m] + std::vector get_delta_clock_correction_m(uint8_t nsys) const; //!< Get delta clock C0 corrections in [m] for system nsys, with 0 <= nsys < Nsys std::vector get_PRNs_in_mask(uint8_t nsys) const; //!< Get PRNs in mask for system nsys, with 0 <= nsys < Nsys std::vector get_PRNs_in_submask(uint8_t nsys) const; //!< Get PRNs in submask for system nsys, with 0 <= nsys < Nsys std::vector get_gnss_iod(uint8_t nsys) const; //!< Get GNSS IODs for for system nsys, with 0 <= nsys < Nsys std::vector get_num_satellites() const; //!< Get Nsys number of satellites uint16_t get_nsat() const; //!< Get total number of satellites with corrections - uint16_t get_nsatprime() const; //!< Get number of satellites in clock subset corrections - uint16_t get_validity_interval_s(uint8_t validity_interval_index) const; //!< Get validity interbal in [s] from the validity_interval_index + uint16_t get_nsat_sub() const; //!< Get number of satellites in clock subset corrections + uint16_t get_validity_interval_s(uint8_t validity_interval_index) const; //!< Get validity interval in [s] from the validity_interval_index uint8_t get_gnss_id(int nsat) const; //!< Get GNSS ID from the nsat satellite // Mask - std::vector gnss_id_mask; - std::vector satellite_mask; - std::vector signal_mask; - std::vector cell_mask_availability_flag; - std::vector>> cell_mask; - std::vector nav_message; + std::vector gnss_id_mask; //!< GNSS ID. See HAS SIS ICD 1.0 Section 5.2.1.1 + std::vector satellite_mask; //!< SatM - Satellite Mask. See HAS SIS ICD 1.0 Section 5.2.1.2 + std::vector signal_mask; //!< SigM - Signal Mask. See HAS SIS ICD 1.0 Section 5.2.1.3 + std::vector cell_mask_availability_flag; //!< CMAF - Cell Mask Availability Flag. See HAS SIS ICD 1.0 Section 5.2.1.4 + std::vector>> cell_mask; //!< CM - Cell Mask. See HAS SIS ICD 1.0 Section 5.2.1.5 + std::vector nav_message; //!< NM - Navigation Message Index. See HAS SIS ICD 1.0 Section 5.2.1.6 // Orbit corrections - std::vector gnss_iod; - std::vector delta_radial; - std::vector delta_along_track; - std::vector delta_cross_track; + std::vector gnss_iod; //!< IODref - Reference Issue of Data. See HAS SIS ICD 1.0 Table 26 + std::vector delta_radial; //!< DR - Delta Radial Correction. See HAS SIS ICD 1.0 Table 25 + std::vector delta_in_track; //!< DIT - Delta In-Track Correction. See HAS SIS ICD 1.0 Table 25 + std::vector delta_cross_track; //!< DCT - Delta Cross Correction. See HAS SIS ICD 1.0 Table 25 // Clock full-set corrections - std::vector delta_clock_c0_multiplier; - std::vector iod_change_flag; - std::vector delta_clock_c0; + std::vector delta_clock_multiplier; //!< DCM - Delta Clock Multipliers. See HAS SIS ICD 1.0 Section 5.2.3.1 + std::vector delta_clock_correction; //!< DCC - Delta Clock Corrections. See HAS SIS ICD 1.0 Section 5.2.3.2 // Clock subset corrections - std::vector gnss_id_clock_subset; - std::vector delta_clock_c0_multiplier_clock_subset; - std::vector satellite_submask; - std::vector> iod_change_flag_clock_subset; - std::vector> delta_clock_c0_clock_subset; + std::vector gnss_id_clock_subset; //!< GNSS ID. Specific GNSS to which the corrections refer. See HAS SIS ICD 1.0 Section 5.2.1.1 + std::vector delta_clock_multiplier_clock_subset; //!< DCM. Multiplier for all Delta Clock corrections. See HAS SIS ICD 1.0 Section 5.2.3.1 + std::vector satellite_submask; //!< SatMsub - Satellite Subset Mask. See HAS SIS ICD 1.0 Section 5.2.4.1 + std::vector> delta_clock_correction_clock_subset; //!< DCCsub - Delta Clock Subset Corrections. See HAS SIS ICD 1.0 Section 5.2.4.1 // Code bias - std::vector> code_bias; + std::vector> code_bias; //!< CB - Code bias for the m-th signal of the n-th SV. See HAS SIS ICD 1.0 Section 5.2.5 // Phase bias - std::vector> phase_bias; - std::vector> phase_discontinuity_indicator; + std::vector> phase_bias; //!< PB - Phase bias for the m-th signal of the n-th SV. See HAS SIS ICD 1.0 Section 5.2.6 + std::vector> phase_discontinuity_indicator; //!< PDI - Phase Discontinuity Indicator. See HAS SIS ICD 1.0 Section 5.2.6. - // URA - std::vector ura; + mt1_header header; //!< MT1 Header parameters. See HAS SIS ICD 1.0 Section 5.1.1 + uint8_t has_status; //!< HASS - HAS Status (from HAS page header). See HAS SIS ICD 1.0 Section 3.1.1 + uint8_t message_id; //!< MID - Message ID (from HAS page header). See HAS SIS ICD 1.0 Section 3.1 - mt1_header header; - uint8_t has_status; - uint8_t message_id; + uint8_t Nsys; //!< Number of GNSS for which corrections are provided. See HAS SIS ICD 1.0 Setion 5.2.1 + uint8_t Nsys_sub; //!< Number of GNSS for which corrections are provided in clock subset corrections. See HAS SIS ICD 1.0 Section 5.2.2.1 - uint8_t Nsys; //!< Number of GNSS for which corrections are provided - uint8_t validity_interval_index_orbit_corrections; // in Orbit corrections - uint8_t validity_interval_index_clock_fullset_corrections; // in Clock full-set corrections - uint8_t validity_interval_index_clock_subset_corrections; // in Clock subset corrections - uint8_t Nsysprime; // in Clock subset corrections - uint8_t validity_interval_index_code_bias_corrections; // in Code bias - uint8_t validity_interval_index_phase_bias_corrections; // in Phase bias - uint8_t validity_interval_index_ura_corrections; // in URA + uint8_t validity_interval_index_orbit_corrections; //!< VI - Validity Interval Index for Orbit corrections. See HAS SIS ICD 1.0 Section 5.2.2.1 + uint8_t validity_interval_index_clock_fullset_corrections; //!< VI - Validity Interval Index for Clock full-set corrections. See HAS SIS ICD 1.0 Section 5.2.2.1 + uint8_t validity_interval_index_clock_subset_corrections; //!< VI - Validity Interval Index for Clock subset corrections. See HAS SIS ICD 1.0 Section 5.2.2.1 + uint8_t validity_interval_index_code_bias_corrections; //!< VI - Validity Interval Index for Code bias. See HAS SIS ICD 1.0 Section 5.2.2.1 + uint8_t validity_interval_index_phase_bias_corrections; //!< VI - Validity Interval Index for Phase bias. See HAS SIS ICD 1.0 Section 5.2.2.1 }; diff --git a/src/core/system_parameters/galileo_has_page.h b/src/core/system_parameters/galileo_has_page.h index be37c0ecd..099ddd645 100644 --- a/src/core/system_parameters/galileo_has_page.h +++ b/src/core/system_parameters/galileo_has_page.h @@ -8,7 +8,7 @@ * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. * This file is part of GNSS-SDR. * - * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) * SPDX-License-Identifier: GPL-3.0-or-later * * ----------------------------------------------------------------------------- @@ -29,8 +29,8 @@ /*! * \brief This class is a storage for Galileo HAS message page, as defined in - * Galileo High Accuracy Service E6-B Signal-In-Space Message Specification v1.2 - * (April 2020). + * Galileo High Accuracy Service Signal-In-Space Interface Control Document + * (HAS SIS ICD) Issue 1.0, May 2022 */ class Galileo_HAS_page { @@ -38,6 +38,7 @@ public: Galileo_HAS_page() = default; std::string has_message_string; //!< HAS message content + uint64_t time_stamp{}; //!< HAS page time stamp, in [s] // HAS page header uint8_t has_status{}; //!< HAS status diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index cbe4ddce9..6efe155cd 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -131,6 +131,7 @@ DECLARE_string(log_dir); #include "unit-tests/system-parameters/glonass_gnav_crc_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/has_decoding_test.cc" #if EXTRA_TESTS #include "unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc" diff --git a/src/tests/unit-tests/system-parameters/galileo_e6b_reed_solomon_test.cc b/src/tests/unit-tests/system-parameters/galileo_e6b_reed_solomon_test.cc index 21f75de1c..78687ae7b 100644 --- a/src/tests/unit-tests/system-parameters/galileo_e6b_reed_solomon_test.cc +++ b/src/tests/unit-tests/system-parameters/galileo_e6b_reed_solomon_test.cc @@ -22,12 +22,12 @@ TEST(ReedSolomonE6BTest, EncodeWithGenMatrix) { - // input vector as defined in Galileo HAS ICD v1.2, Annex D + // input vector const std::vector input = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179}; - // Expected encoded output as defined in Galileo HAS ICD v1.2, Annex D + // Expected encoded output const std::vector expected_output = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179, 133, 210, 122, 224, 75, 138, @@ -55,12 +55,12 @@ TEST(ReedSolomonE6BTest, EncodeWithGenMatrix) TEST(ReedSolomonE6BTest, EncodeWithGenPoly) { - // input vector as defined in Galileo HAS ICD v1.2, Annex D + // input vector const std::vector input = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179}; - // Expected encoded output as defined in Galileo HAS ICD v1.2, Annex D + // Expected encoded output const std::vector expected_output = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179, 133, 210, 122, 224, 75, 138, @@ -88,12 +88,12 @@ TEST(ReedSolomonE6BTest, EncodeWithGenPoly) TEST(ReedSolomonE6BTest, EncodeWithCustomPoly) { - // input vector as defined in Galileo HAS ICD v1.2, Annex D + // input vector const std::vector input = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179}; - // Expected encoded output as defined in Galileo HAS ICD v1.2, Annex D + // Expected encoded output const std::vector expected_output = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179, 133, 210, 122, 224, 75, 138, @@ -144,12 +144,12 @@ TEST(ReedSolomonE6BTest, EncodeWithCustomPoly) TEST(ReedSolomonE6BTest, EncodeWithCustomMatrix) { - // input vector as defined in Galileo HAS ICD v1.2, Annex D + // input vector const std::vector input = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179}; - // Expected encoded output as defined in Galileo HAS ICD v1.2, Annex D + // Expected encoded output const std::vector expected_output = {71, 12, 25, 210, 178, 81, 243, 9, 112, 98, 196, 203, 48, 125, 114, 165, 181, 193, 71, 174, 168, 42, 31, 128, 245, 87, 150, 58, 192, 66, 130, 179, 133, 210, 122, 224, 75, 138, diff --git a/src/tests/unit-tests/system-parameters/has_decoding_test.cc b/src/tests/unit-tests/system-parameters/has_decoding_test.cc new file mode 100644 index 000000000..b4dfc5a46 --- /dev/null +++ b/src/tests/unit-tests/system-parameters/has_decoding_test.cc @@ -0,0 +1,364 @@ +/*! + * \file has_decoding_test.cc + * \brief Tests for HAS message decoder + * \author Carles Fernandez-Prades, 2021. 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-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "galileo_e6_has_msg_receiver.h" +#include "galileo_has_data.h" +#include "galileo_has_page.h" +#include "gnss_sdr_make_unique.h" +#include "has_simple_printer.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Usage: +// ./run_tests --gtest_filter=HAS_Test.Decoder +// ./run_tests --gtest_filter=HAS_Test.Decoder --has_data_test_file=../data/HAS_Messages_sample/encoded/Sample_HAS_Pages_Encoded_20210713_08.txt --start_page_test_file=70 +DEFINE_string(has_data_test_file, std::string(""), "File containing encoded HAS pages (format: [time sat_id HAS_page_in_hex] in each line)"); +DEFINE_int32(start_page_test_file, 0, "Starting page in case of reading HAS pages from a file"); + +#if PMT_USES_BOOST_ANY +namespace wht = boost; +#else +namespace wht = std; +#endif + +// ######## GNURADIO BLOCK MESSAGE RECEVER ######### +class HasDecoderTester; + +using HasDecoderTester_sptr = gnss_shared_ptr; + +HasDecoderTester_sptr HasDecoderTester_make(); + +class HasDecoderTester : public gr::block +{ +private: + friend HasDecoderTester_sptr HasDecoderTester_make(); + HasDecoderTester(); + +public: + std::shared_ptr generate_has_page(const std::string& page, int rx_time); + ~HasDecoderTester(); //!< Default destructor +}; + + +HasDecoderTester_sptr HasDecoderTester_make() +{ + return HasDecoderTester_sptr(new HasDecoderTester()); +} + + +std::shared_ptr HasDecoderTester::generate_has_page(const std::string& page, int rx_time) +{ + auto gh = std::make_shared(); + + std::string bits; + for (size_t i = 0; i < page.size(); i++) + { + std::string s = page.substr(i, 1); + std::stringstream ss; + ss << std::hex << s; + unsigned n; + ss >> n; + std::bitset<4> b(n); + bits += b.to_string(); + } + + if (page.substr(0, 6) != "AF3BC3") // if not dummy + { + gh->has_message_string = bits.substr(24, 424); + } + gh->time_stamp = rx_time; + + std::bitset<2> b_has_status(bits.substr(0, 2)); + gh->has_status = b_has_status.to_ulong(); + + std::bitset<2> b_has_reserved(bits.substr(2, 2)); + gh->reserved = b_has_reserved.to_ulong(); + + std::bitset<2> b_message_type(bits.substr(4, 2)); + gh->message_type = b_message_type.to_ulong(); + + std::bitset<5> b_message_id(bits.substr(6, 5)); + gh->message_id = b_message_id.to_ulong(); + + std::bitset<5> b_message_size(bits.substr(11, 5)); + gh->message_size = b_message_size.to_ulong() + 1; + + std::bitset<8> b_message_page_id(bits.substr(16, 8)); + gh->message_page_id = b_message_page_id.to_ulong(); + + return gh; +} + + +HasDecoderTester::HasDecoderTester() : gr::block("HasDecoderTester", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ +} + + +HasDecoderTester::~HasDecoderTester() = default; + + +// ########################################################### + + +class Read_Encoded_Pages +{ +public: + Read_Encoded_Pages() = default; + std::vector get_pages() const + { + return page; + }; + + std::vector get_sats() const + { + return sat; + }; + + std::vector get_time() const + { + return rx_time; + }; + + size_t get_number_pages() + { + return page.size(); + }; + + bool is_known_data() + { + return known_test_data; + }; + + inline bool read_data(const std::string& input_filename) + { + if (input_filename.empty()) + { + page = { + "07AC6195E44FBA30126D3291D1D973CF3A2C066B77C0816068769E6232772D8086B652DEA8DFC9470B7E12891F27458B84EE1A3D391E20EE", + "07AC746F1DA0B7DE403789910C247DCC8CB7AACBA0A395086EEF44091385FF7625FF276A0DF4131195F28794D812A987C53186CE332C2417", + "07AC877DCF08B51A8CDB5B1D98790DED6E2365123139CFAD31BC6A84D1D9F0FEB70BC74064DD533D069EEF4D3850B500A7CDAAB1B8F56AA4", + "07AC9AAD3485E17627D3697D8537A1DD44BC9923088078182A808E99DE334BEEFF560152B986035325EFDC7D0A43913D926C71C3B12AB757", + "07ACAD94E50D544B7130F3840FCEFD4D2AA5A5A66876ECBFFCB0869C5127401B125F3021E70012ECECC4CF6AC1CE65737585095885067A92", + "07AC263E0B467BD3AB999E76A9A8D05B18E6B818307C81EEF6DD14722388B72D5651E78F04D1D52E131494D624518F6358B19F508F3C7950", + "07AC39CC3470C0F3EEA678A635BA8D7D3FAD16BB0879549DB6835C2DBA92D53A06C919E702C0242F523CCCD5F88FC45AF21DA0519F28E506", + "07AC4C87FB8058D90D518A035849C341E421F0DD280A77FC57FE5823B9AD68A6D8EE7D7EC0FD6208DD969F4FAC3E4D7B4214FF808E2EDC7B", + "07AC5FD3E1D40A6E958E9C139DF60252A67F6DA37B7E416557058E6D979EC4DEB9802EDE9AD49E47B522A133A05FABFAF1212BF3219AED56", + "07AC72FEB3494D685DA492DA72E415A1255BE7DC08AD4EB1617428636CC30EAAB6A9948E03ED69887717D81F2324E48D717847FF29CEE115", + "07AC8553C81674CA989EE3B762BFE9F7113F9458A5FD2749D0B685A4F49012532088C872254C881194C7641762A7B9495A02BCD6686CE17F", + "07AC987E7D3A9854AA56BCCD7170CB6939966DA4F2199A0C6C5F9CAB5B24539786CCB299DA69DE4EEE9698EEDD2D7BD409565C27674B4268", + "07ACAB286D5CA9F01FEC5F5105132F0A41EFCFB5E970C06395B3FE72C3D3B476BADF27DC9CA50ED9EC997AB8BED648DF1424EE56FFAD35B1"}; + rx_time = {690883223, 690883223, 690883223, 690883223, 690883223, 690883224, 690883224, 690883224, 690883224, 690883224, 690883224, 690883224}; + known_test_data = true; + return true; + } + + known_test_data = false; + indata.open(input_filename); + + if (!indata) + { // file couldn't be opened + std::cerr << "Error: file could not be opened\n"; + return false; + } + + while (!indata.eof()) + { + indata >> time_instant; + indata >> sat_id; + indata >> page_content; + rx_time.push_back(time_instant); + sat.push_back(sat_id); + page.push_back(page_content); + } + return true; + } + +private: + std::vector rx_time; + std::vector sat; + std::vector page; + std::ifstream indata; + std::string sat_id; + std::string page_content; + int time_instant{}; + bool known_test_data{}; +}; + + +TEST(HAS_Test, Decoder) +{ + Read_Encoded_Pages read_pages{}; + EXPECT_TRUE(read_pages.read_data(FLAGS_has_data_test_file)); + auto gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make(); + auto has_tester = HasDecoderTester_make(); + std::unique_ptr has_simple_printer = nullptr; + + auto pages = read_pages.get_pages(); + auto rx_time = read_pages.get_time(); + bool known_data = read_pages.is_known_data(); + int init = 0; + if (!known_data) + { + has_simple_printer = std::make_unique(); + if (static_cast(FLAGS_start_page_test_file) < read_pages.get_number_pages()) + { + init = FLAGS_start_page_test_file; + } + else + { + std::cerr << "The flag --start_page_test_file is set beyond the total number of pages in the file (" << read_pages.get_number_pages() << "), ignoring it.\n"; + } + } + + for (size_t p = init; p < read_pages.get_number_pages(); p++) + { + auto has_page = has_tester->generate_has_page(pages[p], rx_time[p]); + if (!has_page->has_message_string.empty()) // if not dummy + { + auto has_message = gal_e6_has_rx_->process_test_page(pmt::make_any(has_page)); + if (has_message != nullptr) + { + if (known_data) + { + EXPECT_EQ(has_message->header.toh, 0); + EXPECT_EQ(has_message->header.mask_flag, true); + EXPECT_EQ(has_message->header.orbit_correction_flag, true); + EXPECT_EQ(has_message->header.clock_fullset_flag, false); + EXPECT_EQ(has_message->header.clock_subset_flag, false); + EXPECT_EQ(has_message->header.code_bias_flag, true); + EXPECT_EQ(has_message->header.phase_bias_flag, true); + EXPECT_EQ(has_message->header.reserved, 0); + EXPECT_EQ(has_message->header.mask_id, 1); + EXPECT_EQ(has_message->header.iod_set_id, 4); + + EXPECT_EQ(has_message->Nsys, 2); + EXPECT_EQ(has_message->gnss_id_mask[0], 0); + std::vector prns = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32}; + EXPECT_EQ(has_message->get_PRNs_in_mask(0), prns); + EXPECT_EQ(has_message->get_signals_in_mask(0)[0], "L1 C/A"); + EXPECT_EQ(has_message->cell_mask_availability_flag[0], false); + EXPECT_EQ(has_message->nav_message[0], 0); + + EXPECT_EQ(has_message->gnss_id_mask[1], 2); + prns = {1, 2, 3, 4, 5, 7, 8, 9, 11, 12, 13, 15, 19, 21, 24, 25, 26, 27, 30, 31, 33, 36}; + EXPECT_EQ(has_message->get_PRNs_in_mask(1), prns); + EXPECT_EQ(has_message->get_signals_in_mask(1)[0], "E1-C"); + EXPECT_EQ(has_message->get_signals_in_mask(1)[1], "E5a-Q"); + EXPECT_EQ(has_message->get_signals_in_mask(1)[2], "E5b-Q"); + EXPECT_EQ(has_message->get_signals_in_mask(1)[3], "E6-C"); + EXPECT_EQ(has_message->cell_mask_availability_flag[1], false); + EXPECT_EQ(has_message->nav_message[1], 0); + + EXPECT_EQ(has_message->validity_interval_index_orbit_corrections, 10); + + std::vector gnss_iod_expected = {17, 92, 1, 169, 13, 15, 67, 10, 84, 108, 34, 22, 203, 75, 91, 10, 60, 42, 35, 92, 48, 74, 81, 21, 19, 6, 0, 4, + 86, 22, 78, 78, 74, 78, 76, 78, 78, 78, 78, 75, 78, 76, 74, 78, 78, 78, 78, 78, 78, 78, 78, 78, 77}; + std::vector delta_radial_expected = {-0.355, -0.0925, -0.3, -0.8675, 0.1, 0.075, -10.24, -0.0575, 0.0725, 0.22, -0.0825, 0.03, -0.95, -0.1125, + -0.045, -0.1725, -0.975, 0.0725, -0.045, -0.13, -0.335, -0.88, -0.0375, 0.2925, 0.2375, 0.1125, -10.24, 0.13, -0.08, -0.1125, -0.115}; + std::vector delta_in_track_expected = {1.552, -0.544, 0.16, -0.272, 0.048, -0.784, -16.384, -0.16, -0.728, -1.528, 0.656, -0.216, -0.552, 0.984, + -0.216, -0.712, 0.184, -0.984, 0.376, 1.136, -0.072, -0.352, 0.488, -0.136, -0.456, 0.04, -16.384, 0.024, -1.616, 1.336, 0.472}; + std::vector delta_cross_track_expected = {0.832, 0.056, -0.024, -0.176, 0.04, -0.352, -16.384, -0.44, 0.016, 0.736, 0.104, 0.136, 0.696, 0.08, + -0.192, 0.32, -0.552, -0.304, 0.464, -0.184, 0.576, 0.864, 0.4, -0.896, -1.288, -0.808, -16.384, -0.304, 1.104, 0.576, 0.576}; + EXPECT_EQ(has_message->gnss_iod, gnss_iod_expected); + EXPECT_EQ(has_message->get_delta_radial_m(0).size(), delta_radial_expected.size()); + EXPECT_EQ(has_message->get_delta_in_track_m(0).size(), delta_in_track_expected.size()); + EXPECT_EQ(has_message->get_delta_cross_track_m(0).size(), delta_cross_track_expected.size()); + for (size_t i = 0; i < has_message->get_delta_radial_m(0).size(); i++) + { + EXPECT_FLOAT_EQ(has_message->get_delta_radial_m(0)[i], delta_radial_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_delta_in_track_m(0)[i], delta_in_track_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_delta_cross_track_m(0)[i], delta_cross_track_expected[i]); + } + + delta_radial_expected = {-0.1325, -0.0025, -0.1325, 0.0675, -0.0525, -0.2675, -0.1475, 0.0475, 0.17, 0.185, -0.195, -0.01, -0.0925, -0.155, 0.0375, + 0.2075, 0.08, -0.175, -0.1925, -0.0375, -0.0575, 0.0875}; + delta_in_track_expected = {0.056, -0.072, 0.184, 0.064, 0.256, -0.088, 0.024, 0.344, -0.16, -0.056, 0.112, 0.176, 0.448, -0.104, 0.264, -0.072, + -0.248, -0.128, 0.456, 0.352, -0.0960, -0.1360}; + delta_cross_track_expected = {0.104, 0.192, 0.312, 0.104, 0.024, 0.392, 0.352, -0.048, -0.12, -0.032, -0.152, -0.328, -0.088, 0.16, 0.464, 0.256, + -0.064, 0.104, -0.128, 0.28, -0.104, -0.288}; + for (size_t i = 0; i < has_message->get_delta_radial_m(1).size(); i++) + { + EXPECT_FLOAT_EQ(has_message->get_delta_radial_m(1)[i], delta_radial_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_delta_in_track_m(1)[i], delta_in_track_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_delta_cross_track_m(1)[i], delta_cross_track_expected[i]); + } + + std::vector code_bias_gps_expected = {-3.76, 4.38, -2.82, -0.60, 1.28, -3.46, 1.48, -3.04, -2.04, -2.64, 1.82, 1.60, 0.36, 1.08, 2.04, 1.44, + 0.42, 4.10, 1.92, 2.28, 4.86, 0.46, -2.94, -3.04, -3.76, -2.08, 2.20, 1.12, -2.58, 1.94, -2.44}; + for (size_t i = 0; i < has_message->get_PRNs_in_mask(0).size(); i++) + { + EXPECT_FLOAT_EQ(has_message->get_code_bias_m()[i][0], code_bias_gps_expected[i]); + } + std::vector code_bias_galileo_e1_expected = {-0.08, 0.56, -1.04, 1.04, -1.22, -1.68, 1.3, -0.54, 4.38, 3.04, -0.06, -2.14, 1.16, -0.74, 1.34, + -1.22, -2.02, -0.7, -0.58, -1.6, 1.12, -1.98}; + std::vector code_bias_galileo_e5a_expected = {-0.12, 0.8, -1.76, 1.80, -2.04, -2.9, 2.18, -0.94, 7.6, 5.64, -0.2, -3.7, 1.96, -1.28, 2.18, + -2.08, -3.44, -1.20, -1.04, -2.72, 1.92, -3.36}; + std::vector code_bias_galileo_e5b_expected = {-0.12, 0.94, -1.76, 1.78, -2.06, -2.88, 2.22, -0.92, 7.46, 5.20, -0.12, -3.64, 1.96, -1.26, 2.28, + -2.06, -3.46, -1.20, -0.98, -2.7, 1.9, -3.38}; + std::vector code_bias_galileo_e6c_expected = {-1.04, -0.16, -0.94, 0.96, -0.28, -1.42, -0.58, -0.86, 7.18, 5.72, -1.26, -2.48, 1.4, -2.00, 1.10, + -0.66, -2.72, -2.0, -0.22, -0.58, 0.58, -2.26}; + + for (size_t i = 0; i < has_message->get_PRNs_in_mask(1).size(); i++) + { + EXPECT_FLOAT_EQ(has_message->get_code_bias_m()[has_message->get_PRNs_in_mask(0).size() + i][0], code_bias_galileo_e1_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_code_bias_m()[has_message->get_PRNs_in_mask(0).size() + i][1], code_bias_galileo_e5a_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_code_bias_m()[has_message->get_PRNs_in_mask(0).size() + i][2], code_bias_galileo_e5b_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_code_bias_m()[has_message->get_PRNs_in_mask(0).size() + i][3], code_bias_galileo_e6c_expected[i]); + } + + std::vector phase_bias_gps_expected(has_message->get_PRNs_in_mask(0).size(), -10.24); + std::vector phase_bias_galileo_e1_expected(has_message->get_PRNs_in_mask(1).size(), -10.24); + std::vector phase_bias_galileo_e5a_expected(has_message->get_PRNs_in_mask(1).size(), -10.24); + std::vector phase_bias_galileo_e5b_expected(has_message->get_PRNs_in_mask(1).size(), -10.24); + std::vector phase_bias_galileo_e6c_expected(has_message->get_PRNs_in_mask(1).size(), -10.24); + std::vector phase_discontinuity_indicator_expected_gps(has_message->get_PRNs_in_mask(0).size(), 0); + std::vector phase_discontinuity_indicator_expected_gal(has_message->get_PRNs_in_mask(1).size(), 0); + for (size_t i = 0; i < has_message->get_PRNs_in_mask(0).size(); i++) + { + EXPECT_FLOAT_EQ(has_message->get_phase_bias_cycle()[i][0], phase_bias_gps_expected[i]); + EXPECT_EQ(has_message->phase_discontinuity_indicator[i][0], phase_discontinuity_indicator_expected_gps[i]); + } + for (size_t i = 0; i < has_message->get_PRNs_in_mask(1).size(); i++) + { + EXPECT_FLOAT_EQ(has_message->get_phase_bias_cycle()[has_message->get_PRNs_in_mask(0).size() + i][0], phase_bias_galileo_e1_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_phase_bias_cycle()[has_message->get_PRNs_in_mask(0).size() + i][1], phase_bias_galileo_e5a_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_phase_bias_cycle()[has_message->get_PRNs_in_mask(0).size() + i][2], phase_bias_galileo_e5b_expected[i]); + EXPECT_FLOAT_EQ(has_message->get_phase_bias_cycle()[has_message->get_PRNs_in_mask(0).size() + i][3], phase_bias_galileo_e6c_expected[i]); + EXPECT_EQ(has_message->phase_discontinuity_indicator[has_message->get_PRNs_in_mask(0).size() + i][0], phase_discontinuity_indicator_expected_gal[i]); + EXPECT_EQ(has_message->phase_discontinuity_indicator[has_message->get_PRNs_in_mask(0).size() + i][1], phase_discontinuity_indicator_expected_gal[i]); + EXPECT_EQ(has_message->phase_discontinuity_indicator[has_message->get_PRNs_in_mask(0).size() + i][2], phase_discontinuity_indicator_expected_gal[i]); + EXPECT_EQ(has_message->phase_discontinuity_indicator[has_message->get_PRNs_in_mask(0).size() + i][3], phase_discontinuity_indicator_expected_gal[i]); + } + } + else + { + has_simple_printer->print_message(has_message.get()); + } + } + } + } +}