diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index fc9b98e7d..d1ca8925f 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -44,6 +44,7 @@ #include "gps_iono.h" #include "gps_utc_model.h" #include "gpx_printer.h" +#include "has_simple_printer.h" #include "kml_printer.h" #include "monitor_ephemeris_udp_sink.h" #include "monitor_pvt.h" @@ -395,6 +396,9 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_xml_base_path = d_xml_base_path + fs::path::preferred_separator; } + // Initialize HAS simple printer + d_has_simple_printer = std::make_unique(); + d_rx_time = 0.0; d_last_status_print_seg = 0; @@ -517,7 +521,6 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_galileo_utc_model_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_galileo_almanac_helper_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_galileo_almanac_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); - d_galileo_has_message_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_glonass_gnav_ephemeris_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_glonass_gnav_utc_model_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_glonass_gnav_almanac_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); @@ -1351,10 +1354,6 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) d_user_pvt_solver->galileo_almanac_map[galileo_alm->PRN] = *galileo_alm; } } - else if (msg_type_hash_code == d_galileo_has_message_sptr_type_hash_code) - { - // Store HAS message and print its content - } // **************** GLONASS GNAV Telemetry ************************* else if (msg_type_hash_code == d_glonass_gnav_ephemeris_sptr_type_hash_code) @@ -1517,8 +1516,7 @@ void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) const if (msg_type_hash_code == d_galileo_has_data_sptr_type_hash_code) { const auto has_data = boost::any_cast>(pmt::any_ref(msg)); - // TODO: Dump HAS message - // std::cout << "HAS data received at PVT block.\n"; + d_has_simple_printer->print_message(has_data.get()); } } catch (const boost::bad_any_cast& e) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index be63fe0ff..e3fe864aa 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -57,6 +57,7 @@ class Nmea_Printer; class Pvt_Conf; class Rinex_Printer; class Rtcm_Printer; +class Has_Simple_Printer; class Rtklib_Solver; class rtklib_pvt_gs; @@ -172,6 +173,7 @@ private: std::unique_ptr d_rtcm_printer; std::unique_ptr d_udp_sink_ptr; std::unique_ptr d_eph_udp_sink_ptr; + std::unique_ptr d_has_simple_printer; std::chrono::time_point d_start; std::chrono::time_point d_end; @@ -218,7 +220,6 @@ private: size_t d_galileo_utc_model_sptr_type_hash_code; size_t d_galileo_almanac_helper_sptr_type_hash_code; size_t d_galileo_almanac_sptr_type_hash_code; - size_t d_galileo_has_message_sptr_type_hash_code; size_t d_glonass_gnav_ephemeris_sptr_type_hash_code; size_t d_glonass_gnav_utc_model_sptr_type_hash_code; size_t d_glonass_gnav_almanac_sptr_type_hash_code; diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 0219fb89f..21f49d2d2 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -21,6 +21,7 @@ set(PVT_LIB_SOURCES rtklib_solver.cc monitor_pvt_udp_sink.cc monitor_ephemeris_udp_sink.cc + has_simple_printer.cc ) set(PVT_LIB_HEADERS @@ -40,6 +41,7 @@ set(PVT_LIB_HEADERS serdes_galileo_eph.h serdes_gps_eph.h monitor_ephemeris_udp_sink.h + has_simple_printer.h ) list(SORT PVT_LIB_HEADERS) diff --git a/src/algorithms/PVT/libs/has_simple_printer.cc b/src/algorithms/PVT/libs/has_simple_printer.cc new file mode 100644 index 000000000..7b53298ce --- /dev/null +++ b/src/algorithms/PVT/libs/has_simple_printer.cc @@ -0,0 +1,327 @@ +/*! + * \file has_simple_printer.cc + * \brief Interface of a class that prints HAS messages content in a txt file. + * \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-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "has_simple_printer.h" +#include "Galileo_CNAV.h" +#include "galileo_has_data.h" +#include "gnss_sdr_filesystem.h" +#include +#include +#include +#include +#include // for tm +#include // for exception +#include // for std::setw +#include // for cout, cerr +#include // for std::stringstream + +Has_Simple_Printer::Has_Simple_Printer(const std::string& base_path, const std::string& filename, bool time_tag_name) +{ + d_data_printed = false; + d_has_base_path = base_path; + fs::path full_path(fs::current_path()); + const fs::path p(d_has_base_path); + if (!fs::exists(p)) + { + std::string new_folder; + for (const auto& folder : fs::path(d_has_base_path)) + { + new_folder += folder.string(); + errorlib::error_code ec; + if (!fs::exists(new_folder)) + { + if (!fs::create_directory(new_folder, ec)) + { + std::cerr << "Could not create the " << new_folder << " folder.\n"; + d_has_base_path = full_path.string(); + } + } + new_folder += fs::path::preferred_separator; + } + } + else + { + d_has_base_path = p.string(); + } + if (d_has_base_path != ".") + { + std::cout << "HAS Message file will be stored at " << d_has_base_path << '\n'; + } + + d_has_base_path = d_has_base_path + fs::path::preferred_separator; + + const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time(); + const tm timeinfo = boost::posix_time::to_tm(pt); + + if (time_tag_name) + { + std::stringstream strm0; + const int year = timeinfo.tm_year - 100; + strm0 << year; + const int month = timeinfo.tm_mon + 1; + if (month < 10) + { + strm0 << "0"; + } + strm0 << month; + const int day = timeinfo.tm_mday; + if (day < 10) + { + strm0 << "0"; + } + strm0 << day << "_"; + const int hour = timeinfo.tm_hour; + if (hour < 10) + { + strm0 << "0"; + } + strm0 << hour; + const int min = timeinfo.tm_min; + if (min < 10) + { + strm0 << "0"; + } + strm0 << min; + const int sec = timeinfo.tm_sec; + if (sec < 10) + { + strm0 << "0"; + } + strm0 << sec; + + d_has_filename = filename + "_" + strm0.str() + ".txt"; + } + else + { + d_has_filename = filename + ".txt"; + } + d_has_filename = d_has_base_path + d_has_filename; + d_has_file.open(d_has_filename.c_str()); +} + + +bool Has_Simple_Printer::print_message(const Galileo_HAS_data* const has_data) +{ + d_data_printed = true; + std::string indent = " "; + + if (d_has_file.is_open()) + { + d_has_file << "HAS Message Type 1 Received.\n"; + d_has_file << "----------------------------\n"; + d_has_file << indent << "MT1 Header\n"; + d_has_file << indent << "----------\n"; + d_has_file << indent << indent << "TOH [s]: " << static_cast(has_data->header.toh) << '\n'; + d_has_file << indent << indent << "Mask flag: " << static_cast(has_data->header.mask_flag) << '\n'; + d_has_file << indent << indent << "Orbit Corr. Flag: " << static_cast(has_data->header.orbit_correction_flag) << '\n'; + d_has_file << indent << indent << "Clock Full-set Flag: " << static_cast(has_data->header.clock_fullset_flag) << '\n'; + d_has_file << indent << indent << "Clock Subset Flag: " << static_cast(has_data->header.clock_subset_flag) << '\n'; + 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'; + d_has_file << '\n'; + + d_has_file << indent << "MT1 Body\n"; + d_has_file << indent << "--------\n"; + d_has_file << indent << indent << "Mask Block\n"; + d_has_file << indent << indent << "----------\n"; + d_has_file << indent << indent << "Nsys: " << static_cast(has_data->Nsys) << '\n'; + d_has_file << indent << indent << "GNSS ID: " << print_vector(has_data->gnss_id_mask) << '\n'; + d_has_file << indent << indent << "Satellite Mask: " << print_vector_binary(has_data->satellite_mask, HAS_MSG_SATELLITE_MASK_LENGTH) << '\n'; + d_has_file << indent << indent << "Signal Mask: " << print_vector_binary(has_data->signal_mask, HAS_MSG_SIGNAL_MASK_LENGTH) << '\n'; + d_has_file << indent << indent << "Cell Mask Availability Flag: " << print_vector(has_data->cell_mask_availability_flag) << '\n'; + for (uint8_t i = 0; i < has_data->Nsys; i++) + { + const std::string text("Cell Mask " + std::to_string(i) + ": "); + d_has_file << indent << indent << text; + const std::string filler(indent.length() * 2 + text.length(), ' '); + d_has_file << print_matrix(has_data->cell_mask[i], filler); + } + d_has_file << indent << indent << "Nav message: " << print_vector(has_data->nav_message) << '\n'; + + if (has_data->header.orbit_correction_flag == true) + { + d_has_file << '\n'; + d_has_file << indent << indent << "Orbit Corrections Block\n"; + d_has_file << indent << indent << "-----------------------\n"; + 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->gnss_iod, HAS_MSG_DELTA_RADIAL_SCALE_FACTOR) << '\n'; + // TODO: complete block + } + + if (has_data->header.clock_fullset_flag == true) + { + 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'; + // TODO: complete block + } + + if (has_data->header.clock_subset_flag == true) + { + d_has_file << '\n'; + 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'; + // TODO: complete block + } + + if (has_data->header.code_bias_flag == true) + { + d_has_file << '\n'; + d_has_file << indent << indent << "Code Bias Block\n"; + d_has_file << indent << indent << "---------------\n"; + d_has_file << indent << indent << "Validity interval: " << static_cast(has_data->validity_interval_index_code_bias_corrections) << '\n'; + const std::string text("Code bias [m]: "); + const std::string filler(indent.length() * 2 + text.length(), ' '); + d_has_file << indent << indent << text << print_matrix(has_data->code_bias, filler, HAS_MSG_CODE_BIAS_SCALE_FACTOR); + } + + if (has_data->header.phase_bias_flag == true) + { + d_has_file << '\n'; + d_has_file << indent << indent << "Phase Bias Block\n"; + d_has_file << indent << indent << "----------------\n"; + d_has_file << indent << indent << "Validity interval: " << static_cast(has_data->validity_interval_index_phase_bias_corrections) << '\n'; + const std::string text("Phase bias [cycles]: "); + const std::string filler(indent.length() * 2 + text.length(), ' '); + d_has_file << indent << indent << text << print_matrix(has_data->phase_bias, filler, HAS_MSG_PHASE_BIAS_SCALE_FACTOR); + const std::string text2("Phase discontinuity indicator: "); + const std::string filler2(indent.length() * 2 + text2.length(), ' '); + d_has_file << indent << indent << text2 << print_matrix(has_data->phase_discontinuity_indicator, filler2); + } + + d_has_file << "\n\n"; + return true; + } + return false; +} + + +bool Has_Simple_Printer::close_file() +{ + if (d_has_file.is_open()) + { + d_has_file.close(); + return true; + } + return false; +} + + +Has_Simple_Printer::~Has_Simple_Printer() +{ + DLOG(INFO) << "KML printer destructor called."; + try + { + close_file(); + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + } + if (!d_data_printed) + { + errorlib::error_code ec; + if (!fs::remove(fs::path(d_has_filename), ec)) + { + LOG(INFO) << "Error deleting temporary HAS Message file"; + } + } +} + + +template +std::string Has_Simple_Printer::print_vector(const std::vector& vec, float scale_factor) const +{ + std::string msg; + std::stringstream ss; + for (auto el : vec) + { + ss << static_cast(el) * scale_factor << " "; + } + msg += ss.str(); + return msg; +} + + +template +std::string Has_Simple_Printer::print_vector_binary(const std::vector& vec, size_t bit_length) const +{ + std::string msg; + std::stringstream ss; + for (auto el : vec) + { + if (bit_length == HAS_MSG_SATELLITE_MASK_LENGTH) + { + std::bitset bits(el); + ss << bits.to_string() << " "; + } + if (bit_length == HAS_MSG_SIGNAL_MASK_LENGTH) + { + std::bitset bits(el); + ss << bits.to_string() << " "; + } + } + msg += ss.str(); + return msg; +} + + +template +std::string Has_Simple_Printer::print_matrix(const std::vector>& mat, const std::string& filler, float scale_factor) const +{ + std::string msg; + std::stringstream ss; + bool first_row = true; + + if (!mat.empty()) + { + for (size_t row = 0; row < mat.size(); row++) + { + if (first_row) + { + first_row = false; + } + else + { + ss << filler; + } + for (size_t col = 0; col < mat[0].size(); col++) + { + if (scale_factor == 1) + { + ss << static_cast(mat[row][col]) << " "; + } + else + { + ss << std::setw(6) << static_cast(mat[row][col]) * scale_factor << " "; + } + } + ss << '\n'; + } + } + else + { + ss << '\n'; + } + msg += ss.str(); + return msg; +} diff --git a/src/algorithms/PVT/libs/has_simple_printer.h b/src/algorithms/PVT/libs/has_simple_printer.h new file mode 100644 index 000000000..9e3856bbf --- /dev/null +++ b/src/algorithms/PVT/libs/has_simple_printer.h @@ -0,0 +1,66 @@ +/*! + * \file has_simple_printer.h + * \brief Interface of a class that prints HAS messages content in a txt file. + * \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-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_HAS_SIMPLE_PRINTER_H +#define GNSS_SDR_HAS_SIMPLE_PRINTER_H + +#include // for size_t +#include // for std::ofstream +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Galileo_HAS_data; + +/*! + * \brief Prints PVT information to OGC KML format file (can be viewed with Google Earth) + * + * See https://www.opengeospatial.org/standards/kml + */ +class Has_Simple_Printer +{ +public: + Has_Simple_Printer(const std::string& base_path = std::string("."), const std::string& filename = std::string("HAS_Messages"), bool time_tag_name = true); + ~Has_Simple_Printer(); + bool print_message(const Galileo_HAS_data* const has_data); + bool close_file(); + +private: + template + std::string print_vector(const std::vector& vec, float scale_factor = 1) const; + + template + std::string print_vector_binary(const std::vector& vec, size_t bit_length) const; + + template + std::string print_matrix(const std::vector>& mat, const std::string& filler, float scale_factor = 1) const; + + std::ofstream d_has_file; + std::string d_has_filename; + std::string d_has_base_path; + bool d_data_printed; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_KML_PRINTER_H diff --git a/src/core/system_parameters/Galileo_CNAV.h b/src/core/system_parameters/Galileo_CNAV.h index 484c1f1b6..6d61cf010 100644 --- a/src/core/system_parameters/Galileo_CNAV.h +++ b/src/core/system_parameters/Galileo_CNAV.h @@ -81,6 +81,10 @@ 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 float HAS_MSG_DELTA_RADIAL_SCALE_FACTOR = 0.0025; +constexpr float HAS_MSG_CODE_BIAS_SCALE_FACTOR = 0.02; +constexpr float HAS_MSG_PHASE_BIAS_SCALE_FACTOR = 0.01; + constexpr uint16_t HAS_MSG_NUMBER_MAX_TOH = 3599; constexpr uint8_t HAS_MSG_GPS_SYSTEM = 0; // Table 8 ICD v1.2