Set configuration of RTCM messages rate

src/algorithms/PVT/adapters/hybrid_pvt.cc

@@ -35,6 +35,7 @@
 #include <glog/logging.h>
 #include <boost/archive/xml_oarchive.hpp>
 #include <boost/archive/xml_iarchive.hpp>
+#include <boost/math/common_factor_rt.hpp>
 #include <boost/serialization/map.hpp>
 #include "configuration_interface.h"
 
@@ -57,34 +58,44 @@ HybridPvt::HybridPvt(ConfigurationInterface* configuration,
     DLOG(INFO) << "role " << role;
     dump_ = configuration->property(role + ".dump", false);
     dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
+
     // moving average depth parameters
-    int averaging_depth;
+    int averaging_depth = configuration->property(role + ".averaging_depth", 10);
-    averaging_depth = configuration->property(role + ".averaging_depth", 10);
+    bool flag_averaging = configuration->property(role + ".flag_averaging", false);
-    bool flag_averaging;
+
-    flag_averaging = configuration->property(role + ".flag_averaging", false);
     // output rate
-    int output_rate_ms;
+    int output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
-    output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
+
     // display rate
-    int display_rate_ms;
+    int display_rate_ms = configuration->property(role + ".display_rate_ms", 500);
-    display_rate_ms = configuration->property(role + ".display_rate_ms", 500);
+
     // NMEA Printer settings
-    bool flag_nmea_tty_port;
+    bool flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false);
-    flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false);
+    std::string nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename);
-    std::string nmea_dump_filename;
+    std::string nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname);
-    nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename);
+
-    std::string nmea_dump_devname;
-    nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname);
     // RTCM Printer settings
-    bool flag_rtcm_tty_port;
+    bool flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false);
-    flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false);
+    std::string rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname);
-    std::string rtcm_dump_devname;
+    bool flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false);
-    rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname);
-    bool flag_rtcm_server;
-    flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false);
     unsigned short rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101);
     unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234);
-    
+    // RTCM message rates: least common multiple with output_rate_ms
+    int rtcm_MT1019_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), output_rate_ms);
+    int rtcm_MT1045_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), output_rate_ms);
+    int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms);
+    int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms);
+    int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms);
+    std::map<int,int> rtcm_msg_rate_ms;
+    rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms;
+    for (int k = 1071; k < 1078; k++) // All GPS MSM
+        {
+            rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms;
+        }
+    for (int k = 1091; k < 1098; k++) // All Galileo MSM
+        {
+            rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms;
+        }
     // getting names from the config file, if available
     // default filename for assistance data
     const std::string eph_default_xml_filename = "./gps_ephemeris.xml";
@@ -99,7 +110,7 @@ HybridPvt::HybridPvt(ConfigurationInterface* configuration,
     //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename);    
     
     // make PVT object
-    pvt_ = hybrid_make_pvt_cc(in_streams_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_dump_devname);
+    pvt_ = hybrid_make_pvt_cc(in_streams_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname);
     DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
 }
 

src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc

@@ -33,6 +33,7 @@
 #include <iostream>
 #include <map>
 #include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/math/common_factor_rt.hpp>
 #include <gnuradio/gr_complex.h>
 #include <gnuradio/io_signature.h>
 #include <glog/logging.h>
@@ -55,6 +56,7 @@ hybrid_make_pvt_cc(unsigned int nchannels,
         bool flag_rtcm_tty_port,
         unsigned short rtcm_tcp_port,
         unsigned short rtcm_station_id,
+        std::map<int,int> rtcm_msg_rate_ms,
         std::string rtcm_dump_devname)
 {
     return hybrid_pvt_cc_sptr(new hybrid_pvt_cc(nchannels,
@@ -71,6 +73,7 @@ hybrid_make_pvt_cc(unsigned int nchannels,
             flag_rtcm_tty_port,
             rtcm_tcp_port,
             rtcm_station_id,
+            rtcm_msg_rate_ms,
             rtcm_dump_devname));
 }
 
@@ -168,7 +171,7 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump
         int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port,
         std::string nmea_dump_filename, std::string nmea_dump_devname,
         bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port,
-        unsigned short rtcm_station_id, std::string rtcm_dump_devname) :
+        unsigned short rtcm_station_id, std::map<int,int> rtcm_msg_rate_ms, std::string rtcm_dump_devname) :
                 gr::block("hybrid_pvt_cc", gr::io_signature::make(nchannels, nchannels,  sizeof(Gnss_Synchro)),
                 gr::io_signature::make(0, 0, sizeof(gr_complex)))
 
@@ -203,6 +206,39 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump
     std::string rtcm_dump_filename;
     rtcm_dump_filename = d_dump_filename;
     d_rtcm_printer = std::make_shared<Rtcm_Printer>(rtcm_dump_filename, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_dump_devname);
