Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

src/algorithms/PVT/adapters/rtklib_pvt.cc

@@ -536,23 +536,31 @@ RtklibPvt::~RtklibPvt()
     rtkfree(&rtk);
 }
 
-std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris()
+
+std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris() const
 {
     return pvt_->get_gps_ephemeris_map();
 }
-std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris()
+
+
+std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris() const
 {
     return pvt_->get_galileo_ephemeris_map();
 }
-std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac()
+
+
+std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac() const
 {
     return pvt_->get_gps_almanac_map();
 }
-std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac()
+
+
+std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac() const
 {
     return pvt_->get_galileo_almanac_map();
 }
 
+
 void RtklibPvt::connect(gr::top_block_sptr top_block)
 {
     if (top_block)

src/algorithms/PVT/adapters/rtklib_pvt.h

@@ -64,10 +64,10 @@ public:
     }
 
     void clear_ephemeris() override;
-    std::map<int, Gps_Ephemeris> get_gps_ephemeris() override;
+    std::map<int, Gps_Ephemeris> get_gps_ephemeris() const override;
-    std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() override;
+    std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() const override;
-    std::map<int, Gps_Almanac> get_gps_almanac() override;
+    std::map<int, Gps_Almanac> get_gps_almanac() const override;
-    std::map<int, Galileo_Almanac> get_galileo_almanac() override;
+    std::map<int, Galileo_Almanac> get_galileo_almanac() const override;
 
     void connect(gr::top_block_sptr top_block) override;
     void disconnect(gr::top_block_sptr top_block) override;

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h

@@ -111,8 +111,9 @@ private:
     bool d_geojson_output_enabled;
     bool d_gpx_output_enabled;
     bool d_kml_output_enabled;
+    bool d_nmea_output_file_enabled;
 
-    std::shared_ptr<rtklib_solver> d_ls_pvt;
+    std::shared_ptr<rtklib_solver> d_pvt_solver;
 
     std::map<int, Gnss_Synchro> gnss_observables_map;
     bool observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b);
@@ -152,13 +153,13 @@ public:
      * \brief Get latest set of ephemeris from PVT block
      *
      */
-    std::map<int, Gps_Ephemeris> get_gps_ephemeris_map();
+    std::map<int, Gps_Ephemeris> get_gps_ephemeris_map() const;
 
-    std::map<int, Gps_Almanac> get_gps_almanac_map();
+    std::map<int, Gps_Almanac> get_gps_almanac_map() const;
 
-    std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map();
+    std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map() const;
 
-    std::map<int, Galileo_Almanac> get_galileo_almanac_map();
+    std::map<int, Galileo_Almanac> get_galileo_almanac_map() const;
 
     /*!
      * \brief Clear all ephemeris information and the almanacs for GPS and Galileo
@@ -166,7 +167,6 @@ public:
      */
     void clear_ephemeris();
 
-
     /*!
      * \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available
      */

src/algorithms/PVT/libs/hybrid_ls_pvt.cc

@@ -338,7 +338,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
                     boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
                     this->set_position_UTC_time(p_time);
 
-                    cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
+                    cart_to_geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
 
                     DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
                                << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()

src/algorithms/PVT/libs/pvt_solution.cc

@@ -87,49 +87,13 @@ arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec
 }
 
 
