mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 20:20:35 +00:00
Final bug fix for the observables clock correction jumps
This commit is contained in:
parent
12aae85f3c
commit
f2becaa7b9
File diff suppressed because it is too large
Load Diff
@ -208,8 +208,11 @@ private:
|
|||||||
bool d_kml_output_enabled;
|
bool d_kml_output_enabled;
|
||||||
bool d_nmea_output_file_enabled;
|
bool d_nmea_output_file_enabled;
|
||||||
|
|
||||||
std::shared_ptr<Rtklib_Solver> d_pvt_solver;
|
std::shared_ptr<Rtklib_Solver> d_internal_pvt_solver;
|
||||||
|
std::shared_ptr<Rtklib_Solver> d_user_pvt_solver;
|
||||||
|
|
||||||
|
int32_t max_obs_block_rx_clock_offset_ms;
|
||||||
|
bool d_waiting_obs_block_rx_clock_offset_correction_msg;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map;
|
std::map<int, Gnss_Synchro> gnss_observables_map;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map_t0;
|
std::map<int, Gnss_Synchro> gnss_observables_map_t0;
|
||||||
std::map<int, Gnss_Synchro> gnss_observables_map_t1;
|
std::map<int, Gnss_Synchro> gnss_observables_map_t1;
|
||||||
|
@ -40,6 +40,7 @@ Pvt_Conf::Pvt_Conf()
|
|||||||
geojson_rate_ms = 1000;
|
geojson_rate_ms = 1000;
|
||||||
nmea_rate_ms = 1000;
|
nmea_rate_ms = 1000;
|
||||||
|
|
||||||
|
max_obs_block_rx_clock_offset_ms = 40;
|
||||||
rinex_version = 0;
|
rinex_version = 0;
|
||||||
rinexobs_rate_ms = 0;
|
rinexobs_rate_ms = 0;
|
||||||
|
|
||||||
|
@ -73,6 +73,8 @@ public:
|
|||||||
bool xml_output_enabled;
|
bool xml_output_enabled;
|
||||||
bool rtcm_output_file_enabled;
|
bool rtcm_output_file_enabled;
|
||||||
|
|
||||||
|
int32_t max_obs_block_rx_clock_offset_ms;
|
||||||
|
|
||||||
std::string output_path;
|
std::string output_path;
|
||||||
std::string rinex_output_path;
|
std::string rinex_output_path;
|
||||||
std::string gpx_output_path;
|
std::string gpx_output_path;
|
||||||
|
@ -93,8 +93,7 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in,
|
|||||||
d_dump_filename = std::move(dump_filename);
|
d_dump_filename = std::move(dump_filename);
|
||||||
d_nchannels_out = nchannels_out;
|
d_nchannels_out = nchannels_out;
|
||||||
d_nchannels_in = nchannels_in;
|
d_nchannels_in = nchannels_in;
|
||||||
T_rx_offset_ms = 0;
|
d_gnss_synchro_history = std::make_shared<Gnss_circular_deque<Gnss_Synchro>>(1000, d_nchannels_out);
|
||||||
d_gnss_synchro_history = std::make_shared<Gnss_circular_deque<Gnss_Synchro>>(500, d_nchannels_out);
|
|
||||||
|
|
||||||
// ############# ENABLE DATA FILE LOG #################
|
// ############# ENABLE DATA FILE LOG #################
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
@ -141,7 +140,6 @@ hybrid_observables_gs::hybrid_observables_gs(uint32_t nchannels_in,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
T_rx_TOW_ms = 0U;
|
T_rx_TOW_ms = 0U;
|
||||||
T_rx_remnant_to_20ms = 0;
|
|
||||||
T_rx_step_ms = 20; // read from config at the adapter GNSS-SDR.observable_interval_ms!!
|
T_rx_step_ms = 20; // read from config at the adapter GNSS-SDR.observable_interval_ms!!
|
||||||
T_rx_TOW_set = false;
|
T_rx_TOW_set = false;
|
||||||
T_status_report_timer_ms = 0;
|
T_status_report_timer_ms = 0;
|
||||||
@ -197,16 +195,14 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
|
|||||||
{
|
{
|
||||||
double new_rx_clock_offset_s;
|
double new_rx_clock_offset_s;
|
||||||
new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
|
new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
|
||||||
T_rx_offset_ms = new_rx_clock_offset_s * 1000.0;
|
T_rx_TOW_ms = T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0));
|
||||||
T_rx_TOW_ms = T_rx_TOW_ms - static_cast<int>(round(T_rx_offset_ms));
|
|
||||||
T_rx_remnant_to_20ms = (T_rx_TOW_ms % 20);
|
|
||||||
// d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
|
// d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
|
||||||
for (uint32_t n = 0; n < d_nchannels_out; n++)
|
for (uint32_t n = 0; n < d_nchannels_out; n++)
|
||||||
{
|
{
|
||||||
d_gnss_synchro_history->clear(n);
|
d_gnss_synchro_history->clear(n);
|
||||||
}
|
}
|
||||||
|
|
||||||
LOG(INFO) << "Corrected new RX Time offset: " << T_rx_offset_ms << "[ms]";
|
LOG(INFO) << "Corrected new RX Time offset: " << static_cast<int>(round(new_rx_clock_offset_s * 1000.0)) << "[ms]";
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
catch (boost::bad_any_cast &e)
|
catch (boost::bad_any_cast &e)
|
||||||
@ -489,8 +485,7 @@ void hybrid_observables_gs::update_TOW(const std::vector<Gnss_Synchro> &data)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
T_rx_TOW_ms = TOW_ref - (TOW_ref % 20);
|
T_rx_TOW_ms = TOW_ref;
|
||||||
T_rx_remnant_to_20ms = 0;
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@ -509,7 +504,7 @@ void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data)
|
|||||||
// std::cout.precision(17);
|
// std::cout.precision(17);
|
||||||
// std::cout << " T_rx_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) << std::endl;
|
// std::cout << " T_rx_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) << std::endl;
|
||||||
std::vector<Gnss_Synchro>::iterator it;
|
std::vector<Gnss_Synchro>::iterator it;
|
||||||
double current_T_rx_TOW_ms = (static_cast<double>(T_rx_TOW_ms - T_rx_remnant_to_20ms));
|
double current_T_rx_TOW_ms = static_cast<double>(T_rx_TOW_ms);
|
||||||
double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0;
|
double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0;
|
||||||
for (it = data.begin(); it != data.end(); it++)
|
for (it = data.begin(); it != data.end(); it++)
|
||||||
{
|
{
|
||||||
@ -582,9 +577,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
|
|||||||
for (uint32_t n = 0; n < d_nchannels_out; n++)
|
for (uint32_t n = 0; n < d_nchannels_out; n++)
|
||||||
{
|
{
|
||||||
Gnss_Synchro interpolated_gnss_synchro{};
|
Gnss_Synchro interpolated_gnss_synchro{};
|
||||||
|
if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front()))
|
||||||
uint32_t T_rx_remnant_to_20ms_samples = T_rx_remnant_to_20ms * in[d_nchannels_in - 1][0].fs / 1000;
|
|
||||||
if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front() - T_rx_remnant_to_20ms_samples))
|
|
||||||
{
|
{
|
||||||
// Produce an empty observation
|
// Produce an empty observation
|
||||||
interpolated_gnss_synchro = Gnss_Synchro();
|
interpolated_gnss_synchro = Gnss_Synchro();
|
||||||
@ -666,7 +659,17 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
|
|||||||
d_dump = false;
|
d_dump = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return 1;
|
|
||||||
|
if (n_valid > 0)
|
||||||
|
{
|
||||||
|
// LOG(INFO) << "OBS: diff time: " << out[0][0].RX_time * 1000.0 - old_time_debug;
|
||||||
|
// old_time_debug = out[0][0].RX_time * 1000.0;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -87,12 +87,11 @@ private:
|
|||||||
bool d_dump;
|
bool d_dump;
|
||||||
bool d_dump_mat;
|
bool d_dump_mat;
|
||||||
uint32_t T_rx_TOW_ms;
|
uint32_t T_rx_TOW_ms;
|
||||||
uint32_t T_rx_remnant_to_20ms;
|
|
||||||
uint32_t T_rx_step_ms;
|
uint32_t T_rx_step_ms;
|
||||||
uint32_t T_status_report_timer_ms;
|
uint32_t T_status_report_timer_ms;
|
||||||
uint32_t d_nchannels_in;
|
uint32_t d_nchannels_in;
|
||||||
uint32_t d_nchannels_out;
|
uint32_t d_nchannels_out;
|
||||||
double T_rx_offset_ms;
|
//double T_rx_offset_ms;
|
||||||
std::string d_dump_filename;
|
std::string d_dump_filename;
|
||||||
std::ofstream d_dump_file;
|
std::ofstream d_dump_file;
|
||||||
boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history
|
boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history
|
||||||
|
Loading…
Reference in New Issue
Block a user