Fix observables clock drift bug (Candidate one)

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc

@@ -605,9 +605,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
                     //std::cout << TEXT_RED << "(internal) accumulated RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]" << TEXT_RESET << std::endl;
                     for (std::map<int, Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
                         {
+                    		//it->second.RX_time+=d_ls_pvt->get_time_offset_s();
                             it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
                         }
-                    double current_RX_time = gnss_observables_map.begin()->second.RX_time;
+
+                    double current_RX_time = gnss_observables_map.begin()->second.RX_time;//+ d_ls_pvt->get_time_offset_s();
 
                     if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast<double>(d_output_rate_ms))
                         {
@@ -672,10 +674,10 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
                                         }
 
                                     // correct the observable to account for the receiver clock offset
-                                    for (std::map<int, Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
+                                    //for (std::map<int, Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
-                                        {
+                                     //   {
-                                            it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
+                                     //       it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
-                                        }
+                                     //   }
                                     if (first_fix == true)
                                         {
                                             std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
@@ -2081,7 +2083,16 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
                             std::cout << TEXT_BOLD_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
                                       << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
                                       << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl;
+
                             std::cout << TEXT_RED << "accumulated RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]" << TEXT_RESET << std::endl;
+
+                            boost::posix_time::ptime p_time;
+                            gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_ls_pvt->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_time);
+                            p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
+                            p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
+                            std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time)<< TEXT_RESET << std::endl;
+
+
                             LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
                                       << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
                                       << " [deg], Height= " << d_ls_pvt->get_height() << " [m]";

src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc

@@ -87,6 +87,8 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
                         }
                 }
         }
+	T_rx_TOW_s=0.0;
+	T_rx_TOW_set=false;
 }
 
 
@@ -316,10 +318,6 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i
     // CARRIER DOPPLER INTERPOLATION
     out.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, 1).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
 
-    // todo: CODE PHASE IS MISSING!
-    // todo: convert to seconds, considering fs per channel
-    out.Code_phase_samples = d_gnss_synchro_history->at(ch, 0).Code_phase_samples + (d_gnss_synchro_history->at(ch, 1).Code_phase_samples - d_gnss_synchro_history->at(ch, 0).Code_phase_samples) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
-
     // TOW INTERPOLATION
     out.TOW_at_current_symbol_s = d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_s + (d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_s - d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_s) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
 
@@ -436,24 +434,26 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
         }
 #endif
     ///////////////////////////////////////////////////////////
-
     double TOW_ref = std::numeric_limits<double>::lowest();
-    double Code_phase_ref_s = 0.0;
+    if (!T_rx_TOW_set)
+    {
+		for (it = data.begin(); it != data.end(); it++)
+			{
+				if (it->TOW_at_current_symbol_s > TOW_ref)
+					{
+						TOW_ref = it->TOW_at_current_symbol_s;
+					}
+			}
+		T_rx_TOW_s=round(TOW_ref*1000.0)/1000.0;
+		T_rx_TOW_set=true;
+    }else{
+    	T_rx_TOW_s+=T_rx_step_s;
+    	TOW_ref=T_rx_TOW_s;
+    }
     for (it = data.begin(); it != data.end(); it++)
         {
-            if (it->TOW_at_current_symbol_s > TOW_ref)
+            double traveltime_s = TOW_ref - it->TOW_at_current_symbol_s + GPS_STARTOFFSET_ms / 1000.0;
-                {
+            it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0;
-                    TOW_ref = it->TOW_at_current_symbol_s;
-                    Code_phase_ref_s = it->Code_phase_samples / static_cast<double>(it->fs);
-                }
-        }
-    for (it = data.begin(); it != data.end(); it++)
-        {
-            //double traveltime_s = TOW_ref - it->TOW_at_current_symbol_s + GPS_STARTOFFSET_ms / 1000.0;
-
-            double traveltime_s = TOW_ref - it->TOW_at_current_symbol_s + Code_phase_ref_s - it->Code_phase_samples / static_cast<double>(it->fs) + GPS_STARTOFFSET_ms / 1000.0;
-            //it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0;
-            it->RX_time = TOW_ref + Code_phase_ref_s + GPS_STARTOFFSET_ms / 1000.0;
             it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
         }
 }

src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h

@@ -77,6 +77,9 @@ private:
     boost::dynamic_bitset<> valid_channels;
     double T_rx_s;
     double T_rx_step_s;
+    //rx time follow GPST
+    bool T_rx_TOW_set;
+    double T_rx_TOW_s;
     double max_delta;
     double d_latency;
     bool d_dump;