diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 8bcc49a88..5a25f24e2 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/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::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(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::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]"; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 06332cc30..f7fb73dc9 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -481,13 +481,26 @@ bool rtklib_solver::get_PVT(const std::map& 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(1) = pvt_sol.rr[1]; - rx_position_and_time(2) = pvt_sol.rr[2]; - rx_position_and_time(3) = pvt_sol.dtr[0]; + rx_position_and_time(0) = pvt_sol.rr[0]; //[m] + rx_position_and_time(1) = pvt_sol.rr[1]; //[m] + rx_position_and_time(2) = pvt_sol.rr[2]; //[m] + + //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(); - 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] + //observable fix: + //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; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index aa7d49cd7..2c3a718d2 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/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::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(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; - it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0; + //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(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; } }