Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

docs/changelog

@@ -2,7 +2,7 @@
 
 ### Improvements in Accuracy
 
-- Local clock correction based on PVT solution (error kept below 1 ms).
+- Local clock correction based on PVT solution, allowing the delivery of continuous observables.
 - Fix a bug in broadcast ionospheric parameters usage.
 
 

src/algorithms/PVT/adapters/rtklib_pvt.cc

@@ -69,10 +69,10 @@ Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration,
     pvt_output_parameters.dump_mat = configuration->property(role + ".dump_mat", true);
 
     // output rate
-    pvt_output_parameters.output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
+    pvt_output_parameters.output_rate_ms = bc::lcm(20, configuration->property(role + ".output_rate_ms", 500));
 
     // display rate
-    pvt_output_parameters.display_rate_ms = configuration->property(role + ".display_rate_ms", 500);
+    pvt_output_parameters.display_rate_ms = bc::lcm(pvt_output_parameters.output_rate_ms, configuration->property(role + ".display_rate_ms", 500));
 
     // NMEA Printer settings
     pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false);

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h

@@ -139,6 +139,30 @@ private:
 
     void msg_handler_telemetry(const pmt::pmt_t& msg);
 
+    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<std::string, StringValue> mapStringValues_;
+
+    void apply_rx_clock_offset(std::map<int, Gnss_Synchro>& observables_map,
+        double rx_clock_offset_s);
+
+    std::map<int, Gnss_Synchro> interpolate_observables(std::map<int, Gnss_Synchro>& observables_map_t0,
+        std::map<int, Gnss_Synchro>& observables_map_t1,
+        double rx_time_s);
+
     bool d_dump;
     bool d_dump_mat;
     bool b_rinex_output_enabled;
@@ -184,10 +208,14 @@ private:
     bool d_kml_output_enabled;
     bool d_nmea_output_file_enabled;
 
-    std::shared_ptr<Rtklib_Solver> d_pvt_solver;
+    std::shared_ptr<Rtklib_Solver> d_internal_pvt_solver;
+    std::shared_ptr<Rtklib_Solver> d_user_pvt_solver;
 
+    int32_t max_obs_block_rx_clock_offset_ms;
+    bool d_waiting_obs_block_rx_clock_offset_correction_msg;
     std::map<int, Gnss_Synchro> gnss_observables_map;
-    bool observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b);
+    std::map<int, Gnss_Synchro> gnss_observables_map_t0;
+    std::map<int, Gnss_Synchro> gnss_observables_map_t1;
 
     uint32_t type_of_rx;
 

src/algorithms/PVT/libs/hybrid_ls_pvt.cc

