From c6ca9f2d2c7ea325fa02419833079b310e63271c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 7 May 2019 08:34:10 +0200 Subject: [PATCH 1/6] Fix display_rate_ms parameter --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 14951701f..292a137d7 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1453,10 +1453,10 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item //end debug if (d_display_rate_ms != 0) { - // if (current_RX_time_ms % d_display_rate_ms == 0) - // { - flag_display_pvt = true; - // } + if (current_RX_time_ms % d_display_rate_ms == 0) + { + flag_display_pvt = true; + } } if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { From 950b4b607a9fe8cfe166a1899085e47f85f911fe Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 7 May 2019 16:39:06 +0200 Subject: [PATCH 2/6] Do not repeat annotations in RINEX navigation files --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 1 - .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 710 +++++++++++------- src/algorithms/PVT/libs/pvt_conf.cc | 1 - src/algorithms/PVT/libs/pvt_conf.h | 1 - 4 files changed, 445 insertions(+), 268 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index ac8c18f48..5eccb86f9 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -106,7 +106,6 @@ Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration, pvt_output_parameters.rinex_version = 2; } pvt_output_parameters.rinexobs_rate_ms = bc::lcm(configuration->property(role + ".rinexobs_rate_ms", 1000), pvt_output_parameters.output_rate_ms); - pvt_output_parameters.rinexnav_rate_ms = bc::lcm(configuration->property(role + ".rinexnav_rate_ms", 6000), pvt_output_parameters.output_rate_ms); // RTCM Printer settings pvt_output_parameters.flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 292a137d7..b74741c1a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -299,7 +299,6 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, rp = nullptr; } d_rinexobs_rate_ms = conf_.rinexobs_rate_ms; - d_rinexnav_rate_ms = conf_.rinexnav_rate_ms; // XML printer d_xml_storage = conf_.xml_output_enabled; @@ -973,6 +972,69 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) << "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 + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data + { + bool new_annotation = false; + if (d_pvt_solver->gps_ephemeris_map.find(gps_eph->i_satellite_PRN) == d_pvt_solver->gps_ephemeris_map.cend()) + { + new_annotation = true; + } + else + { + if (d_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN].d_Toc != gps_eph->d_Toc) + { + new_annotation = true; + } + } + if (new_annotation == true) + { + // New record! + std::map new_eph; + std::map new_gal_eph; + std::map new_glo_eph; + new_eph[gps_eph->i_satellite_PRN] = *gps_eph; + switch (type_of_rx) + { + case 1: // GPS L1 C/A only + rp->log_rinex_nav(rp->navFile, new_eph); + break; + case 8: // L1+L5 + rp->log_rinex_nav(rp->navFile, new_eph); + break; + case 11: // GPS L1 C/A + Galileo E5b + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + case 26: // GPS L1 C/A + GLONASS L1 C/A + if (d_rinex_version == 3) + { + rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); + } + if (d_rinex_version == 2) + { + rp->log_rinex_nav(rp->navFile, new_glo_eph); + } + break; + case 29: // GPS L1 C/A + GLONASS L2 C/A + if (d_rinex_version == 3) + { + rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); + } + if (d_rinex_version == 2) + { + rp->log_rinex_nav(rp->navFile, new_eph); + } + break; + case 32: // L1+E1+L5+E5a + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + case 33: // L1+E1+E5a + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + default: + break; + } + } + } d_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; } else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) @@ -997,171 +1059,362 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) 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) + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data { - d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1; + bool new_annotation = false; + if (d_pvt_solver->gps_cnav_ephemeris_map.find(gps_cnav_ephemeris->i_satellite_PRN) == d_pvt_solver->gps_cnav_ephemeris_map.cend()) + { + new_annotation = true; + } + else + { + if (d_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN].d_Toc != gps_cnav_ephemeris->d_Toc) + { + new_annotation = true; + } + } + if (new_annotation == true) + { + // New record! + std::map new_gal_eph; + std::map new_cnav_eph; + std::map new_glo_eph; + new_cnav_eph[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; + switch (type_of_rx) + { + case 2: // GPS L2C only + rp->log_rinex_nav(rp->navFile, new_cnav_eph); + break; + case 3: // GPS L5 only + rp->log_rinex_nav(rp->navFile, new_cnav_eph); + break; + case 7: // GPS L1 C/A + GPS L2C + rp->log_rinex_nav(rp->navFile, new_cnav_eph); + break; + case 13: // L5+E5a + rp->log_rinex_nav(rp->navFile, new_cnav_eph, new_gal_eph); + break; + case 28: // GPS L2C + GLONASS L1 C/A + rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); + break; + case 31: // GPS L2C + GLONASS L2 C/A + rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); + break; + default: + break; + } + } + 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 "; } - if (sv2.i_satellite_PRN != 0) + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) { - d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2; + // ### 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 "; } - if (sv3.i_satellite_PRN != 0) + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr)) { - d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3; + // ### 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 "; } - 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; - } + 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 "; + } - // ************* 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!"; + // **************** 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 + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data + { + bool new_annotation = false; + if (d_pvt_solver->galileo_ephemeris_map.find(galileo_eph->i_satellite_PRN) == d_pvt_solver->galileo_ephemeris_map.cend()) + { + new_annotation = true; + } + else + { + if (d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN].t0e_1 != galileo_eph->t0e_1) + { + new_annotation = true; + } + } + if (new_annotation == true) + { + // New record! + std::map new_gal_eph; + std::map new_cnav_eph; + std::map new_eph; + std::map new_glo_eph; + new_gal_eph[galileo_eph->i_satellite_PRN] = *galileo_eph; + switch (type_of_rx) + { + case 6: // Galileo E5b only + rp->log_rinex_nav(rp->navGalFile, new_gal_eph); + break; + case 11: // GPS L1 C/A + Galileo E5b + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + case 13: // L5+E5a + rp->log_rinex_nav(rp->navFile, new_cnav_eph, new_gal_eph); + break; + case 15: // Galileo E1B + Galileo E5b + rp->log_rinex_nav(rp->navGalFile, new_gal_eph); + break; + case 27: // Galileo E1B + GLONASS L1 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 30: // Galileo E1B + GLONASS L2 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 32: // L1+E1+L5+E5a + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + case 33: // L1+E1+E5a + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + } + } + } + + 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 + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data + { + bool new_annotation = false; + if (d_pvt_solver->glonass_gnav_ephemeris_map.find(glonass_gnav_eph->i_satellite_PRN) == d_pvt_solver->glonass_gnav_ephemeris_map.cend()) + { + new_annotation = true; + } + else + { + if (d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN].d_t_b != glonass_gnav_eph->d_t_b) + { + new_annotation = true; + } + } + if (new_annotation == true) + { + // New record! + std::map new_gal_eph; + std::map new_cnav_eph; + std::map new_eph; + std::map new_glo_eph; + new_glo_eph[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; + switch (type_of_rx) + { + case 25: // GLONASS L1 C/A + GLONASS L2 C/A + rp->log_rinex_nav(rp->navGloFile, new_glo_eph); + break; + case 26: // GPS L1 C/A + GLONASS L1 C/A + if (d_rinex_version == 3) + { + rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); + } + if (d_rinex_version == 2) + { + rp->log_rinex_nav(rp->navGloFile, new_glo_eph); + } + break; + case 27: // Galileo E1B + GLONASS L1 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 28: // GPS L2C + GLONASS L1 C/A + rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); + break; + case 29: // GPS L1 C/A + GLONASS L2 C/A + if (d_rinex_version == 3) + { + rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); + } + if (d_rinex_version == 2) + { + rp->log_rinex_nav(rp->navGloFile, new_glo_eph); + } + break; + case 30: // Galileo E1B + GLONASS L2 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 31: // GPS L2C + GLONASS L2 C/A + rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); + break; + default: + break; + } + } + } + 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 + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data + { + bool new_annotation = false; + if (d_pvt_solver->beidou_dnav_ephemeris_map.find(bds_dnav_eph->i_satellite_PRN) == d_pvt_solver->beidou_dnav_ephemeris_map.cend()) + { + new_annotation = true; + } + else + { + if (d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN].d_Toc != bds_dnav_eph->d_Toc) + { + new_annotation = true; + } + } + if (new_annotation == true) + { + // New record! + std::map new_bds_eph; + new_bds_eph[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph; + switch (type_of_rx) + { + case 50: // BDS B1I only + rp->log_rinex_nav(rp->navFile, new_bds_eph); + break; + default: + break; + } + } + } + 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) @@ -1336,7 +1589,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item bool flag_write_RTCM_1045_output = false; bool flag_write_RTCM_MSM_output = false; bool flag_write_RINEX_obs_output = false; - bool flag_write_RINEX_nav_output = false; gnss_observables_map.clear(); const auto** in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer @@ -1506,13 +1758,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item flag_write_RINEX_obs_output = true; } } - if (d_rinexnav_rate_ms != 0) - { - if (current_RX_time_ms % static_cast(d_rinexnav_rate_ms) == 0) - { - flag_write_RINEX_nav_output = true; - } - } if (first_fix == true) { @@ -1553,17 +1798,17 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } /* - * TYPE | RECEIVER - * 0 | Unknown - * 1 | GPS L1 C/A - * 2 | GPS L2C - * 3 | GPS L5 - * 4 | Galileo E1B - * 5 | Galileo E5a - * 6 | Galileo E5b - * 7 | GPS L1 C/A + GPS L2C - * 8 | GPS L1 C/A + GPS L5 - * 9 | GPS L1 C/A + Galileo E1B + * TYPE | RECEIVER + * 0 | Unknown + * 1 | GPS L1 C/A + * 2 | GPS L2C + * 3 | GPS L5 + * 4 | Galileo E1B + * 5 | Galileo E5a + * 6 | Galileo E5b + * 7 | GPS L1 C/A + GPS L2C + * 8 | GPS L1 C/A + GPS L5 + * 9 | GPS L1 C/A + Galileo E1B * 10 | GPS L1 C/A + Galileo E5a * 11 | GPS L1 C/A + Galileo E5b * 12 | Galileo E1B + GPS L2C @@ -1611,6 +1856,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time); rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1620,6 +1866,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string signal("2S"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_cnav_iono, d_pvt_solver->gps_cnav_utc_model); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1629,6 +1876,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string signal("L5"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_cnav_iono, d_pvt_solver->gps_cnav_utc_model); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1655,6 +1903,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string signal("7X"); rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navGalFile, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model); + rp->log_rinex_nav(rp->navGalFile, d_pvt_solver->galileo_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1664,6 +1913,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string signal("1C 2S"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1673,6 +1923,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string signal("1C L5"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1700,6 +1951,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string gal_signal("7X"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1710,6 +1962,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string gps_signal("L5"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gps_signal, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_cnav_iono, d_pvt_solver->gps_cnav_utc_model, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1728,6 +1981,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string gal_signal("1B 7X"); rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); rp->rinex_nav_header(rp->navGalFile, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model); + rp->log_rinex_nav(rp->navGalFile, d_pvt_solver->galileo_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1755,6 +2009,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string signal("1G 2G"); rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navGloFile, d_pvt_solver->glonass_gnav_utc_model, glonass_gnav_ephemeris_iter->second); + rp->log_rinex_nav(rp->navGloFile, d_pvt_solver->glonass_gnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1766,11 +2021,14 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_rinex_version == 3) { rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); } if (d_rinex_version == 2) { rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model); rp->rinex_nav_header(rp->navGloFile, d_pvt_solver->glonass_gnav_utc_model, glonass_gnav_ephemeris_iter->second); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); + rp->log_rinex_nav(rp->navGloFile, d_pvt_solver->glonass_gnav_ephemeris_map); } b_rinex_header_written = true; // do not write header anymore } @@ -1782,6 +2040,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string gal_signal("1B"); rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->galileo_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1791,6 +2050,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string glo_signal("1G"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_cnav_iono, d_pvt_solver->gps_cnav_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_cnav_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1802,11 +2062,14 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_rinex_version == 3) { rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); } if (d_rinex_version == 2) { rp->rinex_nav_header(rp->navFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model); rp->rinex_nav_header(rp->navGloFile, d_pvt_solver->glonass_gnav_utc_model, glonass_gnav_ephemeris_iter->second); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); + rp->log_rinex_nav(rp->navGloFile, d_pvt_solver->glonass_gnav_ephemeris_map); } b_rinex_header_written = true; // do not write header anymore } @@ -1818,6 +2081,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string gal_signal("1B"); rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->galileo_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1827,6 +2091,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string glo_signal("2G"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_cnav_iono, d_pvt_solver->gps_cnav_utc_model, d_pvt_solver->glonass_gnav_utc_model, d_pvt_solver->glonass_gnav_almanac); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_cnav_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1839,6 +2104,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string gps_signal("1C L5"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gps_signal, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1849,6 +2115,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item std::string gal_signal("1B 5X"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_pvt_solver->gps_iono, d_pvt_solver->gps_utc_model, d_pvt_solver->galileo_iono, d_pvt_solver->galileo_utc_model); + rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } break; @@ -1857,6 +2124,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { rp->rinex_obs_header(rp->obsFile, beidou_dnav_ephemeris_iter->second, d_rx_time, "B1"); rp->rinex_nav_header(rp->navFile, d_pvt_solver->beidou_dnav_iono, d_pvt_solver->beidou_dnav_utc_model); + rp->log_rinex_nav(rp->navFile, d_pvt_solver->beidou_dnav_ephemeris_map); b_rinex_header_written = true; // do not write header anymore } @@ -1876,94 +2144,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } if (b_rinex_header_written) // The header is already written, we can now log the navigation message data { - if (flag_write_RINEX_nav_output) - { - switch (type_of_rx) - { - case 1: // GPS L1 C/A only - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); - break; - case 2: // GPS L2C only - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map); - break; - case 3: // GPS L5 only - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map); - break; - case 4: - case 5: - case 6: - rp->log_rinex_nav(rp->navGalFile, d_pvt_solver->galileo_ephemeris_map); - break; - case 7: // GPS L1 C/A + GPS L2C - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map); - break; - case 8: // L1+L5 - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); - break; - case 9: - case 10: - case 11: - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); - break; - case 13: // L5+E5a - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_cnav_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); - break; - case 14: - case 15: - rp->log_rinex_nav(rp->navGalFile, d_pvt_solver->galileo_ephemeris_map); - break; - case 23: - case 24: - case 25: - rp->log_rinex_nav(rp->navGloFile, d_pvt_solver->glonass_gnav_ephemeris_map); - break; - case 26: // GPS L1 C/A + GLONASS L1 C/A - if (d_rinex_version == 3) - { - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); - } - if (d_rinex_version == 2) - { - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); - rp->log_rinex_nav(rp->navGloFile, d_pvt_solver->glonass_gnav_ephemeris_map); - } - break; - case 27: // Galileo E1B + GLONASS L1 C/A - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->galileo_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); - break; - case 28: // GPS L2C + GLONASS L1 C/A - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_cnav_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); - break; - case 29: // GPS L1 C/A + GLONASS L2 C/A - if (d_rinex_version == 3) - { - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); - } - if (d_rinex_version == 2) - { - rp->log_rinex_nav(rp->navFile, d_pvt_solver->gps_ephemeris_map); - rp->log_rinex_nav(rp->navGloFile, d_pvt_solver->glonass_gnav_ephemeris_map); - } - break; - case 30: // Galileo E1B + GLONASS L2 C/A - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->galileo_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); - break; - case 31: // GPS L2C + GLONASS L2 C/A - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_cnav_ephemeris_map, d_pvt_solver->glonass_gnav_ephemeris_map); - break; - case 32: // L1+E1+L5+E5a - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); - break; - case 33: // L1+E1+E5a - rp->log_rinex_nav(rp->navMixFile, d_pvt_solver->gps_ephemeris_map, d_pvt_solver->galileo_ephemeris_map); - break; - case 50: // BDS B1I only - rp->log_rinex_nav(rp->navFile, d_pvt_solver->beidou_dnav_ephemeris_map); - break; - default: - break; - } - } galileo_ephemeris_iter = d_pvt_solver->galileo_ephemeris_map.cbegin(); gps_ephemeris_iter = d_pvt_solver->gps_ephemeris_map.cbegin(); gps_cnav_ephemeris_iter = d_pvt_solver->gps_cnav_ephemeris_map.cbegin(); diff --git a/src/algorithms/PVT/libs/pvt_conf.cc b/src/algorithms/PVT/libs/pvt_conf.cc index 5244a9592..990745632 100644 --- a/src/algorithms/PVT/libs/pvt_conf.cc +++ b/src/algorithms/PVT/libs/pvt_conf.cc @@ -38,7 +38,6 @@ Pvt_Conf::Pvt_Conf() rinex_version = 0; rinexobs_rate_ms = 0; - rinexnav_rate_ms = 0; dump = false; dump_mat = true; diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 31f3d19a0..7d6b02f89 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -44,7 +44,6 @@ public: int32_t rinex_version; int32_t rinexobs_rate_ms; - int32_t rinexnav_rate_ms; std::map rtcm_msg_rate_ms; bool dump; From 76b3b932eb740c9ffc0d6628b775ca367a82bac5 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 7 May 2019 18:52:08 +0200 Subject: [PATCH 3/6] Fix RINEX nav annotations --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 543 +++++++++--------- 1 file changed, 271 insertions(+), 272 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index b74741c1a..b0d03ae2a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1104,317 +1104,316 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) break; } } - 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 "; } + 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 "; - } + 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 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 + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data { - // ### 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 - if (b_rinex_header_written) // The header is already written, we can now log the navigation message data + bool new_annotation = false; + if (d_pvt_solver->galileo_ephemeris_map.find(galileo_eph->i_satellite_PRN) == d_pvt_solver->galileo_ephemeris_map.cend()) { - bool new_annotation = false; - if (d_pvt_solver->galileo_ephemeris_map.find(galileo_eph->i_satellite_PRN) == d_pvt_solver->galileo_ephemeris_map.cend()) + new_annotation = true; + } + else + { + if (d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN].t0e_1 != galileo_eph->t0e_1) { new_annotation = true; } - else + } + if (new_annotation == true) + { + // New record! + std::map new_gal_eph; + std::map new_cnav_eph; + std::map new_eph; + std::map new_glo_eph; + new_gal_eph[galileo_eph->i_satellite_PRN] = *galileo_eph; + switch (type_of_rx) { - if (d_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN].t0e_1 != galileo_eph->t0e_1) - { - new_annotation = true; - } - } - if (new_annotation == true) - { - // New record! - std::map new_gal_eph; - std::map new_cnav_eph; - std::map new_eph; - std::map new_glo_eph; - new_gal_eph[galileo_eph->i_satellite_PRN] = *galileo_eph; - switch (type_of_rx) - { - case 6: // Galileo E5b only - rp->log_rinex_nav(rp->navGalFile, new_gal_eph); - break; - case 11: // GPS L1 C/A + Galileo E5b - rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); - break; - case 13: // L5+E5a - rp->log_rinex_nav(rp->navFile, new_cnav_eph, new_gal_eph); - break; - case 15: // Galileo E1B + Galileo E5b - rp->log_rinex_nav(rp->navGalFile, new_gal_eph); - break; - case 27: // Galileo E1B + GLONASS L1 C/A - rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); - break; - case 30: // Galileo E1B + GLONASS L2 C/A - rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); - break; - case 32: // L1+E1+L5+E5a - rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); - break; - case 33: // L1+E1+E5a - rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); - break; - } + case 6: // Galileo E5b only + rp->log_rinex_nav(rp->navGalFile, new_gal_eph); + break; + case 11: // GPS L1 C/A + Galileo E5b + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + case 13: // L5+E5a + rp->log_rinex_nav(rp->navFile, new_cnav_eph, new_gal_eph); + break; + case 15: // Galileo E1B + Galileo E5b + rp->log_rinex_nav(rp->navGalFile, new_gal_eph); + break; + case 27: // Galileo E1B + GLONASS L1 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 30: // Galileo E1B + GLONASS L2 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 32: // L1+E1+L5+E5a + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; + case 33: // L1+E1+E5a + rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); + break; } } - - 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)); + 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); + 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) + 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 + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data + { + bool new_annotation = false; + if (d_pvt_solver->glonass_gnav_ephemeris_map.find(glonass_gnav_eph->i_satellite_PRN) == d_pvt_solver->glonass_gnav_ephemeris_map.cend()) { - d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1; + new_annotation = true; } - if (sv2.i_satellite_PRN != 0) + else { - 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 - if (b_rinex_header_written) // The header is already written, we can now log the navigation message data - { - bool new_annotation = false; - if (d_pvt_solver->glonass_gnav_ephemeris_map.find(glonass_gnav_eph->i_satellite_PRN) == d_pvt_solver->glonass_gnav_ephemeris_map.cend()) + if (d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN].d_t_b != glonass_gnav_eph->d_t_b) { new_annotation = true; } - else + } + if (new_annotation == true) + { + // New record! + std::map new_gal_eph; + std::map new_cnav_eph; + std::map new_eph; + std::map new_glo_eph; + new_glo_eph[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; + switch (type_of_rx) { - if (d_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN].d_t_b != glonass_gnav_eph->d_t_b) + case 25: // GLONASS L1 C/A + GLONASS L2 C/A + rp->log_rinex_nav(rp->navGloFile, new_glo_eph); + break; + case 26: // GPS L1 C/A + GLONASS L1 C/A + if (d_rinex_version == 3) { - new_annotation = true; + rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); } - } - if (new_annotation == true) - { - // New record! - std::map new_gal_eph; - std::map new_cnav_eph; - std::map new_eph; - std::map new_glo_eph; - new_glo_eph[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; - switch (type_of_rx) + if (d_rinex_version == 2) { - case 25: // GLONASS L1 C/A + GLONASS L2 C/A rp->log_rinex_nav(rp->navGloFile, new_glo_eph); - break; - case 26: // GPS L1 C/A + GLONASS L1 C/A - if (d_rinex_version == 3) - { - rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); - } - if (d_rinex_version == 2) - { - rp->log_rinex_nav(rp->navGloFile, new_glo_eph); - } - break; - case 27: // Galileo E1B + GLONASS L1 C/A - rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); - break; - case 28: // GPS L2C + GLONASS L1 C/A - rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); - break; - case 29: // GPS L1 C/A + GLONASS L2 C/A - if (d_rinex_version == 3) - { - rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); - } - if (d_rinex_version == 2) - { - rp->log_rinex_nav(rp->navGloFile, new_glo_eph); - } - break; - case 30: // Galileo E1B + GLONASS L2 C/A - rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); - break; - case 31: // GPS L2C + GLONASS L2 C/A - rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); - break; - default: - break; } + break; + case 27: // Galileo E1B + GLONASS L1 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 28: // GPS L2C + GLONASS L1 C/A + rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); + break; + case 29: // GPS L1 C/A + GLONASS L2 C/A + if (d_rinex_version == 3) + { + rp->log_rinex_nav(rp->navMixFile, new_eph, new_glo_eph); + } + if (d_rinex_version == 2) + { + rp->log_rinex_nav(rp->navGloFile, new_glo_eph); + } + break; + case 30: // Galileo E1B + GLONASS L2 C/A + rp->log_rinex_nav(rp->navMixFile, new_gal_eph, new_glo_eph); + break; + case 31: // GPS L2C + GLONASS L2 C/A + rp->log_rinex_nav(rp->navMixFile, new_cnav_eph, new_glo_eph); + break; + default: + break; } } - 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; } + 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 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 + if (b_rinex_header_written) // The header is already written, we can now log the navigation message data { - // ### 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 - if (b_rinex_header_written) // The header is already written, we can now log the navigation message data + bool new_annotation = false; + if (d_pvt_solver->beidou_dnav_ephemeris_map.find(bds_dnav_eph->i_satellite_PRN) == d_pvt_solver->beidou_dnav_ephemeris_map.cend()) { - bool new_annotation = false; - if (d_pvt_solver->beidou_dnav_ephemeris_map.find(bds_dnav_eph->i_satellite_PRN) == d_pvt_solver->beidou_dnav_ephemeris_map.cend()) + new_annotation = true; + } + else + { + if (d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN].d_Toc != bds_dnav_eph->d_Toc) { new_annotation = true; } - else + } + if (new_annotation == true) + { + // New record! + std::map new_bds_eph; + new_bds_eph[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph; + switch (type_of_rx) { - if (d_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN].d_Toc != bds_dnav_eph->d_Toc) - { - new_annotation = true; - } - } - if (new_annotation == true) - { - // New record! - std::map new_bds_eph; - new_bds_eph[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph; - switch (type_of_rx) - { - case 50: // BDS B1I only - rp->log_rinex_nav(rp->navFile, new_bds_eph); - break; - default: - break; - } + case 50: // BDS B1I only + rp->log_rinex_nav(rp->navFile, new_bds_eph); + break; + default: + break; } } - 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!"; } + 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) From 8c86431e9b5ee238a2f33883eb51b0fe35ec20f9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 7 May 2019 19:27:34 +0200 Subject: [PATCH 4/6] Remove parameter rinexobs_rate_ms --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 5eccb86f9..c5c7391e8 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -105,7 +105,6 @@ Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration, { pvt_output_parameters.rinex_version = 2; } - pvt_output_parameters.rinexobs_rate_ms = bc::lcm(configuration->property(role + ".rinexobs_rate_ms", 1000), pvt_output_parameters.output_rate_ms); // RTCM Printer settings pvt_output_parameters.flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); From 25d1c02aa2eb90e4849bea07b1c55c2a54ff635f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 8 May 2019 08:24:13 +0200 Subject: [PATCH 5/6] Add default case to switch --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 2 ++ src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h | 1 - 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index b0d03ae2a..059838982 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1193,6 +1193,8 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) case 33: // L1+E1+E5a rp->log_rinex_nav(rp->navMixFile, new_eph, new_gal_eph); break; + default: + break; } } } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 9744426d1..a97874da8 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -95,7 +95,6 @@ private: bool b_rinex_header_updated; double d_rinex_version; int32_t d_rinexobs_rate_ms; - int32_t d_rinexnav_rate_ms; bool b_rtcm_writing_started; bool b_rtcm_enabled; From f43fdece82935c7e111dc26db8dde4f0e5eca74f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 8 May 2019 08:56:42 +0200 Subject: [PATCH 6/6] Add rinexobs_rate_ms parameter wrongly deleted --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index c5c7391e8..5eccb86f9 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -105,6 +105,7 @@ Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration, { pvt_output_parameters.rinex_version = 2; } + pvt_output_parameters.rinexobs_rate_ms = bc::lcm(configuration->property(role + ".rinexobs_rate_ms", 1000), pvt_output_parameters.output_rate_ms); // RTCM Printer settings pvt_output_parameters.flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false);