From d4a1bb1147a9c0677f3cdc85ba32c660c3b1f3f5 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 22 Oct 2021 13:54:48 +0200 Subject: [PATCH] Improve ANP printer Provide output since the first synchronized satellite Field microseconds in system time --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 2 +- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 16 +++++++++------- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 1 + src/algorithms/PVT/libs/an_packet_printer.cc | 14 ++++++++------ src/algorithms/PVT/libs/an_packet_printer.h | 2 ++ 5 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 748ce5264..5620d3a51 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -150,7 +150,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, pvt_output_parameters.gpx_rate_ms = bc::lcm(configuration->property(role + ".gpx_rate_ms", pvt_output_parameters.gpx_rate_ms), pvt_output_parameters.output_rate_ms); pvt_output_parameters.geojson_rate_ms = bc::lcm(configuration->property(role + ".geojson_rate_ms", pvt_output_parameters.geojson_rate_ms), pvt_output_parameters.output_rate_ms); pvt_output_parameters.nmea_rate_ms = bc::lcm(configuration->property(role + ".nmea_rate_ms", pvt_output_parameters.nmea_rate_ms), pvt_output_parameters.output_rate_ms); - pvt_output_parameters.an_rate_ms = bc::lcm(configuration->property(role + ".an_rate_ms", pvt_output_parameters.an_rate_ms), pvt_output_parameters.output_rate_ms); + pvt_output_parameters.an_rate_ms = configuration->property(role + ".an_rate_ms", pvt_output_parameters.an_rate_ms); // Infer the type of receiver /* diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index eb73b3192..6b61e7473 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -134,6 +134,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_galileo_has_data_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_rinex_version(conf_.rinex_version), d_rx_time(0.0), + d_local_counter_ms(0ULL), d_rinexobs_rate_ms(conf_.rinexobs_rate_ms), d_kml_rate_ms(conf_.kml_rate_ms), d_gpx_rate_ms(conf_.gpx_rate_ms), @@ -1901,6 +1902,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item bool flag_write_RTCM_1045_output = false; bool flag_write_RTCM_MSM_output = false; bool flag_write_RINEX_obs_output = false; + d_local_counter_ms += static_cast(d_observable_interval_ms); d_gnss_observables_map.clear(); const auto** in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer @@ -2244,13 +2246,6 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item flag_write_RTCM_1045_output, d_enable_rx_clock_correction); } - if (d_an_printer_enabled) - { - if (current_RX_time_ms % d_an_rate_ms == 0) - { - d_an_printer->print_packet(d_user_pvt_solver.get(), d_gnss_observables_map); - } - } } } @@ -2326,6 +2321,13 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item } } } + if (d_an_printer_enabled) + { + if (d_local_counter_ms % static_cast(d_an_rate_ms) == 0) + { + d_an_printer->print_packet(d_user_pvt_solver.get(), d_gnss_observables_map); + } + } } return noutput_items; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 14e1c7ac7..26bf51d8b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -233,6 +233,7 @@ private: double d_rinex_version; double d_rx_time; + uint64_t d_local_counter_ms; key_t d_sysv_msg_key; int d_sysv_msqid; diff --git a/src/algorithms/PVT/libs/an_packet_printer.cc b/src/algorithms/PVT/libs/an_packet_printer.cc index c8f6cc808..5ee613943 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.cc +++ b/src/algorithms/PVT/libs/an_packet_printer.cc @@ -30,8 +30,10 @@ #include // for write(), read(), close() -An_Packet_Printer::An_Packet_Printer(const std::string& an_dump_devname) : d_an_devname(an_dump_devname), - d_an_dev_descriptor(init_serial(d_an_devname)) +An_Packet_Printer::An_Packet_Printer(const std::string& an_dump_devname) + : d_start(std::chrono::system_clock::now()), + d_an_devname(an_dump_devname), + d_an_dev_descriptor(init_serial(d_an_devname)) { if (d_an_dev_descriptor != -1) { @@ -91,10 +93,10 @@ void An_Packet_Printer::close_serial() const */ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const Rtklib_Solver* const pvt, const std::map& gnss_observables_map) const { + std::chrono::time_point this_epoch; std::map::const_iterator gnss_observables_iter; uint8_t num_gps_sats = 0; uint8_t num_gal_sats = 0; - uint32_t microseconds = 0; int index = 0; const int max_reported_sats = *(&_packet->sats + 1) - _packet->sats; @@ -128,7 +130,6 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const } _packet->sats[index].doppler = doppler; - microseconds = static_cast(static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs)) * 1e6; index++; } break; @@ -154,7 +155,6 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const } _packet->sats[index].doppler = doppler; - microseconds = static_cast(static_cast(gnss_observables_iter->second.Tracking_sample_counter) / static_cast(gnss_observables_iter->second.fs)) * 1e6; index++; } break; @@ -164,10 +164,12 @@ void An_Packet_Printer::update_sdr_gnss_packet(sdr_gnss_packet_t* _packet, const } } + this_epoch = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = this_epoch - d_start; _packet->nsvfix = static_cast(pvt->get_num_valid_observations()); _packet->gps_satellites = num_gps_sats; _packet->galileo_satellites = num_gal_sats; - _packet->microseconds = microseconds; + _packet->microseconds = static_cast(elapsed_seconds.count() * 1.0e6); _packet->latitude = static_cast(pvt->get_latitude()) * (M_PI / 180.0); _packet->longitude = static_cast(pvt->get_longitude()) * (M_PI / 180.0); _packet->height = static_cast(pvt->get_height()); diff --git a/src/algorithms/PVT/libs/an_packet_printer.h b/src/algorithms/PVT/libs/an_packet_printer.h index 3f8526524..596c33f26 100644 --- a/src/algorithms/PVT/libs/an_packet_printer.h +++ b/src/algorithms/PVT/libs/an_packet_printer.h @@ -23,6 +23,7 @@ #include "gnss_synchro.h" #include +#include #include #include #include @@ -121,6 +122,7 @@ private: void encode_sdr_gnss_packet(sdr_gnss_packet_t* sdr_gnss_packet, an_packet_t* _packet) const; void LSB_bytes_to_array(void* _in, int offset, uint8_t* _out, uint8_t var_size) const; + std::chrono::time_point d_start; std::string d_an_devname; int d_an_dev_descriptor; // serial device descriptor (i.e. COM port) };