mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 15:23:04 +00:00
Code Issues Releases Wiki Activity

Fix observables clock drift bug (Candidate one)

Browse Source
This commit is contained in:
Javier Arribas
2018-05-18 21:07:12 +02:00
parent 2b7663e70c
commit 6d4a89148a
3 changed files with 38 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

@@ -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; //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) 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; 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)) 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 // 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) if (first_fix == true)
{ {
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) 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()) 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() << " 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; << " [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; 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()) 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() << " 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]"; << " [deg], Height= " << d_ls_pvt->get_height() << " [m]";

38
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@@ -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 // 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); 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 // 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); 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 #endif
/////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////
double TOW_ref = std::numeric_limits<double>::lowest(); 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++) 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; it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
} }
} }

3
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
View File

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