+    if(rtcm_msg_rate_ms.find(1019) != rtcm_msg_rate_ms.end())
+        {
+            d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms[1019];
+        }
+    else
+        {
+            d_rtcm_MT1019_rate_ms = boost::math::lcm(5000, d_output_rate_ms);  // default value if not set
+        }
+    if(rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end())
+        {
+            d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045];
+        }
+    else
+        {
+            d_rtcm_MT1045_rate_ms = boost::math::lcm(5000, d_output_rate_ms);  // default value if not set
+        }
+    if(rtcm_msg_rate_ms.find(1077) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077
+        {
+            d_rtcm_MT1077_rate_ms = rtcm_msg_rate_ms[1077];
+        }
+    else
+        {
+            d_rtcm_MT1077_rate_ms = boost::math::lcm(1000, d_output_rate_ms);  // default value if not set
+        }
+    if(rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077
+        {
+            d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097];
+        }
+    else
+        {
+            d_rtcm_MT1097_rate_ms = boost::math::lcm(1000, d_output_rate_ms);  // default value if not set
+        }
+    b_rtcm_writing_started = false;
 
     d_dump_filename.append("_raw.dat");
     dump_ls_pvt_filename.append("_ls_pvt.dat");
@@ -273,6 +309,8 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
 {
     d_sample_counter++;
     bool arrived_galileo_almanac = false;
+    unsigned int gps_channel = 0;
+    unsigned int gal_channel = 0;
 
     gnss_pseudoranges_map.clear();
 
@@ -280,14 +318,31 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
 
     print_receiver_status(in);
 
+    // ############ 1. READ PSEUDORANGES ####
     for (unsigned int i = 0; i < d_nchannels; i++)
         {
             if (in[i][0].Flag_valid_pseudorange == true)
                 {
-                    gnss_pseudoranges_map.insert(std::pair<int,Gnss_Synchro>(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map
+                    gnss_pseudoranges_map.insert(std::pair<int,Gnss_Synchro>(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map. PROBLEM: sats with the same PRN!!
                     //d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges)
                     d_TOW_at_curr_symbol_constellation = in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug)
                     d_rx_time = in[i][0].d_TOW_hybrid_at_current_symbol; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges)
+                    if(d_ls_pvt->gps_ephemeris_map.size() > 0)
+                        {
+                            std::map<int,Gps_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN);
+                            if(tmp_eph_iter != d_ls_pvt->gps_ephemeris_map.end())
+                                {
+                                    d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time
+                                }
+                        }
+                    if(d_ls_pvt->galileo_ephemeris_map.size() > 0)
+                        {
+                            std::map<int,Galileo_Ephemeris>::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN);
+                            if(tmp_eph_iter != d_ls_pvt->galileo_ephemeris_map.end())
+                                {
+                                    d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time
+                                }
+                        }
                 }
         }
 
@@ -349,6 +404,26 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
                                             b_rinex_header_updated = true;
                                         }
                                 }
+
+                            if(!b_rtcm_writing_started) // the first time
+                                {
+                                    if(d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0
+                                        {
+                                            for(std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ )
+                                                {
+                                                    d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second);
+                                                }
+                                        }
+                                    if(d_rtcm_MT1045_rate_ms != 0)
+                                        {
+                                            for(std::map<int,Galileo_Ephemeris>::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ )
+                                                {
+                                                    d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second);
+                                                }
+                                        }
+                                    // TODO: print observables...
+                                    // b_rtcm_writing_started = true;
+                                }
                         }
                 }
 

src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h

@@ -61,6 +61,7 @@ hybrid_pvt_cc_sptr hybrid_make_pvt_cc(unsigned int n_channels,
                                               bool flag_rtcm_tty_port,
                                               unsigned short rtcm_tcp_port,
                                               unsigned short rtcm_station_id,
+                                              std::map<int,int> rtcm_msg_rate_ms,
                                               std::string rtcm_dump_devname);
 
 /*!
@@ -83,6 +84,7 @@ private:
                                                          bool flag_rtcm_tty_port,
                                                          unsigned short rtcm_tcp_port,
                                                          unsigned short rtcm_station_id,
+                                                         std::map<int,int> rtcm_msg_rate_ms,
                                                          std::string rtcm_dump_devname);
     hybrid_pvt_cc(unsigned int nchannels,
                       bool dump, std::string dump_filename,
@@ -97,6 +99,7 @@ private:
                       bool flag_rtcm_tty_port,
                       unsigned short rtcm_tcp_port,
                       unsigned short rtcm_station_id,
+                      std::map<int,int> rtcm_msg_rate_ms,
                       std::string rtcm_dump_devname);
 
     void msg_handler_telemetry(pmt::pmt_t msg);
@@ -104,6 +107,11 @@ private:
     bool d_dump;
     bool b_rinex_header_writen;
     bool b_rinex_header_updated;
+    bool b_rtcm_writing_started;
+    int d_rtcm_MT1045_rate_ms;
+    int d_rtcm_MT1019_rate_ms;
+    int d_rtcm_MT1077_rate_ms;
+    int d_rtcm_MT1097_rate_ms;
 
     void print_receiver_status(Gnss_Synchro** channels_synchronization_data);
     int d_last_status_print_seg; //for status printer