diff --git a/src/algorithms/PVT/adapters/CMakeLists.txt b/src/algorithms/PVT/adapters/CMakeLists.txt index 954d057bc..538a7ddcf 100644 --- a/src/algorithms/PVT/adapters/CMakeLists.txt +++ b/src/algorithms/PVT/adapters/CMakeLists.txt @@ -31,13 +31,11 @@ add_library(pvt_adapters ${PVT_ADAPTER_SOURCES} ${PVT_ADAPTER_HEADERS}) target_link_libraries(pvt_adapters PUBLIC - Armadillo::armadillo - Gnuradio::runtime pvt_gr_blocks - pvt_libs core_system_parameters PRIVATE - Boost::serialization + gnss_sdr_flags + pvt_libs ) target_include_directories(pvt_adapters diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 61b243633..7491968a1 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -33,16 +33,12 @@ #include "configuration_interface.h" #include "gnss_sdr_flags.h" #include "pvt_conf.h" -#include -#include -#include #include #if OLD_BOOST #include namespace bc = boost::math; #else #include -#include namespace bc = boost::integer; #endif @@ -50,7 +46,7 @@ namespace bc = boost::integer; using google::LogMessage; -RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, +Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams) : role_(role), @@ -724,13 +720,13 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, } -RtklibPvt::~RtklibPvt() +Rtklib_Pvt::~Rtklib_Pvt() { rtkfree(&rtk); } -bool RtklibPvt::get_latest_PVT(double* longitude_deg, +bool Rtklib_Pvt::get_latest_PVT(double* longitude_deg, double* latitude_deg, double* height_m, double* ground_speed_kmh, @@ -746,37 +742,37 @@ bool RtklibPvt::get_latest_PVT(double* longitude_deg, } -void RtklibPvt::clear_ephemeris() +void Rtklib_Pvt::clear_ephemeris() { pvt_->clear_ephemeris(); } -std::map RtklibPvt::get_gps_ephemeris() const +std::map Rtklib_Pvt::get_gps_ephemeris() const { return pvt_->get_gps_ephemeris_map(); } -std::map RtklibPvt::get_galileo_ephemeris() const +std::map Rtklib_Pvt::get_galileo_ephemeris() const { return pvt_->get_galileo_ephemeris_map(); } -std::map RtklibPvt::get_gps_almanac() const +std::map Rtklib_Pvt::get_gps_almanac() const { return pvt_->get_gps_almanac_map(); } -std::map RtklibPvt::get_galileo_almanac() const +std::map Rtklib_Pvt::get_galileo_almanac() const { return pvt_->get_galileo_almanac_map(); } -void RtklibPvt::connect(gr::top_block_sptr top_block) +void Rtklib_Pvt::connect(gr::top_block_sptr top_block) { if (top_block) { /* top_block is not null */ @@ -786,7 +782,7 @@ void RtklibPvt::connect(gr::top_block_sptr top_block) } -void RtklibPvt::disconnect(gr::top_block_sptr top_block) +void Rtklib_Pvt::disconnect(gr::top_block_sptr top_block) { if (top_block) { /* top_block is not null */ @@ -795,13 +791,13 @@ void RtklibPvt::disconnect(gr::top_block_sptr top_block) } -gr::basic_block_sptr RtklibPvt::get_left_block() +gr::basic_block_sptr Rtklib_Pvt::get_left_block() { return pvt_; } -gr::basic_block_sptr RtklibPvt::get_right_block() +gr::basic_block_sptr Rtklib_Pvt::get_right_block() { return nullptr; // this is a sink, nothing downstream } diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index 41235471b..ace356712 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -32,8 +32,14 @@ #ifndef GNSS_SDR_RTKLIB_PVT_H_ #define GNSS_SDR_RTKLIB_PVT_H_ +#include "galileo_almanac.h" +#include "galileo_ephemeris.h" +#include "gps_almanac.h" +#include "gps_ephemeris.h" #include "pvt_interface.h" +#include "rtklib.h" #include "rtklib_pvt_cc.h" +#include #include @@ -42,15 +48,15 @@ class ConfigurationInterface; /*! * \brief This class implements a PvtInterface for the RTKLIB PVT block */ -class RtklibPvt : public PvtInterface +class Rtklib_Pvt : public PvtInterface { public: - RtklibPvt(ConfigurationInterface* configuration, + Rtklib_Pvt(ConfigurationInterface* configuration, const std::string& role, unsigned int in_streams, unsigned int out_streams); - virtual ~RtklibPvt(); + virtual ~Rtklib_Pvt(); inline std::string role() override { diff --git a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt index 6c36caa24..d59bac863 100644 --- a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt @@ -30,15 +30,18 @@ add_library(pvt_gr_blocks ${PVT_GR_BLOCKS_SOURCES} ${PVT_GR_BLOCKS_HEADERS}) target_link_libraries(pvt_gr_blocks PUBLIC - Boost::date_time - Gnuradio::runtime pvt_libs + core_system_parameters + Boost::date_time + Gnuradio::pmt + Gnuradio::runtime PRIVATE + algorithms_libs Gflags::gflags Glog::glog Boost::filesystem Boost::system - core_system_parameters + Boost::serialization ) if(ENABLE_CLANG_TIDY) @@ -50,11 +53,6 @@ if(ENABLE_CLANG_TIDY) endif() endif() -target_include_directories(pvt_gr_blocks - PUBLIC - ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs -) - if(Boost_VERSION LESS 105800) target_compile_definitions(pvt_gr_blocks PRIVATE -DOLD_BOOST=1) endif() @@ -62,5 +60,4 @@ endif() set_property(TARGET pvt_gr_blocks APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES $ - $ ) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index fcf93d12f..c41a1989d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -30,7 +30,6 @@ #include "rtklib_pvt_cc.h" #include "display.h" -#include "galileo_almanac.h" #include "galileo_almanac_helper.h" #include "gnss_sdr_create_directory.h" #include "pvt_conf.h" @@ -44,8 +43,8 @@ #include #include #include +#include #include -#include #include #if OLD_BOOST #include @@ -68,267 +67,6 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t nchannels, } -void rtklib_pvt_cc::msg_handler_telemetry(const pmt::pmt_t& msg) -{ - try - { - // ************* GPS telemetry ***************** - if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS EPHEMERIS ### - std::shared_ptr gps_eph; - gps_eph = boost::any_cast>(pmt::any_ref(msg)); - DLOG(INFO) << "Ephemeris record has arrived from SAT ID " - << gps_eph->i_satellite_PRN << " (Block " - << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" - << "inserted with Toe=" << gps_eph->d_Toe << " and GPS Week=" - << gps_eph->i_GPS_week; - // update/insert new ephemeris record to the global ephemeris map - d_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS IONO ### - std::shared_ptr gps_iono; - gps_iono = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->gps_iono = *gps_iono; - DLOG(INFO) << "New IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS UTC MODEL ### - std::shared_ptr gps_utc_model; - gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->gps_utc_model = *gps_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS CNAV message ### - std::shared_ptr gps_cnav_ephemeris; - gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map - d_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; - DLOG(INFO) << "New GPS CNAV ephemeris record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS CNAV IONO ### - std::shared_ptr gps_cnav_iono; - gps_cnav_iono = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->gps_cnav_iono = *gps_cnav_iono; - DLOG(INFO) << "New CNAV IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS CNAV UTC MODEL ### - std::shared_ptr gps_cnav_utc_model; - gps_cnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model; - DLOG(INFO) << "New CNAV UTC record has arrived "; - } - - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS ALMANAC ### - std::shared_ptr gps_almanac; - gps_almanac = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac; - DLOG(INFO) << "New GPS almanac record has arrived "; - } - - // **************** Galileo telemetry ******************** - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### Galileo EPHEMERIS ### - std::shared_ptr galileo_eph; - galileo_eph = boost::any_cast>(pmt::any_ref(msg)); - // insert new ephemeris record - DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5 - << ", GALILEO Week Number =" << galileo_eph->WN_5 - << " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris; - // update/insert new ephemeris record to the global ephemeris map - d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### Galileo IONO ### - std::shared_ptr galileo_iono; - galileo_iono = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->galileo_iono = *galileo_iono; - DLOG(INFO) << "New IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### Galileo UTC MODEL ### - std::shared_ptr galileo_utc_model; - galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->galileo_utc_model = *galileo_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### Galileo Almanac ### - std::shared_ptr galileo_almanac_helper; - galileo_almanac_helper = boost::any_cast>(pmt::any_ref(msg)); - - Galileo_Almanac sv1 = galileo_almanac_helper->get_almanac(1); - Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2); - Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3); - - if (sv1.i_satellite_PRN != 0) - { - d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1; - } - if (sv2.i_satellite_PRN != 0) - { - d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2; - } - if (sv3.i_satellite_PRN != 0) - { - d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3; - } - DLOG(INFO) << "New Galileo Almanac data have arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### Galileo Almanac ### - std::shared_ptr galileo_alm; - galileo_alm = boost::any_cast>(pmt::any_ref(msg)); - // update/insert new almanac record to the global almanac map - d_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm; - } - - // **************** GLONASS GNAV Telemetry ************************** - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GLONASS GNAV EPHEMERIS ### - std::shared_ptr glonass_gnav_eph; - glonass_gnav_eph = boost::any_cast>(pmt::any_ref(msg)); - // TODO Add GLONASS with gps week number and tow, - // insert new ephemeris record - DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->d_TOW - << ", Week Number =" << glonass_gnav_eph->d_WN - << " and Ephemeris IOD in UTC = " << glonass_gnav_eph->compute_GLONASS_time(glonass_gnav_eph->d_t_b) - << " from SV = " << glonass_gnav_eph->i_satellite_slot_number; - // update/insert new ephemeris record to the global ephemeris map - d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GLONASS GNAV UTC MODEL ### - std::shared_ptr glonass_gnav_utc_model; - glonass_gnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model; - DLOG(INFO) << "New GLONASS GNAV UTC record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GLONASS GNAV Almanac ### - std::shared_ptr glonass_gnav_almanac; - glonass_gnav_almanac = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac; - DLOG(INFO) << "New GLONASS GNAV Almanac has arrived " - << ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A; - } - - // ************* BeiDou telemetry ***************** - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### Beidou EPHEMERIS ### - std::shared_ptr bds_dnav_eph; - bds_dnav_eph = boost::any_cast>(pmt::any_ref(msg)); - DLOG(INFO) << "Ephemeris record has arrived from SAT ID " - << bds_dnav_eph->i_satellite_PRN << " (Block " - << bds_dnav_eph->satelliteBlock[bds_dnav_eph->i_satellite_PRN] << ")" - << "inserted with Toe=" << bds_dnav_eph->d_Toe << " and BDS Week=" - << bds_dnav_eph->i_BEIDOU_week; - // update/insert new ephemeris record to the global ephemeris map - d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### BeiDou IONO ### - std::shared_ptr bds_dnav_iono; - bds_dnav_iono = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->beidou_dnav_iono = *bds_dnav_iono; - DLOG(INFO) << "New BeiDou DNAV IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS UTC MODEL ### - std::shared_ptr bds_dnav_utc_model; - bds_dnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model; - DLOG(INFO) << "New BeiDou DNAV UTC record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) - { - // ### GPS ALMANAC ### - std::shared_ptr bds_dnav_almanac; - bds_dnav_almanac = boost::any_cast>(pmt::any_ref(msg)); - d_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac; - DLOG(INFO) << "New BeiDou DNAV almanac record has arrived "; - } - else - { - LOG(WARNING) << "msg_handler_telemetry unknown object type!"; - } - } - catch (boost::bad_any_cast& e) - { - LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; - } -} - - -std::map rtklib_pvt_cc::get_gps_ephemeris_map() const -{ - return d_pvt_solver->gps_ephemeris_map; -} - - -std::map rtklib_pvt_cc::get_gps_almanac_map() const -{ - return d_pvt_solver->gps_almanac_map; -} - - -std::map rtklib_pvt_cc::get_galileo_ephemeris_map() const -{ - return d_pvt_solver->galileo_ephemeris_map; -} - - -std::map rtklib_pvt_cc::get_galileo_almanac_map() const -{ - return d_pvt_solver->galileo_almanac_map; -} - - -std::map rtklib_pvt_cc::get_beidou_dnav_ephemeris_map() const -{ - return d_pvt_solver->beidou_dnav_ephemeris_map; -} - - -std::map rtklib_pvt_cc::get_beidou_dnav_almanac_map() const -{ - return d_pvt_solver->beidou_dnav_almanac_map; -} - - -void rtklib_pvt_cc::clear_ephemeris() -{ - d_pvt_solver->gps_ephemeris_map.clear(); - d_pvt_solver->gps_almanac_map.clear(); - d_pvt_solver->galileo_ephemeris_map.clear(); - d_pvt_solver->galileo_almanac_map.clear(); - d_pvt_solver->beidou_dnav_ephemeris_map.clear(); - d_pvt_solver->beidou_dnav_almanac_map.clear(); -} - - rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, const Pvt_Conf& conf_, const rtk_t& rtk) : gr::sync_block("rtklib_pvt_cc", @@ -563,9 +301,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, xml_base_path = xml_base_path + boost::filesystem::path::preferred_separator; } - d_rx_time = 0.0; - d_last_status_print_seg = 0; // PVT MONITOR @@ -1147,6 +883,267 @@ rtklib_pvt_cc::~rtklib_pvt_cc() } +void rtklib_pvt_cc::msg_handler_telemetry(const pmt::pmt_t& msg) +{ + try + { + // ************* GPS telemetry ***************** + if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GPS EPHEMERIS ### + std::shared_ptr gps_eph; + gps_eph = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "Ephemeris record has arrived from SAT ID " + << gps_eph->i_satellite_PRN << " (Block " + << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" + << "inserted with Toe=" << gps_eph->d_Toe << " and GPS Week=" + << gps_eph->i_GPS_week; + // update/insert new ephemeris record to the global ephemeris map + d_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GPS IONO ### + std::shared_ptr gps_iono; + gps_iono = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->gps_iono = *gps_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GPS UTC MODEL ### + std::shared_ptr gps_utc_model; + gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->gps_utc_model = *gps_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GPS CNAV message ### + std::shared_ptr gps_cnav_ephemeris; + gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; + DLOG(INFO) << "New GPS CNAV ephemeris record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GPS CNAV IONO ### + std::shared_ptr gps_cnav_iono; + gps_cnav_iono = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->gps_cnav_iono = *gps_cnav_iono; + DLOG(INFO) << "New CNAV IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GPS CNAV UTC MODEL ### + std::shared_ptr gps_cnav_utc_model; + gps_cnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model; + DLOG(INFO) << "New CNAV UTC record has arrived "; + } + + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GPS ALMANAC ### + std::shared_ptr gps_almanac; + gps_almanac = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac; + DLOG(INFO) << "New GPS almanac record has arrived "; + } + + // **************** Galileo telemetry ******************** + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### Galileo EPHEMERIS ### + std::shared_ptr galileo_eph; + galileo_eph = boost::any_cast>(pmt::any_ref(msg)); + // insert new ephemeris record + DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5 + << ", GALILEO Week Number =" << galileo_eph->WN_5 + << " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris; + // update/insert new ephemeris record to the global ephemeris map + d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### Galileo IONO ### + std::shared_ptr galileo_iono; + galileo_iono = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->galileo_iono = *galileo_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### Galileo UTC MODEL ### + std::shared_ptr galileo_utc_model; + galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->galileo_utc_model = *galileo_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### Galileo Almanac ### + std::shared_ptr galileo_almanac_helper; + galileo_almanac_helper = boost::any_cast>(pmt::any_ref(msg)); + + Galileo_Almanac sv1 = galileo_almanac_helper->get_almanac(1); + Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2); + Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3); + + if (sv1.i_satellite_PRN != 0) + { + d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1; + } + if (sv2.i_satellite_PRN != 0) + { + d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2; + } + if (sv3.i_satellite_PRN != 0) + { + d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3; + } + DLOG(INFO) << "New Galileo Almanac data have arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### Galileo Almanac ### + std::shared_ptr galileo_alm; + galileo_alm = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new almanac record to the global almanac map + d_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm; + } + + // **************** GLONASS GNAV Telemetry ************************** + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GLONASS GNAV EPHEMERIS ### + std::shared_ptr glonass_gnav_eph; + glonass_gnav_eph = boost::any_cast>(pmt::any_ref(msg)); + // TODO Add GLONASS with gps week number and tow, + // insert new ephemeris record + DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->d_TOW + << ", Week Number =" << glonass_gnav_eph->d_WN + << " and Ephemeris IOD in UTC = " << glonass_gnav_eph->compute_GLONASS_time(glonass_gnav_eph->d_t_b) + << " from SV = " << glonass_gnav_eph->i_satellite_slot_number; + // update/insert new ephemeris record to the global ephemeris map + d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GLONASS GNAV UTC MODEL ### + std::shared_ptr glonass_gnav_utc_model; + glonass_gnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model; + DLOG(INFO) << "New GLONASS GNAV UTC record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### GLONASS GNAV Almanac ### + std::shared_ptr glonass_gnav_almanac; + glonass_gnav_almanac = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac; + DLOG(INFO) << "New GLONASS GNAV Almanac has arrived " + << ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A; + } + + // ************* BeiDou telemetry ***************** + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### Beidou EPHEMERIS ### + std::shared_ptr bds_dnav_eph; + bds_dnav_eph = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "Ephemeris record has arrived from SAT ID " + << bds_dnav_eph->i_satellite_PRN << " (Block " + << bds_dnav_eph->satelliteBlock[bds_dnav_eph->i_satellite_PRN] << ")" + << "inserted with Toe=" << bds_dnav_eph->d_Toe << " and BDS Week=" + << bds_dnav_eph->i_BEIDOU_week; + // update/insert new ephemeris record to the global ephemeris map + d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### BeiDou IONO ### + std::shared_ptr bds_dnav_iono; + bds_dnav_iono = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->beidou_dnav_iono = *bds_dnav_iono; + DLOG(INFO) << "New BeiDou DNAV IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### BeiDou UTC MODEL ### + std::shared_ptr bds_dnav_utc_model; + bds_dnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model; + DLOG(INFO) << "New BeiDou DNAV UTC record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) + { + // ### BeiDou ALMANAC ### + std::shared_ptr bds_dnav_almanac; + bds_dnav_almanac = boost::any_cast>(pmt::any_ref(msg)); + d_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac; + DLOG(INFO) << "New BeiDou DNAV almanac record has arrived "; + } + else + { + LOG(WARNING) << "msg_handler_telemetry unknown object type!"; + } + } + catch (boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + } +} + + +std::map rtklib_pvt_cc::get_gps_ephemeris_map() const +{ + return d_pvt_solver->gps_ephemeris_map; +} + + +std::map rtklib_pvt_cc::get_gps_almanac_map() const +{ + return d_pvt_solver->gps_almanac_map; +} + + +std::map rtklib_pvt_cc::get_galileo_ephemeris_map() const +{ + return d_pvt_solver->galileo_ephemeris_map; +} + + +std::map rtklib_pvt_cc::get_galileo_almanac_map() const +{ + return d_pvt_solver->galileo_almanac_map; +} + + +std::map rtklib_pvt_cc::get_beidou_dnav_ephemeris_map() const +{ + return d_pvt_solver->beidou_dnav_ephemeris_map; +} + + +std::map rtklib_pvt_cc::get_beidou_dnav_almanac_map() const +{ + return d_pvt_solver->beidou_dnav_almanac_map; +} + + +void rtklib_pvt_cc::clear_ephemeris() +{ + d_pvt_solver->gps_ephemeris_map.clear(); + d_pvt_solver->gps_almanac_map.clear(); + d_pvt_solver->galileo_ephemeris_map.clear(); + d_pvt_solver->galileo_almanac_map.clear(); + d_pvt_solver->beidou_dnav_ephemeris_map.clear(); + d_pvt_solver->beidou_dnav_almanac_map.clear(); +} + + bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair& a, const std::pair& b) { return (a.second.Pseudorange_m) < (b.second.Pseudorange_m); @@ -1215,7 +1212,7 @@ bool rtklib_pvt_cc::load_gnss_synchro_map_xml(const std::string& file_name) } -std::vector rtklib_pvt_cc::split_string(const std::string& s, char delim) +std::vector rtklib_pvt_cc::split_string(const std::string& s, char delim) const { std::vector v; std::stringstream ss(s); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 23ecb0206..dbe2fe3eb 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -31,7 +31,13 @@ #ifndef GNSS_SDR_RTKLIB_PVT_CC_H #define GNSS_SDR_RTKLIB_PVT_CC_H +#include "beidou_dnav_almanac.h" +#include "beidou_dnav_ephemeris.h" +#include "galileo_almanac.h" +#include "galileo_ephemeris.h" #include "geojson_printer.h" +#include "gnss_synchro.h" +#include "gps_almanac.h" #include "gps_ephemeris.h" #include "gpx_printer.h" #include "kml_printer.h" @@ -44,14 +50,17 @@ #include #include #include +#include #include #include -#include +#include +#include #include #include #include #include #include +#include class rtklib_pvt_cc; @@ -146,7 +155,7 @@ private: bool flag_monitor_pvt_enabled; std::unique_ptr udp_sink_ptr; - std::vector split_string(const std::string& s, char delim); + std::vector split_string(const std::string& s, char delim) const; public: rtklib_pvt_cc(uint32_t nchannels, diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 55ba3ac03..f4305a75a 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -60,7 +60,6 @@ #include #include #include -#include #include #include #include diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index a833bfefe..4d3b9f315 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -352,7 +352,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ corr_value += d_preambles_symbols[i]; } } - i += 1; + i++; } } @@ -480,6 +480,5 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ return 1; } - return 0; } diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index dce01a732..a87e5bbba 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -1802,7 +1802,7 @@ std::unique_ptr GNSSBlockFactory::GetBlock( // PVT ------------------------------------------------------------------------- else if ((implementation == "RTKLIB_PVT") || (implementation == "GPS_L1_CA_PVT") || (implementation == "Galileo_E1_PVT") || (implementation == "Hybrid_PVT")) { - std::unique_ptr block_(new RtklibPvt(configuration.get(), role, in_streams, + std::unique_ptr block_(new Rtklib_Pvt(configuration.get(), role, in_streams, out_streams)); block = std::move(block_); }