mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 12:10:34 +00:00
Code Issues Releases Wiki Activity

Bug fix: PVT does not resolve position anymore after a loss of samples event

Browse Source
This commit is contained in:
Javier Arribas 2022-06-30 10:38:29 +02:00
parent 0eb21b8359
commit a7147702bc
4 changed files with 42 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -166,11 +166,13 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
d_flag_monitor_pvt_enabled(conf_.monitor_enabled),
d_flag_monitor_ephemeris_enabled(conf_.monitor_ephemeris_enabled),
d_show_local_time_zone(conf_.show_local_time_zone),
d_waiting_obs_block_rx_clock_offset_correction_msg(false),
d_timestamp_rx_clock_offset_correction_msg_ms(0LL),
d_enable_rx_clock_correction(conf_.enable_rx_clock_correction),
d_an_printer_enabled(conf_.an_output_enabled),
d_log_timetag(conf_.log_source_timetag)
{
//debug
d_pvt_errors_counter = 0;
// Send feedback message to observables block with the receiver clock offset
this->message_port_register_out(pmt::mp("pvt_to_observables"));
// Experimental: VLT commands from PVT to tracking channels
@ -2168,18 +2170,18 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
if (fabs(Rx_clock_offset_s) * 1000.0 > d_max_obs_block_rx_clock_offset_ms)
{
if (!d_waiting_obs_block_rx_clock_offset_correction_msg)
//check if the message was just sent to not duplicate it while it is being applied
if ((d_local_counter_ms - d_timestamp_rx_clock_offset_correction_msg_ms) > 1000)
{
this->message_port_pub(pmt::mp("pvt_to_observables"), pmt::make_any(Rx_clock_offset_s));
d_waiting_obs_block_rx_clock_offset_correction_msg = true;
LOG(INFO) << "Sent clock offset correction to observables: " << Rx_clock_offset_s << "[s]";
d_timestamp_rx_clock_offset_correction_msg_ms = d_local_counter_ms;
LOG(INFO) << "PVT: Sent clock offset correction to observables: " << Rx_clock_offset_s << "[s]";
}
}
else
{
if (d_enable_rx_clock_correction == true)
{
d_waiting_obs_block_rx_clock_offset_correction_msg = false;
d_gnss_observables_map_t0 = d_gnss_observables_map_t1;
apply_rx_clock_offset(d_gnss_observables_map, Rx_clock_offset_s);
d_gnss_observables_map_t1 = d_gnss_observables_map;
@ -2209,6 +2211,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
}
else
{
d_pvt_errors_counter = 0;
d_rx_time = d_gnss_observables_map.begin()->second.RX_time;
current_RX_time_ms = static_cast<uint32_t>(d_rx_time * 1000.0);
if (current_RX_time_ms % d_output_rate_ms == 0)
@ -2221,6 +2224,18 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
}
}
}
else
{
//sanity check: If the PVT solver is getting 100 consecutive errors, send a reset command to observables block
d_pvt_errors_counter++;
if (d_pvt_errors_counter >= 100)
{
int command = 1;
this->message_port_pub(pmt::mp("pvt_to_observables"), pmt::make_any(command));
LOG(INFO) << "PVT: Number of consecutive position solver error reached, Sent reset to observables.";
d_pvt_errors_counter = 0;
}
}
// compute on the fly PVT solution
if (flag_compute_pvt_output == true)

3
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@ -282,7 +282,8 @@ private:
bool d_flag_monitor_pvt_enabled;
bool d_flag_monitor_ephemeris_enabled;
bool d_show_local_time_zone;
bool d_waiting_obs_block_rx_clock_offset_correction_msg;
uint64_t d_timestamp_rx_clock_offset_correction_msg_ms;
uint32_t d_pvt_errors_counter;
bool d_enable_rx_clock_correction;
bool d_enable_has_messages;
bool d_an_printer_enabled;

20
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@ -226,6 +226,25 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
LOG(INFO) << "Corrected new RX Time offset: " << static_cast<int>(round(new_rx_clock_offset_s * 1000.0)) << "[ms]";
}
if (pmt::any_ref(msg).type().hash_code() == d_int_type_hash_code)
{
const auto command_from_pvt = wht::any_cast<int>(pmt::any_ref(msg));
switch (command_from_pvt)
{
case 1: //reset TOW
d_T_rx_TOW_ms = 0;
d_last_rx_clock_round20ms_error = 0;
d_T_rx_TOW_set = false;
for (uint32_t n = 0; n < d_nchannels_out; n++)
{
d_gnss_synchro_history->clear(n);
}
LOG(INFO) << "Received reset observables TOW command from PVT";
break;
default:
break;
}
}
}
catch (const wht::bad_any_cast &e)
{
@ -786,7 +805,6 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
}
epoch_data[n] = interpolated_gnss_synchro;
}
if (d_T_rx_TOW_set)
{
update_TOW(epoch_data);

1
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
View File

@ -70,6 +70,7 @@ private:
explicit hybrid_observables_gs(const Obs_Conf& conf_);
const size_t d_double_type_hash_code = typeid(double).hash_code();
const size_t d_int_type_hash_code = typeid(int).hash_code();
void msg_handler_pvt_to_observables(const pmt::pmt_t& msg);
double compute_T_rx_s(const Gnss_Synchro& a) const;