diff --git a/README.md b/README.md index d64cf0de0..d2c5c2ddf 100644 --- a/README.md +++ b/README.md @@ -23,16 +23,20 @@ position fixes) the following Global Navigation Satellite System's signals: In the L1 band: -- 🛰 GLONASS L1 C/A (centered at 1602.00 MHz) :white_check_mark: -- 🛰 GPS L1 C/A (centered at 1575.42 MHz) :white_check_mark: -- 🛰 Galileo E1b/c (centered at 1575.42 MHz) :white_check_mark: +- 🛰 GLONASS L1 C/A (centered at 1602.000 MHz) :white_check_mark: +- 🛰 GPS L1 C/A (centered at 1575.420 MHz) :white_check_mark: +- 🛰 Galileo E1b/c (centered at 1575.420 MHz) :white_check_mark: - 🛰 BeiDou B1I (centered at 1561.098 MHz) :white_check_mark: +In the E6 band: + +- 🛰 Galileo E6B (centered at 1278.750 MHz) :white_check_mark: + In the L2 band: - 🛰 BeiDou B3I (centered at 1268.520 MHz) :white_check_mark: -- 🛰 GLONASS L2 C/A (centered at 1246.00 MHz) :white_check_mark: -- 🛰 GPS L2C (centered at 1227.60 MHz) :white_check_mark: +- 🛰 GLONASS L2 C/A (centered at 1246.000 MHz) :white_check_mark: +- 🛰 GPS L2C (centered at 1227.600 MHz) :white_check_mark: In the L5 band: @@ -1664,6 +1668,7 @@ identifiers: | Galileo E1b/c | 1B | | Glonass L1 C/A | 1G | | Beidou B1I | B1 | +| Galileo E6B | E6 | | Beidou B3I | B3 | | GPS L2 L2C(M) | 2S | | Glonass L2 C/A | 2G | diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index b1567b209..285d2b9b4 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -14,6 +14,22 @@ All notable changes to GNSS-SDR will be documented in this file. ## [Unreleased](https://github.com/gnss-sdr/gnss-sdr/tree/next) +### Improvements in Interoperability: + +- Enabled PVT computation in the Galileo E5a + E5b receiver. Observables + reported in the RINEX file. +- Fixed PVT computation in the Galileo E5b-only receiver. +- Get E6B observables and PVT solutions in the Galileo E1B + E6B receiver. + Decoding of HAS messages as described in the + [HAS SIS ICD v1.0](https://www.gsc-europa.eu/sites/default/files/sites/all/files/Galileo_HAS_SIS_ICD_v1.0.pdf). + Generation of RTCM 3.2 messages from the received HAS messages in the + [IGS State Space Representation (SSR) Format](https://files.igs.org/pub/data/format/igs_ssr_v1.pdf). + Specifically, it generates messages of type IGM01 (SSR Orbit Correction), + IGM02 (SSR Clock Correction), IGM03 (SSR Combined Orbit and Clock Correction), + and IGM05 (SSR Code Bias). Please note that the content of the HAS messages is + **not** applied to the computed PVT solution. In the Galileo E6B-only + receiver, HAS messages are decoded and reported. + ### Improvements in Portability: - Improved detection of the BLAS library under macOS / Macports (the `lapack` diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 08e388299..aef8a6e58 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -489,28 +489,29 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, int num_bands = 0; - if ((gps_1C_count > 0) || (gal_1B_count > 0) || (gal_E6_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0)) + if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0)) { - num_bands = 1; + num_bands += 1; } - if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0)) && ((gps_2S_count > 0) || (glo_2G_count > 0) || (bds_B3_count > 0))) + if ((gps_2S_count > 0) || (glo_2G_count > 0) || (bds_B3_count > 0)) { - num_bands = 2; + num_bands += 1; } - if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0) || (gps_L5_count > 0))) + if (gal_E6_count > 0) { - num_bands = 2; + num_bands += 1; } - if ((gal_1B_count > 0) && (gal_E6_count > 0)) + if ((gal_E5a_count > 0) || (gps_L5_count > 0)) { - num_bands = 2; + num_bands += 1; } - if ((gal_1B_count > 0) && (gal_E6_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0))) + if (gal_E5b_count > 0) { - num_bands = 3; + num_bands += 1; } - if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0)) && ((gps_2S_count > 0) || (glo_2G_count > 0) || (bds_B3_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0) || (gps_L5_count > 0))) + if (num_bands > 3) { + LOG(WARNING) << "Too much bands: The PVT engine can only handle 3 bands, but " << num_bands << " were set"; num_bands = 3; } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 1c46f3d92..add1f38ed 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -524,21 +524,21 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, { // setup two PVT solvers: internal solver for rx clock and user solver // user PVT solver - d_user_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_dump, d_dump_mat); + d_user_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat); d_user_pvt_solver->set_averaging_depth(1); d_user_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); // internal PVT solver, mainly used to estimate the receiver clock rtk_t internal_rtk = rtk; internal_rtk.opt.mode = PMODE_SINGLE; // use single positioning mode in internal PVT solver - d_internal_pvt_solver = std::make_shared(internal_rtk, dump_ls_pvt_filename, false, false); + d_internal_pvt_solver = std::make_shared(internal_rtk, dump_ls_pvt_filename, d_type_of_rx, false, false); d_internal_pvt_solver->set_averaging_depth(1); d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); } else { // only one solver, customized by the user options - d_internal_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_dump, d_dump_mat); + d_internal_pvt_solver = std::make_shared(rtk, dump_ls_pvt_filename, d_type_of_rx, d_dump, d_dump_mat); d_internal_pvt_solver->set_averaging_depth(1); d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); d_user_pvt_solver = d_internal_pvt_solver; @@ -2146,7 +2146,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } while (fabs(delta_rxtime_to_tag_ms) >= 100 and !d_TimeChannelTagTimestamps.empty()); - // 2. If both timestamps (relative to the receiver's start) are closer than 100 ms (the granularituy of the PVT) if (fabs(delta_rxtime_to_tag_ms) <= 100) // [ms] { @@ -2222,11 +2221,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } } } - // debug code - // else - // { - // DLOG(INFO) << "Internal PVT solver error"; - // } // compute on the fly PVT solution if (flag_compute_pvt_output == true) @@ -2234,7 +2228,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false); } - if (flag_pvt_valid == true) { // experimental VTL tests diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 8aaa838e2..9038e8254 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -11762,33 +11762,6 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep gs.PRN = prn_; total_map.insert(std::pair(prn_, gs)); } - if (found_E5a != std::string::npos) - { - Gnss_Synchro gs = Gnss_Synchro(); - gs.System = 'E'; - gs.Signal[0] = '5'; - gs.Signal[1] = 'X'; - gs.Signal[2] = '\0'; - gs.PRN = prn_; - total_map.insert(std::pair(prn_, gs)); - } - } - else - { - // if 5X is listed but empty - if (found_E5a != std::string::npos) - { - if ((total_map.count(prn_)) == 1) - { - Gnss_Synchro gs = Gnss_Synchro(); - gs.System = 'E'; - gs.Signal[0] = '5'; - gs.Signal[1] = 'X'; - gs.Signal[2] = '\0'; - gs.PRN = prn_; - total_map.insert(std::pair(prn_, gs)); - } - } } total_map.insert(std::pair(prn_, observables_iter->second)); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index cc0f96ac4..bfd3a0075 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -45,14 +45,99 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, const std::string &dump_filename, + uint32_t type_of_rx, bool flag_dump_to_file, - bool flag_dump_to_mat) : d_rtk(rtk), - d_dump_filename(dump_filename), + bool flag_dump_to_mat) : d_dump_filename(dump_filename), + d_rtk(rtk), + d_type_of_rx(type_of_rx), d_flag_dump_enabled(flag_dump_to_file), d_flag_dump_mat_enabled(flag_dump_to_mat) { this->set_averaging_flag(false); + // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc + // function: satwavelen + d_rtklib_freq_index[0] = 0; + d_rtklib_freq_index[1] = 1; + d_rtklib_freq_index[2] = 2; + + d_rtklib_band_index["1G"] = 0; + d_rtklib_band_index["1C"] = 0; + d_rtklib_band_index["1B"] = 0; + d_rtklib_band_index["B1"] = 0; + d_rtklib_band_index["B3"] = 2; + d_rtklib_band_index["2G"] = 1; + d_rtklib_band_index["2S"] = 1; + d_rtklib_band_index["7X"] = 2; + d_rtklib_band_index["5X"] = 2; + d_rtklib_band_index["L5"] = 2; + d_rtklib_band_index["E6"] = 0; + + if (d_type_of_rx == 6) // E5b only + { + d_rtklib_freq_index[2] = 4; + } + if (d_type_of_rx == 11) // GPS L1 C/A + Galileo E5b + { + d_rtklib_freq_index[2] = 4; + } + if (d_type_of_rx == 15) // Galileo E1B + Galileo E5b + { + d_rtklib_freq_index[2] = 4; + } + if (d_type_of_rx == 18) // GPS L2C + Galileo E5b + { + d_rtklib_freq_index[2] = 4; + } + if (d_type_of_rx == 19) // Galileo E5a + Galileo E5b + { + d_rtklib_band_index["5X"] = 0; + d_rtklib_freq_index[0] = 2; + d_rtklib_freq_index[2] = 4; + } + if (d_type_of_rx == 20) // GPS L5 + Galileo E5b + { + d_rtklib_band_index["L5"] = 0; + d_rtklib_freq_index[0] = 2; + d_rtklib_freq_index[2] = 4; + } + if (d_type_of_rx == 100) // E6B only + { + d_rtklib_freq_index[0] = 3; + } + if (d_type_of_rx == 101) // E1 + E6B + { + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + } + if (d_type_of_rx == 102) // E5a + E6B + { + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + } + if (d_type_of_rx == 103) // E5b + E6B + { + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + d_rtklib_freq_index[2] = 4; + } + if (d_type_of_rx == 104) // Galileo E1B + Galileo E5a + Galileo E6B + { + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + } + if (d_type_of_rx == 105) // Galileo E1B + Galileo E5b + Galileo E6B + { + d_rtklib_freq_index[2] = 4; + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + } + if (d_type_of_rx == 106) // GPS L1 C/A + Galileo E1B + Galileo E6B + { + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + } + // ############# ENABLE DATA FILE LOG ################# if (d_flag_dump_enabled == true) { @@ -406,8 +491,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ bool gps_dual_band = false; bool band1 = false; bool band2 = false; - bool gal_e5_is_e5b = false; - bool gal_e6 = false; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); ++gnss_observables_iter) @@ -460,7 +544,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, galileo_ephemeris_iter->second.WN, - 0); + d_rtklib_band_index[sig_]); valid_obs++; } else // the ephemeris are not available for this SV @@ -484,7 +568,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], gnss_observables_iter->second, galileo_ephemeris_iter->second.WN, - 2); // Band 3 (L5/E5) + d_rtklib_band_index[sig_]); found_E1_obs = true; break; } @@ -502,7 +586,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, galileo_ephemeris_iter->second.WN, - 2); // Band 3 (L5/E5) + d_rtklib_band_index[sig_]); valid_obs++; } } @@ -510,14 +594,49 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; } - if (sig_ == "7X") + } + if (sig_ == "E6") + { + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) { - gal_e5_is_e5b = true; + bool found_E1_obs = false; + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) + { + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + found_E1_obs = true; + break; + } + } + if (!found_E1_obs) + { + // insert Galileo E6 obs as new obs and also insert its ephemeris + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + // convert observation from GNSS-SDR class to RTKLIB structure + const auto default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0, 0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; } } if (sig_ == "E6") { - gal_e6 = true; galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) { @@ -576,7 +695,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, gps_ephemeris_iter->second.WN, - 0, + d_rtklib_band_index[sig_], this->is_pre_2009()); valid_obs++; } @@ -606,7 +725,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], gnss_observables_iter->second, eph_data[i].week, - 1); // Band 2 (L2) + d_rtklib_band_index[sig_]); break; } } @@ -625,7 +744,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, gps_cnav_ephemeris_iter->second.WN, - 1); // Band 2 (L2) + d_rtklib_band_index[sig_]); valid_obs++; } } @@ -654,7 +773,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i], gnss_observables_iter->second, gps_cnav_ephemeris_iter->second.WN, - 2); // Band 3 (L5) + d_rtklib_band_index[sig_]); break; } } @@ -672,7 +791,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, gps_cnav_ephemeris_iter->second.WN, - 2); // Band 3 (L5) + d_rtklib_band_index[sig_]); valid_obs++; } } @@ -700,7 +819,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, glonass_gnav_ephemeris_iter->second.d_WN, - 0); // Band 0 (L1) + d_rtklib_band_index[sig_]); glo_valid_obs++; } else // the ephemeris are not available for this SV @@ -723,7 +842,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[i + valid_obs] = insert_obs_to_rtklib(d_obs_data[i + valid_obs], gnss_observables_iter->second, glonass_gnav_ephemeris_iter->second.d_WN, - 1); // Band 1 (L2) + d_rtklib_band_index[sig_]); found_L1_obs = true; break; } @@ -738,7 +857,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, glonass_gnav_ephemeris_iter->second.d_WN, - 1); // Band 1 (L2) + d_rtklib_band_index[sig_]); glo_valid_obs++; } } @@ -766,7 +885,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, - 0); + d_rtklib_band_index[sig_]); valid_obs++; } else // the ephemeris are not available for this SV @@ -788,7 +907,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], gnss_observables_iter->second, beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, - 2); // Band 3 (L2/G2/B3) + d_rtklib_band_index[sig_]); found_B1I_obs = true; break; } @@ -806,7 +925,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, - 2); // Band 2 (L2/G2) + d_rtklib_band_index[sig_]); valid_obs++; } } @@ -922,20 +1041,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { for (int j = 0; j < NFREQ; j++) { - if (j == 2 && gal_e6) - { - // frq = 3 corresponds to E6 in that function - nav_data.lam[i][j] = satwavelen(i + 1, 3, &nav_data); - } - if (j == 2 && gal_e5_is_e5b) - { - // frq = 4 corresponds to E5B in that function - nav_data.lam[i][j] = satwavelen(i + 1, 4, &nav_data); - } - else - { - nav_data.lam[i][j] = satwavelen(i + 1, j, &nav_data); - } + nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &nav_data); } } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 003c8bc8e..697e46687 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -58,6 +58,7 @@ #include "pvt_solution.h" #include "rtklib.h" #include +#include #include #include #include @@ -75,7 +76,11 @@ class Rtklib_Solver : public Pvt_Solution { public: - Rtklib_Solver(const rtk_t& rtk, const std::string& dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat); + Rtklib_Solver(const rtk_t& rtk, + const std::string& dump_filename, + uint32_t type_of_rx, + bool flag_dump_to_file, + bool flag_dump_to_mat); ~Rtklib_Solver(); bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); @@ -118,10 +123,13 @@ private: std::array d_obs_data{}; std::array d_dop{}; - rtk_t d_rtk{}; - Monitor_Pvt d_monitor_pvt{}; + std::map d_rtklib_freq_index; + std::map d_rtklib_band_index; std::string d_dump_filename; std::ofstream d_dump_file; + rtk_t d_rtk{}; + Monitor_Pvt d_monitor_pvt{}; + uint32_t d_type_of_rx; bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; }; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index f17255053..d687694f4 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -112,7 +112,6 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) d_mapStringValues["B2"] = evBDS_B2; d_mapStringValues["B3"] = evBDS_B3; - d_SourceTagTimestamps = std::vector>(d_nchannels_out); set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work() diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt index 91d80c257..e84f8c8f4 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt @@ -107,6 +107,13 @@ else() ) endif() +if(PMT_USES_BOOST_ANY) + target_compile_definitions(telemetry_decoder_gr_blocks + PRIVATE + -DPMT_USES_BOOST_ANY=1 + ) +endif() + if(ENABLE_CLANG_TIDY) if(CLANG_TIDY_EXE) set_target_properties(telemetry_decoder_gr_blocks diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 900ce8d78..a3739ff6e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -36,17 +36,33 @@ #include "viterbi_decoder.h" // for Viterbi_Decoder #include // for LOG, DLOG #include // for gr::io_signature::make -#include // for pmt::make_any #include // for pmt::mp #include // for std::array #include // for std::fmod, std::abs #include // for size_t #include // for std::exception #include // for std::cout +#include // for std::numeric_limits +#include // for std::map +#include // for std::out_of_range +#include // for typeid +#include // for std::pair + +#if HAS_GENERIC_LAMBDA +#else +#include +#endif + +#if PMT_USES_BOOST_ANY +#include +namespace wht = boost; +#else +#include +namespace wht = std; +#endif #define CRC_ERROR_LIMIT 6 - galileo_telemetry_decoder_gs_sptr galileo_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, const Tlm_Conf &conf, int frame_type) { @@ -63,7 +79,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_delta_t(0), d_sample_counter(0ULL), d_preamble_index(0ULL), - d_last_valid_preamble(0), + d_last_valid_preamble(0ULL), + d_received_sample_counter(0), d_frame_type(frame_type), d_CRC_error_counter(0), d_channel(0), @@ -71,6 +88,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_stat(0), d_TOW_at_Preamble_ms(0), d_TOW_at_current_symbol_ms(0), + d_received_tow_ms(std::numeric_limits::max()), d_band('1'), d_sent_tlm_failed_msg(false), d_flag_frame_sync(false), @@ -86,7 +104,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_dump_crc_stats(conf.dump_crc_stats), d_enable_reed_solomon_inav(false), d_valid_timetag(false), - d_E6_TOW_set(false) + d_E6_TOW_set(false), + d_there_are_e6_channels(conf.there_are_e6_channels) { // prevent telemetry symbols accumulation in output buffers this->set_max_noutput_items(1); @@ -94,8 +113,27 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block this->message_port_register_out(pmt::mp("telemetry_to_trk")); - // register Gal E6 messages HAS out - this->message_port_register_out(pmt::mp("E6_HAS_from_TLM")); + + if (d_there_are_e6_channels) + { + // register Gal E6 messages HAS out + this->message_port_register_out(pmt::mp("E6_HAS_from_TLM")); + // register TOW from map out + this->message_port_register_out(pmt::mp("TOW_from_TLM")); + // register TOW to TLM input + this->message_port_register_in(pmt::mp("TOW_to_TLM")); + // handler for input port + this->set_msg_handler(pmt::mp("TOW_to_TLM"), +#if HAS_GENERIC_LAMBDA + [this](auto &&PH1) { msg_handler_read_galileo_tow_map(PH1); }); +#else +#if USE_BOOST_BIND_PLACEHOLDERS + boost::bind(&galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map, this, boost::placeholders::_1)); +#else + boost::bind(&galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map, this, _1)); +#endif +#endif + } if (d_enable_navdata_monitor) { @@ -281,6 +319,36 @@ galileo_telemetry_decoder_gs::~galileo_telemetry_decoder_gs() } +void galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map(const pmt::pmt_t &msg) +{ + if (d_frame_type == 3) + { + try + { + const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); + if (msg_type_hash_code == typeid(std::shared_ptr>>).hash_code()) + { + const auto received_tow_map = wht::any_cast>>>(pmt::any_ref(msg)); + const std::pair received_tow_sample = received_tow_map->at(d_satellite.get_PRN()); + if (received_tow_sample.first < 604800000) + { + d_received_tow_ms = received_tow_sample.first; + d_received_sample_counter = received_tow_sample.second; + } + } + } + catch (const wht::bad_any_cast &e) + { + LOG(WARNING) << "msg_handler_read_galileo_tow_map Bad any_cast: " << e.what(); + } + catch (const std::out_of_range &oor) + { + LOG(WARNING) << "msg_handler_read_galileo_tow_map Out of Range error: " << oor.what(); + } + } +} + + void galileo_telemetry_decoder_gs::deinterleaver(int32_t rows, int32_t cols, const float *in, float *out) { for (int32_t r = 0; r < rows; r++) @@ -550,6 +618,12 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(uint64_t time_stamp, float * page_String.push_back('0'); } } + + if (d_enable_navdata_monitor) + { + d_nav_msg_packet.nav_message = page_String; + } + d_cnav_nav.read_HAS_page(page_String); d_cnav_nav.set_time_stamp(time_stamp); // 4. If we have a new HAS page, read it @@ -596,7 +670,15 @@ void galileo_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satellite d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); d_last_valid_preamble = d_sample_counter; d_sent_tlm_failed_msg = false; + d_received_tow_ms = std::numeric_limits::max(); d_E6_TOW_set = false; + d_valid_timetag = false; + if (d_there_are_e6_channels) + { + const std::pair tow_and_sample{d_received_tow_ms, 0ULL}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite; DLOG(INFO) << "Navigation Satellite set to " << d_satellite; } @@ -615,7 +697,15 @@ void galileo_telemetry_decoder_gs::reset() d_sent_tlm_failed_msg = false; d_E6_TOW_set = false; d_stat = 0; + d_received_tow_ms = std::numeric_limits::max(); d_viterbi->reset(); + d_valid_timetag = false; + if (d_there_are_e6_channels) + { + const std::pair tow_and_sample{d_received_tow_ms, 0ULL}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } if (d_enable_reed_solomon_inav == true) { d_inav_nav.enable_reed_solomon(); @@ -870,7 +960,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( else { d_CRC_error_counter++; - if ((d_CRC_error_counter > CRC_ERROR_LIMIT) && (d_frame_type != 3)) + if (d_CRC_error_counter > CRC_ERROR_LIMIT) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; gr::thread::scoped_lock lock(d_setlock); @@ -878,6 +968,14 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_stat = 0; d_TOW_at_current_symbol_ms = 0; d_TOW_at_Preamble_ms = 0; + d_E6_TOW_set = false; + d_valid_timetag = false; + if (d_there_are_e6_channels) + { + const std::pair tow_and_sample{std::numeric_limits::max(), 0ULL}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } d_fnav_nav.set_flag_TOW_set(false); d_inav_nav.set_flag_TOW_set(false); } @@ -904,6 +1002,12 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW5() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW5_flag(false); + if (d_there_are_e6_channels && !d_valid_timetag) + { + const std::pair tow_and_sample{d_TOW_at_current_symbol_ms, current_symbol.Tracking_sample_counter}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } // timetag debug if (d_valid_timetag == true) { @@ -924,6 +1028,12 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW6() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW6_flag(false); + if (d_there_are_e6_channels && !d_valid_timetag) + { + const std::pair tow_and_sample{d_TOW_at_current_symbol_ms, current_symbol.Tracking_sample_counter}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } // timetag debug if (d_valid_timetag == true) { @@ -943,6 +1053,12 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW0() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW0_flag(false); + if (d_there_are_e6_channels && !d_valid_timetag) + { + const std::pair tow_and_sample{d_TOW_at_current_symbol_ms, current_symbol.Tracking_sample_counter}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } // timetag debug if (d_valid_timetag == true) { @@ -982,29 +1098,49 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.get_TOW1() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); - // d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_fnav_nav.set_TOW1_flag(false); + if (d_there_are_e6_channels && !d_valid_timetag) + { + const std::pair tow_and_sample{d_TOW_at_current_symbol_ms, current_symbol.Tracking_sample_counter}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } } else if (d_fnav_nav.is_TOW2_set() == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.get_TOW2() * 1000.0); - // d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.set_TOW2_flag(false); + if (d_there_are_e6_channels && !d_valid_timetag) + { + const std::pair tow_and_sample{d_TOW_at_current_symbol_ms, current_symbol.Tracking_sample_counter}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } } else if (d_fnav_nav.is_TOW3_set() == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.get_TOW3() * 1000.0); - // d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.set_TOW3_flag(false); + if (d_there_are_e6_channels && !d_valid_timetag) + { + const std::pair tow_and_sample{d_TOW_at_current_symbol_ms, current_symbol.Tracking_sample_counter}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } } else if (d_fnav_nav.is_TOW4_set() == true) { d_TOW_at_Preamble_ms = static_cast(d_fnav_nav.get_TOW4() * 1000.0); - // d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS); d_fnav_nav.set_TOW4_flag(false); + if (d_there_are_e6_channels && !d_valid_timetag) + { + const std::pair tow_and_sample{d_TOW_at_current_symbol_ms, current_symbol.Tracking_sample_counter}; + const auto tmp_obj = std::make_shared>>(d_satellite.get_PRN(), tow_and_sample); + this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj)); + } } else { @@ -1035,7 +1171,8 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( int rx_tow_at_preamble = d_current_timetag.tow_ms; uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000); // floor to integer number of seconds d_TOW_at_Preamble_ms = predicted_tow_at_preamble_ms; - d_TOW_at_current_symbol_ms = predicted_tow_at_preamble_ms + static_cast((d_required_symbols + 1) * d_PRN_code_period_ms); + d_TOW_at_current_symbol_ms = predicted_tow_at_preamble_ms + static_cast((d_required_symbols)*d_PRN_code_period_ms); + if (d_E6_TOW_set == false) { std::cout << " Sat PRN " << d_satellite.get_PRN() << " E6 TimeTag TOW at preamble: " << predicted_tow_at_preamble_ms @@ -1043,6 +1180,27 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_E6_TOW_set = true; } } + else + { + if (d_received_tow_ms < 604800000) + { + const int64_t diff = current_symbol.Tracking_sample_counter - d_received_sample_counter; + const double time_since_reference_ms = (double(diff) * 1000.0) / static_cast(current_symbol.fs); + d_TOW_at_current_symbol_ms = d_received_tow_ms + static_cast(time_since_reference_ms) + GALILEO_E6_CODE_PERIOD_MS; + d_TOW_at_Preamble_ms = (d_TOW_at_current_symbol_ms / 1000) * 1000; + d_E6_TOW_set = true; + } + } + if (d_E6_TOW_set && d_enable_navdata_monitor && !d_nav_msg_packet.nav_message.empty()) + { + d_nav_msg_packet.system = std::string(1, current_symbol.System); + d_nav_msg_packet.signal = std::string(current_symbol.Signal); + d_nav_msg_packet.prn = static_cast(current_symbol.PRN); + d_nav_msg_packet.tow_at_current_symbol_ms = static_cast(d_TOW_at_current_symbol_ms); + const std::shared_ptr tmp_obj = std::make_shared(d_nav_msg_packet); + this->message_port_pub(pmt::mp("Nav_msg_from_TLM"), pmt::make_any(tmp_obj)); + d_nav_msg_packet.nav_message = ""; + } } } } @@ -1111,7 +1269,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( } } - if (d_inav_nav.get_flag_TOW_set() == true || d_fnav_nav.get_flag_TOW_set() == true || d_cnav_nav.get_flag_CRC_test() == true) + if (current_symbol.Flag_valid_word == true) { current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; // todo: Galileo to GPS time conversion should be moved to observable block. diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index 98c1494a1..f601a5f59 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -32,6 +32,7 @@ #include // for boost::circular_buffer #include // for block #include // for gr_vector_const_void_star +#include // for pmt::pmt_t #include // for int32_t, uint32_t #include // for std::ofstream #include // for std::unique_ptr @@ -80,6 +81,7 @@ private: galileo_telemetry_decoder_gs(const Gnss_Satellite &satellite, const Tlm_Conf &conf, int frame_type); + void msg_handler_read_galileo_tow_map(const pmt::pmt_t &msg); void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out); void decode_INAV_word(float *page_part_symbols, int32_t frame_length); void decode_FNAV_word(float *page_symbols, int32_t frame_length); @@ -111,6 +113,7 @@ private: uint64_t d_sample_counter; uint64_t d_preamble_index; uint64_t d_last_valid_preamble; + uint64_t d_received_sample_counter; int32_t d_mm; int32_t d_codelength; @@ -130,6 +133,7 @@ private: uint32_t d_TOW_at_Preamble_ms; uint32_t d_TOW_at_current_symbol_ms; uint32_t d_max_symbols_without_valid_frame; + uint32_t d_received_tow_ms; char d_band; // This variable will store which band we are dealing with (Galileo E1 or E5b) @@ -148,6 +152,7 @@ private: bool d_enable_reed_solomon_inav; bool d_valid_timetag; bool d_E6_TOW_set; + bool d_there_are_e6_channels; }; diff --git a/src/algorithms/telemetry_decoder/libs/tlm_conf.cc b/src/algorithms/telemetry_decoder/libs/tlm_conf.cc index a09fd6041..2b9b3c59d 100644 --- a/src/algorithms/telemetry_decoder/libs/tlm_conf.cc +++ b/src/algorithms/telemetry_decoder/libs/tlm_conf.cc @@ -30,4 +30,8 @@ void Tlm_Conf::SetFromConfiguration(const ConfigurationInterface *configuration, const std::string default_crc_stats_dumpname("telemetry_crc_stats"); dump_crc_stats_filename = configuration->property(role + ".dump_crc_stats_filename", default_crc_stats_dumpname); enable_navdata_monitor = configuration->property("NavDataMonitor.enable_monitor", false); + if (configuration->property("Channels_E6.count", 0) > 0) + { + there_are_e6_channels = true; + } } diff --git a/src/algorithms/telemetry_decoder/libs/tlm_conf.h b/src/algorithms/telemetry_decoder/libs/tlm_conf.h index 6ac5f81e2..abac3ac87 100644 --- a/src/algorithms/telemetry_decoder/libs/tlm_conf.h +++ b/src/algorithms/telemetry_decoder/libs/tlm_conf.h @@ -42,6 +42,7 @@ public: bool enable_reed_solomon{false}; // for INAV message in Galileo E1B bool dump_crc_stats{false}; // telemetry CRC statistics bool enable_navdata_monitor{false}; + bool there_are_e6_channels{false}; }; diff --git a/src/core/libs/CMakeLists.txt b/src/core/libs/CMakeLists.txt index ac8fdea75..c04d19695 100644 --- a/src/core/libs/CMakeLists.txt +++ b/src/core/libs/CMakeLists.txt @@ -20,6 +20,7 @@ set(CORE_LIBS_SOURCES galileo_e6_has_msg_receiver.cc nav_message_monitor.cc nav_message_udp_sink.cc + galileo_tow_map.cc ) set(CORE_LIBS_HEADERS @@ -35,6 +36,7 @@ set(CORE_LIBS_HEADERS nav_message_udp_sink.h serdes_nav_message.h nav_message_monitor.h + galileo_tow_map.h ) if(ENABLE_FPGA) diff --git a/src/core/libs/galileo_tow_map.cc b/src/core/libs/galileo_tow_map.cc new file mode 100644 index 000000000..6b4da0fb5 --- /dev/null +++ b/src/core/libs/galileo_tow_map.cc @@ -0,0 +1,98 @@ +/*! + * \file galileo_tow_map.cc + * \brief GNU Radio block that stores TOW for Galileo channels + * \author Carles Fernandez-Prades, 2022. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "galileo_tow_map.h" +#include // for LOG +#include // for std::numeric_limits +#include // for std::shared +#include // for typeid + +#if HAS_GENERIC_LAMBDA +#else +#include +#endif + +#if PMT_USES_BOOST_ANY +#include +namespace wht = boost; +#else +#include +namespace wht = std; +#endif + +galileo_tow_map_sptr galileo_tow_map_make() +{ + return galileo_tow_map_sptr(new galileo_tow_map()); +} + + +galileo_tow_map::galileo_tow_map() : gr::block("galileo_tow_map", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + // register Gal E6 HAS input message port from telemetry blocks + this->message_port_register_in(pmt::mp("TOW_from_TLM")); + // register nav message monitor out + this->message_port_register_out(pmt::mp("TOW_to_TLM")); + // handler for input port + this->set_msg_handler(pmt::mp("TOW_from_TLM"), +#if HAS_GENERIC_LAMBDA + [this](auto&& PH1) { msg_handler_galileo_tow_map(PH1); }); +#else +#if USE_BOOST_BIND_PLACEHOLDERS + boost::bind(&galileo_tow_map::msg_handler_galileo_tow_map, this, boost::placeholders::_1)); +#else + boost::bind(&galileo_tow_map::msg_handler_galileo_tow_map, this, _1)); +#endif +#endif + + for (uint32_t prn = 0; prn < 37; prn++) + { + d_galileo_tow[prn] = std::pair(std::numeric_limits::max(), std::numeric_limits::max()); + } +} + + +void galileo_tow_map::msg_handler_galileo_tow_map(const pmt::pmt_t& msg) +{ + gr::thread::scoped_lock lock(d_setlock); + try + { + const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); + if (msg_type_hash_code == typeid(std::shared_ptr>>).hash_code()) + { + const auto received_tow_map = wht::any_cast>>>(pmt::any_ref(msg)); + const uint32_t received_prn = received_tow_map->first; + const uint32_t received_tow = received_tow_map->second.first; + const uint64_t received_sample_counter = received_tow_map->second.second; + + d_galileo_tow.erase(received_prn); + if (received_tow < 604800000) // received TOW is in ms + { + d_galileo_tow[received_prn] = std::pair(received_tow, received_sample_counter); + } + else + { + d_galileo_tow[received_prn] = std::pair(std::numeric_limits::max(), std::numeric_limits::max()); + } + const std::shared_ptr>> tmp_obj = std::make_shared>>(d_galileo_tow); + this->message_port_pub(pmt::mp("TOW_to_TLM"), pmt::make_any(tmp_obj)); + } + } + catch (const wht::bad_any_cast& e) + { + LOG(WARNING) << "galileo_tow_map Bad any_cast: " << e.what(); + } +} diff --git a/src/core/libs/galileo_tow_map.h b/src/core/libs/galileo_tow_map.h new file mode 100644 index 000000000..02a5ab07f --- /dev/null +++ b/src/core/libs/galileo_tow_map.h @@ -0,0 +1,54 @@ +/*! + * \file galileo_tow_map.h + * \brief GNU Radio block that stores TOW for Galileo channels + * \author Carles Fernandez-Prades, 2022. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GALILEO_TOW_MAP_H +#define GNSS_SDR_GALILEO_TOW_MAP_H + +#include "gnss_block_interface.h" // for gnss_shared_ptr +#include // for gr::block +#include // for pmt::pmt_t +#include +#include +#include + +/** \addtogroup Core + * \{ */ +/** \addtogroup Core_Receiver_Library + * \{ */ + +class galileo_tow_map; + +using galileo_tow_map_sptr = gnss_shared_ptr; + +galileo_tow_map_sptr galileo_tow_map_make(); + +class galileo_tow_map : public gr::block +{ +public: + ~galileo_tow_map() = default; //!< Default destructor + +private: + friend galileo_tow_map_sptr galileo_tow_map_make(); + galileo_tow_map(); + + void msg_handler_galileo_tow_map(const pmt::pmt_t& msg); + + std::map> d_galileo_tow; +}; + +/** \} */ +/** \} */ +#endif // GNSS_SDR_GALILEO_TOW_MAP_H diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index d0a71dbff..990780e3d 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -106,16 +106,26 @@ void GNSSFlowgraph::init() { enable_e6_has_rx_ = true; gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make(); + galileo_tow_map_ = galileo_tow_map_make(); } else { gal_e6_has_rx_ = nullptr; + galileo_tow_map_ = nullptr; } // 1. read the number of RF front-ends available (one file_source per RF front-end) int sources_count_deprecated = configuration_->property("Receiver.sources_count", 1); sources_count_ = configuration_->property("GNSS-SDR.num_sources", sources_count_deprecated); + // Avoid segmentation fault caused by wrong configuration + if (sources_count_ == 2 && block_factory->GetSignalSource(configuration_.get(), queue_.get(), 0)->implementation() == "Multichannel_File_Signal_Source") + { + std::cout << " * Please set GNSS-SDR.num_sources=1 in your configuraiion file\n"; + std::cout << " if you are using the Multichannel_File_Signal_Source implementation.\n"; + sources_count_ = 1; + } + int signal_conditioner_ID = 0; for (int i = 0; i < sources_count_; i++) @@ -474,7 +484,12 @@ int GNSSFlowgraph::connect_desktop_flowgraph() { return 1; } + if (connect_galileo_tow_map() != 0) + { + return 1; + } } + // Activate acquisition in enabled channels for (int i = 0; i < channels_count_; i++) { @@ -582,6 +597,18 @@ int GNSSFlowgraph::connect_fpga_flowgraph() return 1; } + if (enable_e6_has_rx_) + { + if (connect_gal_e6_has() != 0) + { + return 1; + } + if (connect_galileo_tow_map() != 0) + { + return 1; + } + } + check_desktop_conf_in_fpga_env(); LOG(INFO) << "The GNU Radio flowgraph for the current GNSS-SDR configuration with FPGA off-loading has been successfully connected"; @@ -781,6 +808,26 @@ int GNSSFlowgraph::connect_pvt() } +int GNSSFlowgraph::connect_galileo_tow_map() +{ + try + { + for (int i = 0; i < channels_count_; i++) + { + top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("TOW_from_TLM"), galileo_tow_map_, pmt::mp("TOW_from_TLM")); + top_block_->msg_connect(galileo_tow_map_, pmt::mp("TOW_to_TLM"), channels_.at(i)->get_right_block(), pmt::mp("TOW_to_TLM")); + } + } + catch (const std::exception& e) + { + LOG(ERROR) << "Can't connect The Galileo TOW map internally: " << e.what(); + top_block_->disconnect_all(); + return 1; + } + return 0; +} + + int GNSSFlowgraph::connect_sample_counter() { // connect the sample counter to the Signal Conditioner diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index b4bdbeeb9..be7c5e8bf 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -27,6 +27,7 @@ #include "channel_status_msg_receiver.h" #include "concurrent_queue.h" #include "galileo_e6_has_msg_receiver.h" +#include "galileo_tow_map.h" #include "gnss_sdr_sample_counter.h" #include "gnss_signal.h" #include "pvt_interface.h" @@ -171,6 +172,7 @@ private: int connect_observables(); int connect_pvt(); int connect_sample_counter(); + int connect_galileo_tow_map(); int connect_signal_sources_to_signal_conditioners(); int connect_signal_conditioners_to_channels(); @@ -231,6 +233,7 @@ private: gr::basic_block_sptr NavDataMonitor_; channel_status_msg_receiver_sptr channels_status_; // class that receives and stores the current status of the receiver channels galileo_e6_has_msg_receiver_sptr gal_e6_has_rx_; + galileo_tow_map_sptr galileo_tow_map_; gnss_sdr_sample_counter_sptr ch_out_sample_counter_; #if ENABLE_FPGA diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc index e640a3d2c..b4dfc483f 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc @@ -143,7 +143,7 @@ void NmeaPrinterTest::conf() TEST_F(NmeaPrinterTest, PrintLine) { std::string filename("nmea_test.nmea"); - std::shared_ptr pvt_solution = std::make_shared(rtk, "filename", false, false); + std::shared_ptr pvt_solution = std::make_shared(rtk, "filename", 1, false, false); boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc index 27b6dcaa6..2eb6e1987 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc @@ -141,7 +141,7 @@ void RinexPrinterTest::conf() TEST_F(RinexPrinterTest, GalileoObsHeader) { - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 4, false, false); auto eph = Galileo_Ephemeris(); eph.PRN = 1; pvt_solution->galileo_ephemeris_map[1] = eph; @@ -227,7 +227,7 @@ TEST_F(RinexPrinterTest, GalileoObsHeader) TEST_F(RinexPrinterTest, GlonassObsHeader) { - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 28, false, false); auto eph = Glonass_Gnav_Ephemeris(); eph.PRN = 1; pvt_solution->glonass_gnav_ephemeris_map[1] = eph; @@ -287,7 +287,7 @@ TEST_F(RinexPrinterTest, MixedObsHeader) auto eph_gps = Gps_Ephemeris(); eph_gal.PRN = 1; eph_gps.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 106, false, false); pvt_solution->galileo_ephemeris_map[1] = eph_gal; pvt_solution->gps_ephemeris_map[1] = eph_gps; @@ -357,7 +357,7 @@ TEST_F(RinexPrinterTest, MixedObsHeaderGpsGlo) auto eph_gps = Gps_Ephemeris(); eph_glo.PRN = 1; eph_gps.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 26, false, false); pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo; pvt_solution->gps_ephemeris_map[1] = eph_gps; @@ -424,7 +424,7 @@ TEST_F(RinexPrinterTest, GalileoObsLog) bool no_more_finds = false; auto eph = Galileo_Ephemeris(); eph.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 4, false, false); pvt_solution->galileo_ephemeris_map[1] = eph; std::map gnss_observables_map; @@ -504,7 +504,7 @@ TEST_F(RinexPrinterTest, GlonassObsLog) bool no_more_finds = false; auto eph = Glonass_Gnav_Ephemeris(); eph.PRN = 22; - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 23, false, false); pvt_solution->glonass_gnav_ephemeris_map[1] = eph; std::map gnss_observables_map; @@ -586,7 +586,7 @@ TEST_F(RinexPrinterTest, GpsObsLogDualBand) auto eph_cnav = Gps_CNAV_Ephemeris(); eph.PRN = 1; eph_cnav.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 7, false, false); pvt_solution->gps_ephemeris_map[1] = eph; pvt_solution->gps_cnav_ephemeris_map[1] = eph_cnav; std::map gnss_observables_map; @@ -674,7 +674,7 @@ TEST_F(RinexPrinterTest, GpsObsLogDualBand) TEST_F(RinexPrinterTest, GalileoObsLogDualBand) { - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 14, false, false); auto eph = Galileo_Ephemeris(); eph.PRN = 1; pvt_solution->galileo_ephemeris_map[1] = eph; @@ -774,7 +774,7 @@ TEST_F(RinexPrinterTest, MixedObsLog) auto eph_gal = Galileo_Ephemeris(); eph_gps.PRN = 1; eph_gal.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 9, false, false); pvt_solution->gps_ephemeris_map[1] = eph_gps; pvt_solution->galileo_ephemeris_map[1] = eph_gal; std::map gnss_observables_map; @@ -898,7 +898,7 @@ TEST_F(RinexPrinterTest, MixedObsLogGpsGlo) auto eph_glo = Glonass_Gnav_Ephemeris(); eph_gps.PRN = 1; eph_glo.PRN = 1; - auto pvt_solution = std::make_shared(rtk, "filename", false, false); + auto pvt_solution = std::make_shared(rtk, "filename", 26, false, false); pvt_solution->gps_ephemeris_map[1] = eph_gps; pvt_solution->glonass_gnav_ephemeris_map[1] = eph_glo; std::map gnss_observables_map; diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc index b1bfdcad8..9804fbde1 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc @@ -383,7 +383,7 @@ TEST(RTKLibSolverTest, test1) bool save_to_mat = false; rtk_t rtk = configure_rtklib_options(); - auto d_ls_pvt = std::make_unique(rtk, nchannels, dump_filename, flag_dump_to_file, save_to_mat); + auto d_ls_pvt = std::make_unique(rtk, nchannels, dump_filename, 1, flag_dump_to_file, save_to_mat); d_ls_pvt->set_averaging_depth(1); // load ephemeris