From b6762bf438872691e39f8f9ef743e1adbebdac2d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 1 Jun 2022 12:17:50 +0200 Subject: [PATCH] Use E6 channels in PVT --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 9 ++++ src/algorithms/PVT/libs/rtklib_solver.cc | 48 +++++++++++++++++++ .../galileo_telemetry_decoder_gs.cc | 35 ++++++++++++-- .../galileo_telemetry_decoder_gs.h | 2 + src/core/libs/gnss_sdr_supl_client.cc | 5 ++ 5 files changed, 96 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 83299a356..f44573db8 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -651,6 +651,11 @@ rtklib_pvt_gs::~rtklib_pvt_gs() { ofs.open(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); boost::archive::xml_oarchive xml(ofs); + // Annotate as full GPS week number + for (auto& gal_eph_iter : d_internal_pvt_solver->galileo_ephemeris_map) + { + gal_eph_iter.second.WN += 1024; + } xml << boost::serialization::make_nvp("GNSS-SDR_gal_ephemeris_map", d_internal_pvt_solver->galileo_ephemeris_map); LOG(INFO) << "Saved Galileo E1 Ephemeris map data"; } @@ -2049,6 +2054,10 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item store_valid_observable = true; } } + if (std::string(in[i][epoch].Signal) == std::string("E6")) + { + store_valid_observable = true; + } if (store_valid_observable) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index bfa3bd52b..cc0f96ac4 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -407,6 +407,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ 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) @@ -514,6 +515,48 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ gal_e5_is_e5b = true; } } + 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()) + { + 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, + 2); // Band E6 + 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, + 2); // Band E6 + // std::cout << "Week " << galileo_ephemeris_iter->second.WN << '\n'; + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } break; } case 'G': @@ -879,6 +922,11 @@ 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 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 89214345c..e22435dd6 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 @@ -42,6 +42,7 @@ #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 @@ -79,6 +80,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_sample_counter(0ULL), d_preamble_index(0ULL), d_last_valid_preamble(0), + d_received_sample_counter(0), d_frame_type(frame_type), d_CRC_error_counter(0), d_channel(0), @@ -86,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(std::numeric_limits::max()), d_band('1'), d_sent_tlm_failed_msg(false), d_flag_frame_sync(false), @@ -324,8 +327,11 @@ void galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map(const pmt::p { 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()); - std::cout << "Received TOW: " << received_tow_sample.first << '\n'; - std::cout << "Received sample counter: " << received_tow_sample.second << '\n'; + if (received_tow_sample.first < 604800) + { + d_received_tow = received_tow_sample.first; + d_received_sample_counter = received_tow_sample.second; + } } } catch (const wht::bad_any_cast &e) @@ -1105,7 +1111,30 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( } case 3: // CNAV { - // TODO + if (!d_valid_timetag) + { + if (d_received_tow < 604800) + { + const int64_t diff = d_received_sample_counter - current_symbol.Tracking_sample_counter; + bool overflowed = (diff < 0) ^ (d_received_sample_counter < current_symbol.Tracking_sample_counter); + if (!overflowed && (std::abs(diff) / static_cast(current_symbol.fs) < 1.0)) + { + d_TOW_at_Preamble_ms = d_received_tow; + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_CNAV_SYMBOLS_PER_PAGE * GALILEO_CNAV_PAGE_MS); + 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 = ""; + } } } } 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 58e4a8ab3..d9877a796 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 @@ -113,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; @@ -132,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; char d_band; // This variable will store which band we are dealing with (Galileo E1 or E5b) diff --git a/src/core/libs/gnss_sdr_supl_client.cc b/src/core/libs/gnss_sdr_supl_client.cc index e079b0a9f..e2e9a3c73 100644 --- a/src/core/libs/gnss_sdr_supl_client.cc +++ b/src/core/libs/gnss_sdr_supl_client.cc @@ -414,6 +414,11 @@ bool Gnss_Sdr_Supl_Client::load_gal_ephemeris_xml(const std::string& file_name) gal_ephemeris_map.clear(); xml >> boost::serialization::make_nvp("GNSS-SDR_gal_ephemeris_map", this->gal_ephemeris_map); LOG(INFO) << "Loaded Ephemeris map data with " << this->gal_ephemeris_map.size() << " satellites"; + // Convert to full GPS week number + for (auto& gal_eph_iter : this->gal_ephemeris_map) + { + gal_eph_iter.second.WN -= 1024; + } } catch (std::exception& e) {