mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-04 17:16:26 +00:00
Code Issues Releases Wiki Activity

Observables and all PVT products now are referenced to the uncorrected RX clock, that is guaranteed to be integer multiple of 1 ms

Browse Source
This commit is contained in:
Javier Arribas 2018-06-02 12:55:00 +02:00
parent 03c7278b27
commit 908aa1515f
16 changed files with 116 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -339,16 +339,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
d_ls_pvt->set_averaging_depth(1); d_ls_pvt->set_averaging_depth(1);
d_rx_time = 0.0; d_rx_time = 0.0;
last_pvt_display_T_rx_s = 0.0;
last_RTCM_1019_output_time = 0.0;
last_RTCM_1020_output_time = 0.0;
last_RTCM_1045_output_time = 0.0;
last_RTCM_1077_output_time = 0.0;
last_RTCM_1087_output_time = 0.0;
last_RTCM_1097_output_time = 0.0;
last_RTCM_MSM_output_time = 0.0;
last_RINEX_obs_output_time = 0.0;
last_RINEX_nav_output_time = 0.0;
b_rinex_header_written = false; b_rinex_header_written = false;
b_rinex_header_updated = false; b_rinex_header_updated = false;
@ -600,84 +590,75 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
// ############ 2 COMPUTE THE PVT ################################ // ############ 2 COMPUTE THE PVT ################################
if (gnss_observables_map.size() > 0) if (gnss_observables_map.size() > 0)
{ {
// correct the observable to account for the receiver clock offset double current_RX_time = gnss_observables_map.begin()->second.RX_time;
//observables fix unsigned int current_RX_time_ms = static_cast<unsigned int>(current_RX_time * 1000.0);
//std::cout << TEXT_RED << "(internal) accumulated RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]" << TEXT_RESET << std::endl; //if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast<double>(d_output_rate_ms))
for (std::map<int, Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) if (current_RX_time_ms % d_output_rate_ms == 0)
{
//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;//+ 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))
{ {
flag_compute_pvt_output = true; flag_compute_pvt_output = true;
d_rx_time = current_RX_time; d_rx_time = current_RX_time;
//std::cout.precision(17);
//std::cout << "current_RX_time: " << current_RX_time << " map time: " << gnss_observables_map.begin()->second.RX_time << std::endl;
} }
// compute on the fly PVT solution // compute on the fly PVT solution
if (flag_compute_pvt_output == true) if (flag_compute_pvt_output == true)
{ {
if (d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false)) // correct the observable to account for the receiver clock offset
//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)
{ {
if (std::fabs(current_RX_time - last_pvt_display_T_rx_s) * 1000.0 >= static_cast<double>(d_display_rate_ms)) //todo: check if it has effect to correct the receiver time for the internal pvt solution
// take into account that the RINEX obs with the RX time (integer ms) CAN NOT be corrected to keep the coherence in obs time
it->second.Pseudorange_m = it->second.Pseudorange_m; // - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
}
if (d_ls_pvt->get_PVT(gnss_observables_map, false))
{
if (current_RX_time_ms % d_display_rate_ms == 0)
{ {
flag_display_pvt = true; flag_display_pvt = true;
last_pvt_display_T_rx_s = current_RX_time;
} }
if ((std::fabs(current_RX_time - last_RTCM_1019_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1019_rate_ms)) and (d_rtcm_MT1019_rate_ms != 0)) // allows deactivating messages by setting rate = 0 if (current_RX_time_ms % d_rtcm_MT1019_rate_ms == 0 and d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0
{ {
flag_write_RTCM_1019_output = true; flag_write_RTCM_1019_output = true;
last_RTCM_1019_output_time = current_RX_time;
} }
if ((std::fabs(current_RX_time - last_RTCM_1020_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1020_rate_ms)) and (d_rtcm_MT1020_rate_ms != 0)) // allows deactivating messages by setting rate = 0 if (current_RX_time_ms % d_rtcm_MT1020_rate_ms == 0 and d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0
{ {
flag_write_RTCM_1020_output = true; flag_write_RTCM_1020_output = true;
last_RTCM_1020_output_time = current_RX_time;
} }
if ((std::fabs(current_RX_time - last_RTCM_1045_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1045_rate_ms)) and (d_rtcm_MT1045_rate_ms != 0)) if (current_RX_time_ms % d_rtcm_MT1045_rate_ms == 0 and d_rtcm_MT1045_rate_ms != 0)
{ {
flag_write_RTCM_1045_output = true; flag_write_RTCM_1045_output = true;
last_RTCM_1045_output_time = current_RX_time;
} }
if ((std::fabs(current_RX_time - last_RTCM_1077_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1077_rate_ms)) and (d_rtcm_MT1077_rate_ms != 0)) // if (current_RX_time_ms % d_rtcm_MT1077_rate_ms==0 and d_rtcm_MT1077_rate_ms != 0)
{ // {
last_RTCM_1077_output_time = current_RX_time; // last_RTCM_1077_output_time = current_RX_time;
} // }
if ((std::fabs(current_RX_time - last_RTCM_1087_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1087_rate_ms)) and (d_rtcm_MT1087_rate_ms != 0)) // if (current_RX_time_ms % d_rtcm_MT1087_rate_ms==0 and d_rtcm_MT1087_rate_ms != 0)
{ // {
last_RTCM_1087_output_time = current_RX_time; // last_RTCM_1087_output_time = current_RX_time;
} // }
if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1097_rate_ms)) and (d_rtcm_MT1097_rate_ms != 0)) // if (current_RX_time_ms % d_rtcm_MT1097_rate_ms==0 and d_rtcm_MT1097_rate_ms != 0)
{ // {
last_RTCM_1097_output_time = current_RX_time; // last_RTCM_1097_output_time = current_RX_time;
} // }
if ((std::fabs(current_RX_time - last_RTCM_MSM_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MSM_rate_ms)) and (d_rtcm_MSM_rate_ms != 0)) if (current_RX_time_ms % d_rtcm_MSM_rate_ms == 0 and d_rtcm_MSM_rate_ms != 0)
{ {
flag_write_RTCM_MSM_output = true; flag_write_RTCM_MSM_output = true;
last_RTCM_MSM_output_time = current_RX_time;
} }
if ((std::fabs(current_RX_time - last_RINEX_obs_output_time) >= 1.0)) // TODO: Make it configurable if (current_RX_time_ms % 1000 == 0) // TODO: Make it configurable
{ {
flag_write_RINEX_obs_output = true; flag_write_RINEX_obs_output = true;
last_RINEX_obs_output_time = current_RX_time;
} }
if ((std::fabs(current_RX_time - last_RINEX_nav_output_time) >= 6.0)) // TODO: Make it configurable if (current_RX_time_ms % 6000 == 0) // TODO: Make it configurable
{ {
flag_write_RINEX_nav_output = true; flag_write_RINEX_nav_output = true;
last_RINEX_nav_output_time = current_RX_time;
} }
// 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;
// }
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())
@ -2090,7 +2071,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_ls_pvt->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_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::from_time_t(rtklib_utc_time.time);
p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6)); 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; 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())

