Fix in rx time correction and adding the missing code phase to the observables. More test required

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc

@@ -600,6 +600,13 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
             // ############ 2 COMPUTE THE PVT ################################
             if (gnss_observables_map.size() > 0)
                 {
+                    // correct the observable to account for the receiver clock offset
+                    //observables fix
+                    //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.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;
 
                     if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast<double>(d_output_rate_ms))
@@ -665,7 +672,6 @@ 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)
                                         {
                                             it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
@@ -2075,7 +2081,7 @@ 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;
                             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/PVT/libs/rtklib_solver.cc

@@ -481,13 +481,26 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
 
                     this->set_valid_position(true);
                     arma::vec rx_position_and_time(4);
-                    rx_position_and_time(0) = pvt_sol.rr[0];
+                    rx_position_and_time(0) = pvt_sol.rr[0];  //[m]
-                    rx_position_and_time(1) = pvt_sol.rr[1];
+                    rx_position_and_time(1) = pvt_sol.rr[1];  //[m]
-                    rx_position_and_time(2) = pvt_sol.rr[2];
+                    rx_position_and_time(2) = pvt_sol.rr[2];  //[m]
-                    rx_position_and_time(3) = pvt_sol.dtr[0];
+
+                    //todo: fix this ambiguity in the RTKLIB units in receiver clock offset!
+                    if (rtk_.opt.mode == PMODE_SINGLE)
+                        {
+                            rx_position_and_time(3) = pvt_sol.dtr[0];  //if the RTKLIB solver is set to SINGLE, the dtr is already expressed in [s]
+                        }
+                    else
+                        {
+                            rx_position_and_time(3) = pvt_sol.dtr[0] / GPS_C_m_s;  // the receiver clock offset is expressed in [meters]
+                        }
+                    //[s]
                     this->set_rx_pos(rx_position_and_time.rows(0, 2));  // save ECEF position for the next iteration
-                    double offset_s = this->get_time_offset_s();
+                    //observable fix:
-                    this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s));  // accumulate the rx time error for the next iteration [meters]->[seconds]
+                    //double offset_s = this->get_time_offset_s();
+                    //this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s));  // accumulate the rx time error for the next iteration [meters]->[seconds]
+                    this->set_time_offset_s(rx_position_and_time(3));
+
                     DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
 
                     boost::posix_time::ptime p_time;

src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc

@@ -316,6 +316,10 @@ 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);
 
@@ -434,17 +438,22 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
     ///////////////////////////////////////////////////////////
 
     double TOW_ref = std::numeric_limits<double>::lowest();
+    double Code_phase_ref_s = 0.0;
     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;
+                    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 + GPS_STARTOFFSET_ms / 1000.0;
-            it->RX_time = TOW_ref + 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;
         }
 }