@@ -45,7 +45,6 @@ Hybrid_Ls_Pvt::Hybrid_Ls_Pvt(int nchannels, std::string dump_filename, bool flag
     d_dump_filename = std::move(dump_filename);
     d_flag_dump_enabled = flag_dump_to_file;
     d_galileo_current_time = 0;
-    count_valid_position = 0;
     this->set_averaging_flag(false);
     // ############# ENABLE DATA FILE LOG #################
     if (d_flag_dump_enabled == true)

src/algorithms/PVT/libs/hybrid_ls_pvt.h

@@ -69,7 +69,6 @@ public:
     Gps_CNAV_Utc_Model gps_cnav_utc_model;
 
 private:
-    int count_valid_position;
     bool d_flag_dump_enabled;
     std::string d_dump_filename;
     std::ofstream d_dump_file;

src/algorithms/PVT/libs/pvt_conf.cc

@@ -40,6 +40,7 @@ Pvt_Conf::Pvt_Conf()
     geojson_rate_ms = 1000;
     nmea_rate_ms = 1000;
 
+    max_obs_block_rx_clock_offset_ms = 40;
     rinex_version = 0;
     rinexobs_rate_ms = 0;
 

src/algorithms/PVT/libs/pvt_conf.h

@@ -73,6 +73,8 @@ public:
     bool xml_output_enabled;
     bool rtcm_output_file_enabled;
 
+    int32_t max_obs_block_rx_clock_offset_ms;
+
     std::string output_path;
     std::string rinex_output_path;
     std::string gpx_output_path;

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -3,8 +3,8 @@
  * \brief PVT solver based on rtklib library functions adapted to the GNSS-SDR
  *  data flow and structures
  * \authors <ul>
- *          <li> 2017, Javier Arribas
+ *          <li> 2017-2019, Javier Arribas
- *          <li> 2017, Carles Fernandez
+ *          <li> 2017-2019, Carles Fernandez
  *          <li> 2007-2013, T. Takasu
  *          </ul>
  *
@@ -94,7 +94,6 @@ Rtklib_Solver::Rtklib_Solver(int nchannels, std::string dump_filename, bool flag
     d_dump_filename = std::move(dump_filename);
     d_flag_dump_enabled = flag_dump_to_file;
     d_flag_dump_mat_enabled = flag_dump_to_mat;
-    count_valid_position = 0;
     this->set_averaging_flag(false);
     rtk_ = rtk;
 
@@ -443,7 +442,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
     int valid_obs = 0;      // valid observations counter
     int glo_valid_obs = 0;  // GLONASS L1/L2 valid observations counter
 
-    std::array<obsd_t, MAXOBS> obs_data{};
+    obs_data.fill({});
     std::vector<eph_t> eph_data(MAXOBS);
     std::vector<geph_t> geph_data(MAXOBS);
 

src/algorithms/PVT/libs/rtklib_solver.h

@@ -126,18 +126,17 @@ public:
     Beidou_Dnav_Iono beidou_dnav_iono;
     std::map<int, Beidou_Dnav_Almanac> beidou_dnav_almanac_map;
 
-    int count_valid_position;
-
 private:
     rtk_t rtk_{};
+    Monitor_Pvt monitor_pvt{};
+    std::array<obsd_t, MAXOBS> obs_data{};
+    std::array<double, 4> dop_{};
     std::string d_dump_filename;
     std::ofstream d_dump_file;
-    bool save_matfile();
+    int d_nchannels;  // Number of available channels for positioning
     bool d_flag_dump_enabled;
     bool d_flag_dump_mat_enabled;
-    int d_nchannels;  // Number of available channels for positioning
+    bool save_matfile();
-    std::array<double, 4> dop_{};
-    Monitor_Pvt monitor_pvt{};
 };
 
 #endif

src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc

@@ -93,8 +93,7 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in,
     d_dump_filename = std::move(dump_filename);
     d_nchannels_out = nchannels_out;
     d_nchannels_in = nchannels_in;
-    T_rx_offset_ms = 0;
+    d_gnss_synchro_history = std::make_shared<Gnss_circular_deque<Gnss_Synchro>>(1000, d_nchannels_out);
-    d_gnss_synchro_history = std::make_shared<Gnss_circular_deque<Gnss_Synchro>>(500, d_nchannels_out);
 
     // ############# ENABLE DATA FILE LOG #################
     if (d_dump)
@@ -141,7 +140,6 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in,
                 }
         }
     T_rx_TOW_ms = 0U;
-    T_rx_remnant_to_20ms = 0;
     T_rx_step_ms = 20;  // read from config at the adapter GNSS-SDR.observable_interval_ms!!
     T_rx_TOW_set = false;
     T_status_report_timer_ms = 0;
@@ -197,16 +195,14 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
                 {
                     double new_rx_clock_offset_s;
                     new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
-                    T_rx_offset_ms = new_rx_clock_offset_s * 1000.0;
+                    T_rx_TOW_ms = T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0));
-                    T_rx_TOW_ms = T_rx_TOW_ms - static_cast<int>(round(T_rx_offset_ms));
-                    T_rx_remnant_to_20ms = (T_rx_TOW_ms % 20);
                     // d_Rx_clock_buffer.clear();  // Clear all the elements in the buffer
                     for (uint32_t n = 0; n < d_nchannels_out; n++)
                         {
                             d_gnss_synchro_history->clear(n);
                         }
 
-                    LOG(INFO) << "Corrected new RX Time offset: " << T_rx_offset_ms << "[ms]";
+                    LOG(INFO) << "Corrected new RX Time offset: " << static_cast<int>(round(new_rx_clock_offset_s * 1000.0)) << "[ms]";
                 }
         }
     catch (boost::bad_any_cast &e)