11
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@ -126,16 +126,7 @@ private:
std::shared_ptr<GeoJSON_Printer> d_geojson_printer; std::shared_ptr<GeoJSON_Printer> d_geojson_printer;
std::shared_ptr<Rtcm_Printer> d_rtcm_printer; std::shared_ptr<Rtcm_Printer> d_rtcm_printer;
double d_rx_time; double d_rx_time;
double last_pvt_display_T_rx_s;
double last_RTCM_1019_output_time;
double last_RTCM_1020_output_time;
double last_RTCM_1045_output_time;
double last_RTCM_1077_output_time;
double last_RTCM_1087_output_time;
double last_RTCM_1097_output_time;
double last_RTCM_MSM_output_time;
double last_RINEX_obs_output_time;
double last_RINEX_nav_output_time;
std::shared_ptr<rtklib_solver> d_ls_pvt; std::shared_ptr<rtklib_solver> d_ls_pvt;
std::map<int, Gnss_Synchro> gnss_observables_map; std::map<int, Gnss_Synchro> gnss_observables_map;

11
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -133,7 +133,7 @@ double rtklib_solver::get_vdop() const
} }
bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging) bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging)
{ {
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter; std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter; std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@ -501,10 +501,13 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
//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(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)); 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; DLOG(INFO) << "RTKLIB Position at RX TOW = " << gnss_observables_map.begin()->second.RX_time
<< " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
boost::posix_time::ptime p_time; boost::posix_time::ptime p_time;
gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); //gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time);
gtime_t rtklib_utc_time = gpst2time(adjgpsweek(nav_data.eph[0].week), gnss_observables_map.begin()->second.RX_time);
p_time = boost::posix_time::from_time_t(rtklib_utc_time.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)); p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
this->set_position_UTC_time(p_time); this->set_position_UTC_time(p_time);
@ -523,7 +526,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
{ {
double tmp_double; double tmp_double;
// PVT GPS time // PVT GPS time
tmp_double = Rx_time; tmp_double = gnss_observables_map.begin()->second.RX_time;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position East [m] // ECEF User Position East [m]
tmp_double = rx_position_and_time(0); tmp_double = rx_position_and_time(0);

2
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -85,7 +85,7 @@ public:
rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk); rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk);
~rtklib_solver(); ~rtklib_solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging); bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging);
double get_hdop() const; double get_hdop() const;
double get_vdop() const; double get_vdop() const;
double get_pdop() const; double get_pdop() const;

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