-int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
+int Pvt_Solution::cart_to_geo(double X, double Y, double Z, int elipsoid_selection)
 {
-    /* Conversion of Cartesian coordinates (X,Y,Z) to geographical
+    arma::vec XYZ = {X, Y, Z};
-     coordinates (latitude, longitude, h) on a selected reference ellipsoid.
+    arma::vec LLH = cart2geo(XYZ, elipsoid_selection);
-
+    d_latitude_d = LLH(0) * 180.0 / GPS_PI;
-       Choices of Reference Ellipsoid for Geographical Coordinates
+    d_longitude_d = LLH(1) * 180.0 / GPS_PI;
-                 0. International Ellipsoid 1924
+    d_height_m = LLH(2);
-                 1. International Ellipsoid 1967
-                 2. World Geodetic System 1972
-                 3. Geodetic Reference System 1980
-                 4. World Geodetic System 1984
-     */
-
-    const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
-    const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
-
-    double lambda = atan2(Y, X);
-    double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
-    double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
-    double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
-
-    double h = 0.1;
-    double oldh = 0.0;
-    double N;
-    int iterations = 0;
-    do
-        {
-            oldh = h;
-            N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
-            phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
-            h = sqrt(X * X + Y * Y) / cos(phi) - N;
-            iterations = iterations + 1;
-            if (iterations > 100)
-                {
-                    DLOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
-                    break;
-                }
-        }
-    while (std::abs(h - oldh) > 1.0e-12);
-    d_latitude_d = phi * 180.0 / GPS_PI;
-    d_longitude_d = lambda * 180.0 / GPS_PI;
-    d_height_m = h;
-    //todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
     return 0;
 }
 

src/algorithms/PVT/libs/pvt_solution.h

@@ -127,7 +127,7 @@ public:
       * 4 - World Geodetic System 1984.
       *
       */
