From b1c0e867517f6339840e5cc1fdd4e07408be9963 Mon Sep 17 00:00:00 2001 From: Javier Date: Mon, 10 Feb 2020 10:59:10 +0100 Subject: [PATCH] New receiver feature: optional carrier smoothing of code range observables --- src/algorithms/observables/CMakeLists.txt | 1 + .../observables/adapters/CMakeLists.txt | 1 + .../adapters/hybrid_observables.cc | 18 ++- .../gnuradio_blocks/CMakeLists.txt | 1 + .../gnuradio_blocks/hybrid_observables_gs.cc | 108 +++++++++++++++--- .../gnuradio_blocks/hybrid_observables_gs.h | 46 +++++--- .../observables/libs/CMakeLists.txt | 52 +++++++++ src/algorithms/observables/libs/obs_conf.cc | 43 +++++++ src/algorithms/observables/libs/obs_conf.h | 55 +++++++++ 9 files changed, 289 insertions(+), 36 deletions(-) create mode 100644 src/algorithms/observables/libs/CMakeLists.txt create mode 100644 src/algorithms/observables/libs/obs_conf.cc create mode 100644 src/algorithms/observables/libs/obs_conf.h diff --git a/src/algorithms/observables/CMakeLists.txt b/src/algorithms/observables/CMakeLists.txt index f17f1f9c7..87e9bf982 100644 --- a/src/algorithms/observables/CMakeLists.txt +++ b/src/algorithms/observables/CMakeLists.txt @@ -5,5 +5,6 @@ # SPDX-License-Identifier: GPL-3.0-or-later # +add_subdirectory(libs) add_subdirectory(adapters) add_subdirectory(gnuradio_blocks) diff --git a/src/algorithms/observables/adapters/CMakeLists.txt b/src/algorithms/observables/adapters/CMakeLists.txt index f16e74c12..6454fdd27 100644 --- a/src/algorithms/observables/adapters/CMakeLists.txt +++ b/src/algorithms/observables/adapters/CMakeLists.txt @@ -27,6 +27,7 @@ target_link_libraries(obs_adapters PUBLIC obs_gr_blocks algorithms_libs + observables_libs PRIVATE Gflags::gflags Glog::glog diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index bf08436b9..85a265a46 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -18,9 +18,9 @@ * ------------------------------------------------------------------------- */ - #include "hybrid_observables.h" #include "configuration_interface.h" +#include "obs_conf.h" #include #include // for operator<< @@ -34,7 +34,21 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration, dump_mat_ = configuration->property(role + ".dump_mat", true); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - observables_ = hybrid_observables_gs_make(in_streams_, out_streams_, dump_, dump_mat_, dump_filename_); + Obs_Conf conf; + + conf.dump = dump_; + conf.dump_mat = dump_mat_; + conf.dump_filename = dump_filename_; + conf.nchannels_in = in_streams_; + conf.nchannels_out = out_streams_; + + conf.enable_carrier_smoothing = configuration->property(role + ".enable_carrier_smoothing", false); + conf.smoothing_factor = configuration->property(role + ".smoothing_factor", 200.0); + if (conf.enable_carrier_smoothing == true) + { + LOG(INFO) << "Observables carrier smoothing enabled with smoothing factor " << conf.smoothing_factor; + } + observables_ = hybrid_observables_gs_make(conf); DLOG(INFO) << "Observables block ID (" << observables_->unique_id() << ")"; } diff --git a/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt b/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt index 7293c25b4..2a3cd70f3 100644 --- a/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt @@ -37,6 +37,7 @@ target_link_libraries(obs_gr_blocks Boost::headers Gnuradio::blocks PRIVATE + observables_libs algorithms_libs core_system_parameters Gflags::gflags diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index c3ab4df52..9e32585b6 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -55,19 +55,15 @@ namespace errorlib = boost::system; #endif -hybrid_observables_gs_sptr hybrid_observables_gs_make(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, bool dump_mat, const std::string &dump_filename) +hybrid_observables_gs_sptr hybrid_observables_gs_make(const Obs_Conf &conf_) { - return hybrid_observables_gs_sptr(new hybrid_observables_gs(nchannels_in, nchannels_out, dump, dump_mat, dump_filename)); + return hybrid_observables_gs_sptr(new hybrid_observables_gs(conf_)); } -hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in, - uint32_t nchannels_out, - bool dump, - bool dump_mat, - const std::string &dump_filename) : gr::block("hybrid_observables_gs", - gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) +hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block("hybrid_observables_gs", + gr::io_signature::make(conf_.nchannels_in, conf_.nchannels_in, sizeof(Gnss_Synchro)), + gr::io_signature::make(conf_.nchannels_out, conf_.nchannels_out, sizeof(Gnss_Synchro))) { // PVT input message port this->message_port_register_in(pmt::mp("pvt_to_observables")); @@ -75,12 +71,12 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in, // Send Channel status to gnss_flowgraph this->message_port_register_out(pmt::mp("status")); - - d_dump = dump; - d_dump_mat = dump_mat and d_dump; - d_dump_filename = dump_filename; - d_nchannels_out = nchannels_out; - d_nchannels_in = nchannels_in; + d_conf = conf_; + d_dump = conf_.dump; + d_dump_mat = conf_.dump_mat and d_dump; + d_dump_filename = conf_.dump_filename; + d_nchannels_out = conf_.nchannels_out; + d_nchannels_in = conf_.nchannels_in; d_gnss_synchro_history = std::make_shared>(1000, d_nchannels_out); // ############# ENABLE DATA FILE LOG ################# @@ -134,6 +130,12 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in, // rework d_Rx_clock_buffer.set_capacity(10); // 10*20 ms = 200 ms of data in buffer d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer + + d_channel_last_pll_lock = std::vector(d_nchannels_out, false); + d_channel_last_pseudorange_smooth = std::vector(d_nchannels_out, 0.0); + d_channel_last_carrier_phase_rads = std::vector(d_nchannels_out, 0.0); + + d_smooth_filter_M = conf_.smoothing_factor; } @@ -525,6 +527,75 @@ void hybrid_observables_gs::compute_pranges(std::vector &data) } } +void hybrid_observables_gs::smooth_pseudoranges(std::vector &data) +{ + std::vector::iterator it; + for (it = data.begin(); it != data.end(); it++) + { + if (it->Flag_valid_pseudorange) + { + //0. get wavelength for the current signal + double wavelength_m = 0; + switch (mapStringValues_[it->Signal]) + { + case evGPS_1C: + wavelength_m = SPEED_OF_LIGHT / FREQ1; + break; + case evGPS_L5: + wavelength_m = SPEED_OF_LIGHT / FREQ5; + break; + case evSBAS_1C: + wavelength_m = SPEED_OF_LIGHT / FREQ1; + break; + case evGAL_1B: + wavelength_m = SPEED_OF_LIGHT / FREQ1; + break; + case evGAL_5X: + wavelength_m = SPEED_OF_LIGHT / FREQ5; + break; + case evGPS_2S: + wavelength_m = SPEED_OF_LIGHT / FREQ2; + break; + case evBDS_B3: + wavelength_m = SPEED_OF_LIGHT / FREQ3_BDS; + break; + case evGLO_1G: + wavelength_m = SPEED_OF_LIGHT / FREQ1_GLO; + break; + case evGLO_2G: + wavelength_m = SPEED_OF_LIGHT / FREQ2_GLO; + break; + case evBDS_B1: + wavelength_m = SPEED_OF_LIGHT / FREQ1_BDS; + break; + case evBDS_B2: + wavelength_m = SPEED_OF_LIGHT / FREQ2_BDS; + break; + default: + break; + } + + //todo: propagate the PLL lock status in Gnss_Synchro + //1. check if last PLL lock status was false and initialize last d_channel_last_pseudorange_smooth + if (d_channel_last_pll_lock.at(it->Channel_ID) == true) + { + //2. Compute the smoothed pseudorange for this channel + // Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/) + double r_sm = d_channel_last_pseudorange_smooth.at(it->Channel_ID); + double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M); + it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / PI_2) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads.at(it->Channel_ID)); + } + d_channel_last_pseudorange_smooth.at(it->Channel_ID) = it->Pseudorange_m; + d_channel_last_carrier_phase_rads.at(it->Channel_ID) = it->Carrier_phase_rads; + d_channel_last_pll_lock.at(it->Channel_ID) = it->Flag_valid_pseudorange; + } + else + { + d_channel_last_pll_lock.at(it->Channel_ID) = false; + } + } +} + int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, @@ -607,11 +678,16 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) compute_pranges(epoch_data); } + //Carrier smoothing (optional) + if (d_conf.enable_carrier_smoothing == true) + { + smooth_pseudoranges(epoch_data); + } + //output the observables set to the PVT block for (uint32_t n = 0; n < d_nchannels_out; n++) { out[n][0] = epoch_data[n]; } - // report channel status every second T_status_report_timer_ms += T_rx_step_ms; if (T_status_report_timer_ms >= 1000) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index 54c89b39a..224a3f192 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -23,6 +23,7 @@ #ifndef GNSS_SDR_HYBRID_OBSERVABLES_GS_H #define GNSS_SDR_HYBRID_OBSERVABLES_GS_H +#include "obs_conf.h" #include // for boost::circular_buffer #include // for boost::shared_ptr #include // for block @@ -41,12 +42,7 @@ class Gnss_circular_deque; using hybrid_observables_gs_sptr = boost::shared_ptr; -hybrid_observables_gs_sptr hybrid_observables_gs_make( - unsigned int nchannels_in, - unsigned int nchannels_out, - bool dump, - bool dump_mat, - const std::string& dump_filename); +hybrid_observables_gs_sptr hybrid_observables_gs_make(const Obs_Conf& conf_); /*! * \brief This class implements a block that computes observables @@ -60,19 +56,33 @@ public: gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); private: - friend hybrid_observables_gs_sptr hybrid_observables_gs_make( - uint32_t nchannels_in, - uint32_t nchannels_out, - bool dump, - bool dump_mat, - const std::string& dump_filename); + friend hybrid_observables_gs_sptr hybrid_observables_gs_make(const Obs_Conf& conf_); - hybrid_observables_gs( - uint32_t nchannels_in, - uint32_t nchannels_out, - bool dump, - bool dump_mat, - const std::string& dump_filename); + hybrid_observables_gs(const Obs_Conf& conf_); + + Obs_Conf d_conf; + + enum StringValue + { + evGPS_1C, + evGPS_2S, + evGPS_L5, + evSBAS_1C, + evGAL_1B, + evGAL_5X, + evGLO_1G, + evGLO_2G, + evBDS_B1, + evBDS_B2, + evBDS_B3 + }; + + std::map mapStringValues_; + std::vector d_channel_last_pll_lock; + std::vector d_channel_last_pseudorange_smooth; + std::vector d_channel_last_carrier_phase_rads; + double d_smooth_filter_M; + void smooth_pseudoranges(std::vector& data); bool T_rx_TOW_set; // rx time follow GPST bool d_dump; diff --git a/src/algorithms/observables/libs/CMakeLists.txt b/src/algorithms/observables/libs/CMakeLists.txt new file mode 100644 index 000000000..dbb70d7b9 --- /dev/null +++ b/src/algorithms/observables/libs/CMakeLists.txt @@ -0,0 +1,52 @@ +# Copyright (C) 2012-2019 (see AUTHORS file for a list of contributors) +# +# This file is part of GNSS-SDR. +# +# GNSS-SDR is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# GNSS-SDR is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with GNSS-SDR. If not, see . +# + +set(OBSERVABLES_LIB_HEADERS ${OBSERVABLES_LIB_HEADERS} obs_conf.h) +set(OBSERVABLES_LIB_SOURCES ${OBSERVABLES_LIB_SOURCES} obs_conf.cc) + +list(SORT OBSERVABLES_LIB_HEADERS) +list(SORT OBSERVABLES_LIB_SOURCES) + +source_group(Headers FILES ${OBSERVABLES_LIB_HEADERS}) + +add_library(observables_libs + ${OBSERVABLES_LIB_SOURCES} + ${OBSERVABLES_LIB_HEADERS} +) + +target_link_libraries(observables_libs + PRIVATE + Gflags::gflags + Glog::glog + algorithms_libs + core_system_parameters +) + +if(ENABLE_CLANG_TIDY) + if(CLANG_TIDY_EXE) + set_target_properties(observables_libs + PROPERTIES + CXX_CLANG_TIDY "${DO_CLANG_TIDY}" + ) + endif() +endif() + +set_property(TARGET observables_libs + APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES + $ +) diff --git a/src/algorithms/observables/libs/obs_conf.cc b/src/algorithms/observables/libs/obs_conf.cc new file mode 100644 index 000000000..d16e3eb66 --- /dev/null +++ b/src/algorithms/observables/libs/obs_conf.cc @@ -0,0 +1,43 @@ +/*! + * \file obs_conf.h + * \brief Class that contains all the configuration parameters for generic + * observables block + * \author Javier Arribas, 2020. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "obs_conf.h" + +Obs_Conf::Obs_Conf() +{ + enable_carrier_smoothing = false; + smoothing_factor = 200; + nchannels_in = 0; + nchannels_out = 0; + dump = false; + dump_mat = false; + dump_filename = "obs_dump.dat"; +} diff --git a/src/algorithms/observables/libs/obs_conf.h b/src/algorithms/observables/libs/obs_conf.h new file mode 100644 index 000000000..6f98cd61b --- /dev/null +++ b/src/algorithms/observables/libs/obs_conf.h @@ -0,0 +1,55 @@ +/*! + * \file obs_conf.h + * \brief Class that contains all the configuration parameters for generic + * observables block + * \author Javier Arribas, 2020. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_OBS_CONF_H_ +#define GNSS_SDR_OBS_CONF_H_ + +#include "configuration_interface.h" +#include +#include +#include + +class Obs_Conf +{ +public: + bool enable_carrier_smoothing; + double smoothing_factor; + unsigned int nchannels_in; + unsigned int nchannels_out; + bool dump; + bool dump_mat; + std::string dump_filename; + + + Obs_Conf(); +}; + +#endif