From 650d637ee24cd9365e4522d8c09a9126d5bac7fa Mon Sep 17 00:00:00 2001
From: Javier <javiarribas@gmail.com>
Date: Wed, 22 Jan 2020 12:16:48 +0100
Subject: [PATCH] Better fix for bug fix that prevented PVT when disabled
 rx_clock_correction, issue #343

---
 .../gnuradio_blocks/hybrid_observables_gs.cc      | 15 +++++++++++----
 .../gnuradio_blocks/hybrid_observables_gs.h       |  1 -
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
index 4d04546ea..5b18a4584 100644
--- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
+++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
@@ -195,8 +195,11 @@ 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_TOW_ms = T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0));
-                    //compute the required T_rx_TOW adjustment to match integer multiple of 20 ms
-                    adjust_next_20ms = 20 - T_rx_TOW_ms % 20;
+                    //align the receiver clock to integer multiple of 20 ms
+                    if (T_rx_TOW_ms % 20)
+                        {
+                            T_rx_TOW_ms += 20 - 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++)
                         {
@@ -485,6 +488,11 @@ void hybrid_observables_gs::update_TOW(const std::vector<Gnss_Synchro> &data)
                         }
                 }
             T_rx_TOW_ms = TOW_ref;
+            //align the receiver clock to integer multiple of 20 ms
+            if (T_rx_TOW_ms % 20)
+                {
+                    T_rx_TOW_ms += 20 - T_rx_TOW_ms % 20;
+                }
         }
     else
         {
@@ -576,8 +584,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{};
-                    //align the receiver clock to integer multiple of 20 ms and interpolate observables at current receiver clock
-                    if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front() + adjust_next_20ms))
+                    if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front()))
                         {
                             // Produce an empty observation
                             interpolated_gnss_synchro = Gnss_Synchro();
diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
index 6d4dbe2ae..15c9ab722 100644
--- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
+++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
@@ -89,7 +89,6 @@ private:
     bool d_dump;
     bool d_dump_mat;
     uint32_t T_rx_TOW_ms;
-    uint32_t adjust_next_20ms;
     uint32_t T_rx_step_ms;
     uint32_t T_status_report_timer_ms;
     uint32_t d_nchannels_in;