@ -62,7 +62,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
d_nchannels = nchannels_out; d_nchannels = nchannels_out;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
T_rx_s = 0.0; T_rx_s = 0.0;
T_rx_step_s = 0.001; // 1 ms T_rx_step_ms = 1; // 1 ms
max_delta = 1.5; // 1.5 s max_delta = 1.5; // 1.5 s
d_latency = 0.5; // 300 ms d_latency = 0.5; // 300 ms
valid_channels.resize(d_nchannels, false); valid_channels.resize(d_nchannels, false);
@ -87,8 +87,8 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
} }
} }
} }
T_rx_TOW_s=0.0; T_rx_TOW_ms = 0;
T_rx_TOW_set=false; T_rx_TOW_set = false;
} }
@ -310,7 +310,10 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i
} }
find_interp_elements(ch, ti); find_interp_elements(ch, ti);
//Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1) //1st: copy the nearest gnss_synchro data for that channel
out = d_gnss_synchro_history->at(ch, 0);
//2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
// CARRIER PHASE INTERPOLATION // CARRIER PHASE INTERPOLATION
out.Carrier_phase_rads = d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, 1).Carrier_phase_rads - d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads) * (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_phase_rads = d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, 1).Carrier_phase_rads - d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads) * (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);
@ -319,7 +322,12 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i
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);
// 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.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_ms) - static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms)) * (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);
//std::cout.precision(17);
//std::cout << "Diff TOW_at_current_symbol_ms(1) - out.interp_TOW_ms: " << static_cast<double>(d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_ms) - out.interp_TOW_ms << std::endl;
return true; return true;
} }
@ -417,8 +425,8 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
{ {
if (it->PRN == it2->PRN and it->System == it2->System) if (it->PRN == it2->PRN and it->System == it2->System)
{ {
double tow_dif_ = std::fabs(it->TOW_at_current_symbol_s - it2->TOW_at_current_symbol_s); double tow_dif_ = std::fabs(it->TOW_at_current_symbol_ms - it2->TOW_at_current_symbol_ms);
if (tow_dif_ > thr_) if (tow_dif_ > thr_ * 1000.0)
{ {
DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal
<< ". TOW difference in PRN " << it->PRN << ". TOW difference in PRN " << it->PRN
@ -433,27 +441,39 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
} }
} }
#endif #endif
/////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////
double TOW_ref = std::numeric_limits<double>::lowest();
if (!T_rx_TOW_set) if (!T_rx_TOW_set)
{ {
unsigned int TOW_ref = std::numeric_limits<unsigned int>::lowest();
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) if (it->TOW_at_current_symbol_ms > TOW_ref)
{ {
TOW_ref = it->TOW_at_current_symbol_s; TOW_ref = it->TOW_at_current_symbol_ms;
} }
} }
T_rx_TOW_s=round(TOW_ref*1000.0)/1000.0; T_rx_TOW_ms = TOW_ref;
T_rx_TOW_set=true; T_rx_TOW_set = true;
}else{ }
T_rx_TOW_s+=T_rx_step_s; else
TOW_ref=T_rx_TOW_s; {
T_rx_TOW_ms += T_rx_step_ms;
//todo: check what happen during the week rollover
if (T_rx_TOW_ms >= 604800000)
{
T_rx_TOW_ms = T_rx_TOW_ms % 604800000;
}
} }
for (it = data.begin(); it != data.end(); it++) 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 = (static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0;
//std::cout.precision(17);
//std::cout << "Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
it->RX_time = (T_rx_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
} }
} }
@ -475,7 +495,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
} }
for (int epoch = 0; epoch < ninput_items[d_nchannels]; epoch++) for (int epoch = 0; epoch < ninput_items[d_nchannels]; epoch++)
{ {
T_rx_s += T_rx_step_s; T_rx_s += (static_cast<double>(T_rx_step_ms) / 1000.0);
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
if ((total_input_items == 0) and (d_num_valid_channels == 0)) if ((total_input_items == 0) and (d_num_valid_channels == 0))
@ -557,7 +577,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
{ {
if (valid_channels[i]) if (valid_channels[i])
{ {
Gnss_Synchro interpolated_gnss_synchro = d_gnss_synchro_history->back(i); Gnss_Synchro interpolated_gnss_synchro; // empty set, it is required to COPY the nearest in the interpolation history = d_gnss_synchro_history->back(i);
if (interpolate_data(interpolated_gnss_synchro, i, T_rx_s_out)) if (interpolate_data(interpolated_gnss_synchro, i, T_rx_s_out))
{ {
epoch_data.push_back(interpolated_gnss_synchro); epoch_data.push_back(interpolated_gnss_synchro);
@ -600,7 +620,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
{ {
tmp_double = out[i][epoch].RX_time; tmp_double = out[i][epoch].RX_time;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].TOW_at_current_symbol_s; tmp_double = out[i][epoch].interp_TOW_ms / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Carrier_Doppler_hz; tmp_double = out[i][epoch].Carrier_Doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));

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

@ -76,10 +76,10 @@ private:
Gnss_circular_deque<Gnss_Synchro>* d_gnss_synchro_history; Gnss_circular_deque<Gnss_Synchro>* d_gnss_synchro_history;
boost::dynamic_bitset<> valid_channels; boost::dynamic_bitset<> valid_channels;
double T_rx_s; double T_rx_s;
double T_rx_step_s; unsigned int T_rx_step_ms;
//rx time follow GPST //rx time follow GPST
bool T_rx_TOW_set; bool T_rx_TOW_set;
double T_rx_TOW_s; unsigned int T_rx_TOW_ms;
double max_delta; double max_delta;
double d_latency; double d_latency;
bool d_dump; bool d_dump;

5
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
View File

@ -456,8 +456,9 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
current_symbol.Flag_valid_word = false; current_symbol.Flag_valid_word = false;
} }
current_symbol.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_symbol.TOW_at_current_symbol_s -= delta_t; //Galileo to GPS TOW //todo: Galileo to GPS time conversion should be moved to observable block.
//current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW
if (d_dump == true) if (d_dump == true)
{ {

2
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
View File

@ -470,7 +470,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
current_sample.Flag_valid_word = false; current_sample.Flag_valid_word = false;
} }
current_sample.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; current_sample.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
if (d_dump) if (d_dump)
{ {

5
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc
View File

@ -413,8 +413,9 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
} }
current_symbol.PRN = this->d_satellite.get_PRN(); current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW //todo: glonass time to gps time should be done in observables block
//current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW
if (d_dump == true) if (d_dump == true)
{ {

5
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_cc.cc
View File

@ -413,8 +413,9 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
} }
current_symbol.PRN = this->d_satellite.get_PRN(); current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW //todo: glonass time to gps time should be done in observables block
//current_symbol.TOW_at_current_symbol_s -= delta_t;
if (d_dump == true) if (d_dump == true)
{ {

13
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -91,7 +91,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_prev_GPS_frame_4bytes = 0; d_prev_GPS_frame_4bytes = 0;
d_flag_parity = false; d_flag_parity = false;
d_TOW_at_Preamble = 0.0; d_TOW_at_Preamble = 0.0;
d_TOW_at_current_symbol = 0.0;
flag_TOW_set = false; flag_TOW_set = false;
d_average_count = 0; d_average_count = 0;
d_flag_preamble = false; d_flag_preamble = false;
@ -396,25 +395,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
//2. Add the telemetry decoder information //2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_flag_new_tow_available == true) if (this->d_flag_preamble == true and d_flag_new_tow_available == true)
{ {
//double decoder_latency_ms=(double)(current_symbol.Tracking_sample_counter-d_symbol_history.at(0).Tracking_sample_counter)
// /(double)current_symbol.fs;
// update TOW at the preamble instant (account with decoder latency)
d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2.0 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2.0 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S;
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + 161; d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + 161;
//d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0;
d_TOW_at_current_symbol = d_TOW_at_Preamble;
flag_TOW_set = true; flag_TOW_set = true;
d_flag_new_tow_available = false; d_flag_new_tow_available = false;
} }
else else
{ {
d_TOW_at_current_symbol += GPS_L1_CA_CODE_PERIOD;
d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS;
} }
current_symbol.TOW_at_current_symbol_s = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0; current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
//current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
current_symbol.Flag_valid_word = flag_TOW_set; current_symbol.Flag_valid_word = flag_TOW_set;
if (flag_PLL_180_deg_phase_locked == true) if (flag_PLL_180_deg_phase_locked == true)
@ -430,7 +421,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
{ {
double tmp_double; double tmp_double;
unsigned long int tmp_ulong_int; unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol; tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter; tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));

1
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@ -112,7 +112,6 @@ private:
unsigned long int d_preamble_time_samples; unsigned long int d_preamble_time_samples;
double d_TOW_at_Preamble; double d_TOW_at_Preamble;
double d_TOW_at_current_symbol;
unsigned int d_TOW_at_current_symbol_ms; unsigned int d_TOW_at_current_symbol_ms;
bool flag_TOW_set; bool flag_TOW_set;

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc
View File

@ -201,7 +201,7 @@ int gps_l2c_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
d_flag_valid_word = false; d_flag_valid_word = false;
} }
} }
current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_synchro_data.Flag_valid_word = d_flag_valid_word; current_synchro_data.Flag_valid_word = d_flag_valid_word;
if (d_dump == true) if (d_dump == true)

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc
View File

@ -253,7 +253,7 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u
d_flag_valid_word = false; d_flag_valid_word = false;
} }
} }
current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_synchro_data.Flag_valid_word = d_flag_valid_word; current_synchro_data.Flag_valid_word = d_flag_valid_word;
if (d_dump == true) if (d_dump == true)

