Improve ANP printer

Provide output since the first synchronized satellite
Field microseconds in system time

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
     /*

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<Galileo_HAS_data>).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<uint64_t>(d_observable_interval_ms);
 
             d_gnss_observables_map.clear();
             const auto** in = reinterpret_cast<const Gnss_Synchro**>(&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<uint64_t>(d_an_rate_ms) == 0)
+                        {
+                            d_an_printer->print_packet(d_user_pvt_solver.get(), d_gnss_observables_map);
+                        }
+                }
         }
 
     return noutput_items;

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;

src/algorithms/PVT/libs/an_packet_printer.cc

@@ -30,8 +30,10 @@
 #include <unistd.h>         // for write(), read(), close()
 
 
-An_Packet_Printer::An_Packet_Printer(const std::string& an_dump_devname) : d_an_devname(an_dump_devname),
+An_Packet_Printer::An_Packet_Printer(const std::string& an_dump_devname)
-                                                                           d_an_dev_descriptor(init_serial(d_an_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<int, Gnss_Synchro>& gnss_observables_map) const
 {
+    std::chrono::time_point<std::chrono::system_clock> this_epoch;
     std::map<int, Gnss_Synchro>::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<uint32_t>(static_cast<double>(gnss_observables_iter->second.Tracking_sample_counter) / static_cast<double>(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<uint32_t>(static_cast<double>(gnss_observables_iter->second.Tracking_sample_counter) / static_cast<double>(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<double> elapsed_seconds = this_epoch - d_start;
     _packet->nsvfix = static_cast<uint8_t>(pvt->get_num_valid_observations());
     _packet->gps_satellites = num_gps_sats;
     _packet->galileo_satellites = num_gal_sats;
-    _packet->microseconds = microseconds;
+    _packet->microseconds = static_cast<uint32_t>(elapsed_seconds.count() * 1.0e6);
     _packet->latitude = static_cast<double>(pvt->get_latitude()) * (M_PI / 180.0);
     _packet->longitude = static_cast<double>(pvt->get_longitude()) * (M_PI / 180.0);
     _packet->height = static_cast<double>(pvt->get_height());

src/algorithms/PVT/libs/an_packet_printer.h

@@ -23,6 +23,7 @@
 
 #include "gnss_synchro.h"
 #include <array>
+#include <chrono>
 #include <cstddef>
 #include <cstdint>
 #include <map>
@@ -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<std::chrono::system_clock> d_start;
     std::string d_an_devname;
     int d_an_dev_descriptor;  // serial device descriptor (i.e. COM port)
 };