@@ -440,8 +436,6 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
                             //           << d_gnss_synchro_history->at(ch, t2_idx).RX_time - d_gnss_synchro_history->at(ch, t1_idx).RX_time
                             //           << " trx - t1: "
                             //           << T_rx_s - d_gnss_synchro_history->at(ch, t1_idx).RX_time;
-
-                            //
                             // std::cout << "Rx samplestamp: " << T_rx_s << " Channel " << ch << " interp buff idx " << nearest_element
                             //           << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs) << " s)\n";
                             return true;
@@ -489,8 +483,7 @@ void hybrid_observables_gs::update_TOW(const std::vector<Gnss_Synchro> &data)
                                 }
                         }
                 }
-            T_rx_TOW_ms = TOW_ref - (TOW_ref % 20);
+            T_rx_TOW_ms = TOW_ref;
-            T_rx_remnant_to_20ms = 0;
         }
     else
         {
@@ -509,7 +502,7 @@ void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data)
     // std::cout.precision(17);
     // std::cout << " T_rx_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) << std::endl;
     std::vector<Gnss_Synchro>::iterator it;
-    double current_T_rx_TOW_ms = (static_cast<double>(T_rx_TOW_ms - T_rx_remnant_to_20ms));
+    auto current_T_rx_TOW_ms = static_cast<double>(T_rx_TOW_ms);
     double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0;
     for (it = data.begin(); it != data.end(); it++)
         {
@@ -582,9 +575,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
             for (uint32_t n = 0; n < d_nchannels_out; n++)
                 {
                     Gnss_Synchro interpolated_gnss_synchro{};
-
+                    if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front()))
-                    uint32_t T_rx_remnant_to_20ms_samples = T_rx_remnant_to_20ms * in[d_nchannels_in - 1][0].fs / 1000;
-                    if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front() - T_rx_remnant_to_20ms_samples))
                         {
                             // Produce an empty observation
                             interpolated_gnss_synchro = Gnss_Synchro();
@@ -666,7 +657,17 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
                             d_dump = false;
                         }
                 }
+
+            if (n_valid > 0)
+                {
+                    // LOG(INFO) << "OBS: diff time: " << out[0][0].RX_time * 1000.0 - old_time_debug;
+                    // old_time_debug = out[0][0].RX_time * 1000.0;
                     return 1;
                 }
+            else
+                {
+                    return 0;
+                }
+        }
     return 0;
 }

src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h

@@ -87,12 +87,10 @@ private:
     bool d_dump;
     bool d_dump_mat;
     uint32_t T_rx_TOW_ms;
-    uint32_t T_rx_remnant_to_20ms;
     uint32_t T_rx_step_ms;
     uint32_t T_status_report_timer_ms;
     uint32_t d_nchannels_in;
     uint32_t d_nchannels_out;
-    double T_rx_offset_ms;
     std::string d_dump_filename;
     std::ofstream d_dump_file;
     boost::circular_buffer<uint64_t> d_Rx_clock_buffer;                         // time history

src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc

@@ -550,7 +550,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
             flag_SOW_set = true;
             d_nav.flag_new_SOW_available = false;
 
-            if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms)
+            if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(d_symbol_duration_ms))
                 {
                     LOG(INFO) << "Warning: BEIDOU B1I TOW update in ch " << d_channel
                               << " does not match the TLM TOW counter " << static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n";

src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc

@@ -581,7 +581,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
             flag_SOW_set = true;
             d_nav.flag_new_SOW_available = false;
 
-            if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms)
+            if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(d_symbol_duration_ms))
                 {
                     LOG(INFO) << "Warning: BEIDOU B3I TOW update in ch " << d_channel
                               << " does not match the TLM TOW counter " << static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n";

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc

@@ -1680,9 +1680,6 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
                                                         d_current_fpga_integration_period = d_fpga_integration_period;
                                                         d_current_extended_correlation_in_fpga = true;
 
-                                                        d_P_accu_old.real(d_P_accu_old.real() * d_fpga_integration_period);
-                                                        d_P_accu_old.imag(d_P_accu_old.imag() * d_fpga_integration_period);
-
                                                         if (d_sc_demodulate_enabled)
                                                             {
                                                                 multicorrelator_fpga->enable_secondary_codes();