4
src/core/system_parameters/GPS_L1_CA.h
View File

@ -67,8 +67,8 @@ const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period
const double MAX_TOA_DELAY_MS = 20; const double MAX_TOA_DELAY_MS = 20;
//#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here //#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here
const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) //const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
const double GPS_STARTOFFSET_ms = 69.0;
// OBSERVABLE HISTORY DEEP FOR INTERPOLATION // OBSERVABLE HISTORY DEEP FOR INTERPOLATION
const int GPS_L1_CA_HISTORY_DEEP = 100; const int GPS_L1_CA_HISTORY_DEEP = 100;

3
src/core/system_parameters/gnss_synchro.h
View File

@ -66,12 +66,13 @@ public:
//Telemetry Decoder //Telemetry Decoder
bool Flag_valid_word; //!< Set by Telemetry Decoder processing block bool Flag_valid_word; //!< Set by Telemetry Decoder processing block
double TOW_at_current_symbol_s; //!< Set by Telemetry Decoder processing block unsigned int TOW_at_current_symbol_ms; //!< Set by Telemetry Decoder processing block
// Observables // Observables
double Pseudorange_m; //!< Set by Observables processing block double Pseudorange_m; //!< Set by Observables processing block
double RX_time; //!< Set by Observables processing block double RX_time; //!< Set by Observables processing block
bool Flag_valid_pseudorange; //!< Set by Observables processing block bool Flag_valid_pseudorange; //!< Set by Observables processing block
double interp_TOW_ms; //!< Set by Observables processing block
}; };
#endif #endif