-    int cart2geo(double X, double Y, double Z, int elipsoid_selection);
+    int cart_to_geo(double X, double Y, double Z, int elipsoid_selection);
 
     /*!
       * \brief Tropospheric correction

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -99,6 +99,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
         }
 }
 
+
 bool rtklib_solver::save_matfile()
 {
     // READ DUMP FILE
@@ -395,6 +396,7 @@ bool rtklib_solver::save_matfile()
     return true;
 }
 
+
 rtklib_solver::~rtklib_solver()
 {
     if (d_dump_file.is_open() == true)
@@ -868,7 +870,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                     p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
                     p_time += boost::posix_time::microseconds(static_cast<long>(round(rtklib_utc_time.sec * 1e6)));
                     this->set_position_UTC_time(p_time);
-                    cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
+                    cart_to_geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
 
                     DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time)
                                << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()

src/algorithms/libs/geofunctions.cc

@@ -301,7 +301,7 @@ double mstokph(double MetersPerSeconds)
 arma::vec CTM_to_Euler(const arma::mat &C)
 {
     // Calculate Euler angles using (2.23)
-    arma::mat CTM = C;
+    arma::mat CTM(C);
     arma::vec eul = arma::zeros(3, 1);
     eul(0) = atan2(CTM(1, 2), CTM(2, 2));  // roll
     if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0;

src/core/interfaces/pvt_interface.h

@@ -57,10 +57,10 @@ class PvtInterface : public GNSSBlockInterface
 public:
     virtual void reset() = 0;
     virtual void clear_ephemeris() = 0;
-    virtual std::map<int, Gps_Ephemeris> get_gps_ephemeris() = 0;
+    virtual std::map<int, Gps_Ephemeris> get_gps_ephemeris() const = 0;
-    virtual std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() = 0;
+    virtual std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() const = 0;
-    virtual std::map<int, Gps_Almanac> get_gps_almanac() = 0;
+    virtual std::map<int, Gps_Almanac> get_gps_almanac() const = 0;
-    virtual std::map<int, Galileo_Almanac> get_galileo_almanac() = 0;
+    virtual std::map<int, Galileo_Almanac> get_galileo_almanac() const = 0;
 
     virtual bool get_latest_PVT(double* longitude_deg,
         double* latitude_deg,

src/core/receiver/control_thread.cc

@@ -94,6 +94,7 @@ ControlThread::ControlThread(std::shared_ptr<ConfigurationInterface> configurati
 {
     configuration_ = configuration;
     delete_configuration_ = false;
+    restart_ = false;
     init();
 }
 
@@ -112,6 +113,7 @@ void ControlThread::telecommand_listener()
     cmd_interface_.run_cmd_server(tcp_cmd_port);
 }
 
+
 /*
  * Runs the control thread that manages the receiver control plane
  *
@@ -154,17 +156,16 @@ int ControlThread::run()
             return 0;
         }
 
-    //launch GNSS assistance process AFTER the flowgraph is running because the GNURadio asynchronous queues must be already running to transport msgs
+    // launch GNSS assistance process AFTER the flowgraph is running because the GNU Radio asynchronous queues must be already running to transport msgs
     assist_GNSS();
     // start the keyboard_listener thread
     keyboard_thread_ = boost::thread(&ControlThread::keyboard_listener, this);
     sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
 
-    //start the telecommand listener thread
+    // start the telecommand listener thread
     cmd_interface_.set_pvt(flowgraph_->get_pvt());
     cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);
 
-
     bool enable_FPGA = configuration_->property("Channel.enable_FPGA", false);
     if (enable_FPGA == true)
         {
@@ -764,22 +765,23 @@ void ControlThread::apply_action(unsigned int what)
         }
 }
 
-std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, arma::vec LLH)
+
+std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, const arma::vec &LLH)
 {
-    //1. Compute rx ECEF position from LLH WGS84
+    // 1. Compute rx ECEF position from LLH WGS84
     arma::vec LLH_rad = arma::vec{degtorad(LLH(0)), degtorad(LLH(1)), LLH(2)};
     arma::mat C_tmp = arma::zeros(3, 3);
     arma::vec r_eb_e = arma::zeros(3, 1);
     arma::vec v_eb_e = arma::zeros(3, 1);
     Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp);
 
-    //2. Compute rx GPS time from UTC time
+    // 2. Compute rx GPS time from UTC time
     gtime_t utc_gtime;
     utc_gtime.time = rx_utc_time;
     utc_gtime.sec = 0;
     gtime_t gps_gtime = utc2gpst(utc_gtime);
 
-    //2. loop throught all the available ephemeris or almanac and compute satellite positions and elevations
+    // 3. loop through all the available ephemeris or almanac and compute satellite positions and elevations
     // store visible satellites in a vector of pairs <int,Gnss_Satellite> to associate an elevation to the each satellite
     std::vector<std::pair<int, Gnss_Satellite>> available_satellites;
 
@@ -805,7 +807,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
             arma::vec dx = r_sat_eb_e - r_eb_e;
             topocent(&Az, &El, &dist_m, r_eb_e, dx);
-            //push sat
+            // push sat
             if (El > 0)
                 {
                     std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@@ -827,7 +829,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
             arma::vec dx = r_sat_eb_e - r_eb_e;
             topocent(&Az, &El, &dist_m, r_eb_e, dx);
-            //push sat
+            // push sat
             if (El > 0)
                 {
                     std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@@ -848,7 +850,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
             arma::vec dx = r_sat_eb_e - r_eb_e;
             topocent(&Az, &El, &dist_m, r_eb_e, dx);
-            //push sat
+            // push sat
             if (El > 0)
                 {
                     std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@@ -870,7 +872,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
             arma::vec dx = r_sat_eb_e - r_eb_e;
             topocent(&Az, &El, &dist_m, r_eb_e, dx);
             std::cout << "Using Galileo Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
-            //push sat
+            // push sat
             if (El > 0)
                 {
                     std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@@ -879,11 +881,11 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
                 }
         }
 
-    //sort the visible satellites in ascending order of elevation
+    // sort the visible satellites in ascending order of elevation
     std::sort(available_satellites.begin(), available_satellites.end(), [](const std::pair<int, Gnss_Satellite> &a, const std::pair<int, Gnss_Satellite> &b) {  // use lambda. Cleaner and easier to read
         return a.first < b.first;
     });
-    //std::reverse(available_satellites.begin(), available_satellites.end());
+    // std::reverse(available_satellites.begin(), available_satellites.end());
     return available_satellites;
 }
 

src/core/receiver/control_thread.h

@@ -148,8 +148,8 @@ private:
      * Compute elevations for the specified time and position for all the available satellites in ephemeris and almanac queues
      * returns a vector filled with the available satellites ordered from high elevation to low elevation angle.
      */
+    std::vector<std::pair<int, Gnss_Satellite>> get_visible_sats(time_t rx_utc_time, const arma::vec& LLH);
 
-    std::vector<std::pair<int, Gnss_Satellite>> get_visible_sats(time_t rx_utc_time, arma::vec LLH);
     /*
      * Read initial GNSS assistance from SUPL server or local XML files
      */