From 802b8bc1fffae6bc3f49ddcc47511eb0b6663e10 Mon Sep 17 00:00:00 2001 From: Mathieu Favreau Date: Thu, 24 Jul 2025 12:23:56 +0000 Subject: [PATCH] Fix rtcm printer --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 26 ++++++++++--- src/algorithms/PVT/libs/rtcm_printer.cc | 39 ++++++++++++------- src/algorithms/PVT/libs/rtcm_printer.h | 10 +++-- 3 files changed, 51 insertions(+), 24 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index e79db7d8c..1b8992d71 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -2349,6 +2349,15 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // save_gnss_synchro_map_xml("./gnss_synchro_map.xml"); // getchar(); // stop the execution // end debug + + // allows deactivating messages by setting rate = 0 + const bool rtcm_MT1019_enabled = d_rtcm_MT1019_rate_ms != 0; + const bool rtcm_MT1020_enabled = d_rtcm_MT1020_rate_ms != 0; + const bool rtcm_MT1045_enabled = d_rtcm_MT1045_rate_ms != 0; + const bool rtcm_MT1077_enabled = d_rtcm_MT1077_rate_ms != 0; + const bool rtcm_MT1087_enabled = d_rtcm_MT1087_rate_ms != 0; + const bool rtcm_MT1097_enabled = d_rtcm_MT1097_rate_ms != 0; + if (d_display_rate_ms != 0) { if (current_RX_time_ms % d_display_rate_ms == 0) @@ -2356,27 +2365,28 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item flag_display_pvt = true; } } - if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 + if (rtcm_MT1019_enabled) { if (current_RX_time_ms % d_rtcm_MT1019_rate_ms == 0) { flag_write_RTCM_1019_output = true; } } - if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 + if (rtcm_MT1020_enabled) // allows deactivating messages by setting rate = 0 { if (current_RX_time_ms % d_rtcm_MT1020_rate_ms == 0) { flag_write_RTCM_1020_output = true; } } - if (d_rtcm_MT1045_rate_ms != 0) + if (rtcm_MT1045_enabled != 0) { if (current_RX_time_ms % d_rtcm_MT1045_rate_ms == 0) { flag_write_RTCM_1045_output = true; } } + // TODO: RTCM 1077, 1087 and 1097 are not used, so, disable the output rates // if (current_RX_time_ms % d_rtcm_MT1077_rate_ms==0 && d_rtcm_MT1077_rate_ms != 0) // { @@ -2390,6 +2400,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item // { // last_RTCM_1097_output_time = current_RX_time; // } + if (d_rtcm_MSM_rate_ms != 0) { if (current_RX_time_ms % d_rtcm_MSM_rate_ms == 0) @@ -2462,9 +2473,12 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item d_gnss_observables_map, d_rx_time, d_signal_enabled_flags, - d_rtcm_MT1077_rate_ms, - d_rtcm_MT1087_rate_ms, - d_rtcm_MT1097_rate_ms, + rtcm_MT1019_enabled, + rtcm_MT1020_enabled, + rtcm_MT1045_enabled, + rtcm_MT1077_enabled, + rtcm_MT1087_enabled, + rtcm_MT1097_enabled, flag_write_RTCM_MSM_output, flag_write_RTCM_1019_output, flag_write_RTCM_1020_output, diff --git a/src/algorithms/PVT/libs/rtcm_printer.cc b/src/algorithms/PVT/libs/rtcm_printer.cc index d7bd3ba3c..d20d108fd 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.cc +++ b/src/algorithms/PVT/libs/rtcm_printer.cc @@ -57,6 +57,7 @@ Rtcm_Printer::Rtcm_Printer(const std::string& filename, rtcm_devname(rtcm_dump_devname), port(rtcm_tcp_port), station_id(rtcm_station_id), + d_rtcm_has_written_once(false), d_rtcm_file_dump(flag_rtcm_file_dump) { const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time(); @@ -226,9 +227,12 @@ void Rtcm_Printer::Print_Rtcm_Messages(const Rtklib_Solver* pvt_solver, const std::map& gnss_observables_map, double rx_time, uint32_t signal_enabled_flags, - int32_t rtcm_MT1077_rate_ms, - int32_t rtcm_MT1087_rate_ms, - int32_t rtcm_MT1097_rate_ms, + bool rtcm_MT1019_enabled, + bool rtcm_MT1020_enabled, + bool rtcm_MT1045_enabled, + bool rtcm_MT1077_enabled, + bool rtcm_MT1087_enabled, + bool rtcm_MT1097_enabled, bool flag_write_RTCM_MSM_output, bool flag_write_RTCM_1019_output, bool flag_write_RTCM_1020_output, @@ -244,31 +248,35 @@ void Rtcm_Printer::Print_Rtcm_Messages(const Rtklib_Solver* pvt_solver, const auto has_beidou = flags.check_any_enabled(BDS_B1, BDS_B3); const auto only_galileo = has_galileo && !(has_gps || has_glonass || has_beidou); const auto only_glonass = has_glonass && !(has_gps || has_galileo || has_beidou); + const auto print_MT1019 = (!d_rtcm_has_written_once && rtcm_MT1019_enabled) || flag_write_RTCM_1019_output; + const auto print_MT1020 = (!d_rtcm_has_written_once && rtcm_MT1020_enabled) || flag_write_RTCM_1020_output; + const auto print_MT1045 = (!d_rtcm_has_written_once && rtcm_MT1045_enabled) || flag_write_RTCM_1045_output; + const auto Print_MSM = !d_rtcm_has_written_once || flag_write_RTCM_MSM_output; - if (flag_write_RTCM_1019_output && flags.check_any_enabled(GPS_1C)) + if (print_MT1019 && flags.check_any_enabled(GPS_1C)) { for (const auto& gps_eph_iter : pvt_solver->gps_ephemeris_map) { Print_Rtcm_MT1019(gps_eph_iter.second); } } - if (flag_write_RTCM_1020_output && has_glonass) + if (print_MT1020 && has_glonass) { for (const auto& glonass_gnav_eph_iter : pvt_solver->glonass_gnav_ephemeris_map) { Print_Rtcm_MT1020(glonass_gnav_eph_iter.second, pvt_solver->glonass_gnav_utc_model); } } - if (flag_write_RTCM_1045_output && has_galileo) + if (print_MT1045 && has_galileo) { for (const auto& gal_eph_iter : pvt_solver->galileo_ephemeris_map) { Print_Rtcm_MT1045(gal_eph_iter.second); } } - if (flag_write_RTCM_MSM_output) + if (Print_MSM) { - if (flags.check_only_enabled(GPS_1C) || flags.check_only_enabled(GPS_1C, GAL_E6)) + if (rtcm_MT1077_enabled && (flags.check_only_enabled(GPS_1C) || flags.check_only_enabled(GPS_1C, GAL_E6))) { const auto gps_eph_iter = pvt_solver->gps_ephemeris_map.cbegin(); if (gps_eph_iter != pvt_solver->gps_ephemeris_map.cend()) @@ -276,7 +284,7 @@ void Rtcm_Printer::Print_Rtcm_Messages(const Rtklib_Solver* pvt_solver, Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, rx_time, gnss_observables_map, enable_rx_clock_correction, 0, 0, false, false); } } - else if (flags.check_only_enabled(GPS_1C, GPS_2S) || flags.check_only_enabled(GPS_1C, GPS_L5)) + else if (rtcm_MT1077_enabled && (flags.check_only_enabled(GPS_1C, GPS_2S) || flags.check_only_enabled(GPS_1C, GPS_L5))) { const auto gps_eph_iter = pvt_solver->gps_ephemeris_map.cbegin(); const auto gps_cnav_eph_iter = pvt_solver->gps_cnav_ephemeris_map.cbegin(); @@ -285,7 +293,7 @@ void Rtcm_Printer::Print_Rtcm_Messages(const Rtklib_Solver* pvt_solver, Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, rx_time, gnss_observables_map, enable_rx_clock_correction, 0, 0, false, false); } } - else if (only_galileo) + else if (rtcm_MT1097_enabled && only_galileo) { const auto gal_eph_iter = pvt_solver->galileo_ephemeris_map.cbegin(); if (gal_eph_iter != pvt_solver->galileo_ephemeris_map.cend()) @@ -293,7 +301,7 @@ void Rtcm_Printer::Print_Rtcm_Messages(const Rtklib_Solver* pvt_solver, Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, rx_time, gnss_observables_map, enable_rx_clock_correction, 0, 0, false, false); } } - else if (only_glonass) + else if (rtcm_MT1087_enabled && only_glonass) { const auto glo_gnav_ephemeris_iter = pvt_solver->glonass_gnav_ephemeris_map.cbegin(); if (glo_gnav_ephemeris_iter != pvt_solver->glonass_gnav_ephemeris_map.cend()) @@ -354,24 +362,25 @@ void Rtcm_Printer::Print_Rtcm_Messages(const Rtklib_Solver* pvt_solver, } } - if (gps_eph_iter != pvt_solver->gps_ephemeris_map.cend() && rtcm_MT1077_rate_ms != 0) + if (gps_eph_iter != pvt_solver->gps_ephemeris_map.cend() && rtcm_MT1077_enabled) { Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, rx_time, gnss_observables_map, enable_rx_clock_correction, 0, 0, false, false); } - if (gps_cnav_eph_iter != pvt_solver->gps_cnav_ephemeris_map.cend() && rtcm_MT1077_rate_ms != 0) + if (gps_cnav_eph_iter != pvt_solver->gps_cnav_ephemeris_map.cend() && rtcm_MT1077_enabled) { Print_Rtcm_MSM(7, {}, gps_cnav_eph_iter->second, {}, {}, rx_time, gnss_observables_map, enable_rx_clock_correction, 0, 0, false, false); } - if (gal_eph_iter != pvt_solver->galileo_ephemeris_map.cend() && rtcm_MT1097_rate_ms != 0) + if (gal_eph_iter != pvt_solver->galileo_ephemeris_map.cend() && rtcm_MT1097_enabled) { Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, rx_time, gnss_observables_map, enable_rx_clock_correction, 0, 0, false, false); } - if (glonass_gnav_eph_iter != pvt_solver->glonass_gnav_ephemeris_map.cend() && rtcm_MT1087_rate_ms != 0) + if (glonass_gnav_eph_iter != pvt_solver->glonass_gnav_ephemeris_map.cend() && rtcm_MT1087_enabled) { Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, rx_time, gnss_observables_map, enable_rx_clock_correction, 0, 0, false, false); } } } + d_rtcm_has_written_once = true; } catch (const boost::exception& ex) { diff --git a/src/algorithms/PVT/libs/rtcm_printer.h b/src/algorithms/PVT/libs/rtcm_printer.h index 0ad27a879..4445e42be 100644 --- a/src/algorithms/PVT/libs/rtcm_printer.h +++ b/src/algorithms/PVT/libs/rtcm_printer.h @@ -73,9 +73,12 @@ public: const std::map& gnss_observables_map, double rx_time, uint32_t signal_enabled_flags, - int32_t rtcm_MT1077_rate_ms, - int32_t rtcm_MT1087_rate_ms, - int32_t rtcm_MT1097_rate_ms, + bool rtcm_MT1019_enabled, + bool rtcm_MT1020_enabled, + bool rtcm_MT1045_enabled, + bool rtcm_MT1077_enabled, + bool rtcm_MT1087_enabled, + bool rtcm_MT1097_enabled, bool flag_write_RTCM_MSM_output, bool flag_write_RTCM_1019_output, bool flag_write_RTCM_1020_output, @@ -195,6 +198,7 @@ private: int32_t rtcm_dev_descriptor; // RTCM serial device descriptor (i.e. COM port) uint16_t port; uint16_t station_id; + bool d_rtcm_has_written_once; bool d_rtcm_file_dump; };