Merge branch 'fix_dop' into next

src/algorithms/PVT/libs/gpx_printer.cc

@@ -98,7 +98,9 @@ bool Gpx_Printer::set_headers(std::string filename, bool time_tag_name)
                      << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
                      << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
                      << "<trk>" << std::endl
-                     << "<trkseg>" << std::endl;
+                     << indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << std::endl
+                     << indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << std::endl
+                     << indent << "<trkseg>" << std::endl;
             return true;
         }
     else
@@ -108,15 +110,21 @@ bool Gpx_Printer::set_headers(std::string filename, bool time_tag_name)
 }
 
 
-bool Gpx_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values)
+bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position, bool print_average_values)
 {
     double latitude;
     double longitude;
     double height;
 
     positions_printed = true;
+    std::shared_ptr<rtklib_solver> position_ = position;
 
-    std::shared_ptr<Pvt_Solution> position_ = position;
+    double hdop = position_->get_hdop();
+    double vdop = position_->get_vdop();
+    double pdop = position_->get_pdop();
+    std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time());
+    utc_time.resize(23);   // time up to ms
+    utc_time.append("Z");  // UTC time zone
 
     if (print_average_values == false)
         {
@@ -133,7 +141,9 @@ bool Gpx_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
 
     if (gpx_file.is_open())
         {
-            gpx_file << "<trkpt lon=\"" << longitude << "\" lat=\"" << latitude << "\"><ele>" << height << "</ele></trkpt>" << std::endl;
+            gpx_file << indent << indent << "<trkpt lon=\"" << longitude << "\" lat=\"" << latitude << "\"><ele>" << height << "</ele>"
+                     << "<time>" << utc_time << "</time>"
+                     << "<hdop>" << hdop << "</hdop><vdop>" << vdop << "</vdop><pdop>" << pdop << "</pdop></trkpt>" << std::endl;
             return true;
         }
     else
@@ -147,7 +157,7 @@ bool Gpx_Printer::close_file()
 {
     if (gpx_file.is_open())
         {
-            gpx_file << "</trkseg>" << std::endl
+            gpx_file << indent << "</trkseg>" << std::endl
                      << "</trk>" << std::endl
                      << "</gpx>";
             gpx_file.close();
@@ -163,6 +173,7 @@ bool Gpx_Printer::close_file()
 Gpx_Printer::Gpx_Printer()
 {
     positions_printed = false;
+    indent = "  ";
 }
 
 

src/algorithms/PVT/libs/gpx_printer.h

@@ -34,6 +34,7 @@
 #define GNSS_SDR_GPX_PRINTER_H_
 
 #include "pvt_solution.h"
+#include "rtklib_solver.h"
 #include <fstream>
 #include <memory>
 #include <string>
@@ -50,12 +51,13 @@ private:
     std::ofstream gpx_file;
     bool positions_printed;
     std::string gpx_filename;
+    std::string indent;
 
 public:
     Gpx_Printer();
     ~Gpx_Printer();
     bool set_headers(std::string filename, bool time_tag_name = true);
-    bool print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values);
+    bool print_position(const std::shared_ptr<rtklib_solver>& position, bool print_average_values);
     bool close_file();
 };
 

src/algorithms/PVT/libs/hybrid_ls_pvt.cc

@@ -350,9 +350,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
                                << " [deg], Height= " << this->get_height() << " [m]"
                                << " RX time offset= " << this->get_time_offset_s() << " [s]";
 
-                    // ###### Compute DOPs ########
-                    hybrid_ls_pvt::compute_DOP();
-
                     // ######## LOG FILE #########
                     if (d_flag_dump_enabled == true)
                         {

src/algorithms/PVT/libs/ls_pvt.cc

@@ -281,7 +281,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
         }
 
     //-- compute the Dilution Of Precision values
-    this->set_Q(arma::inv(arma::htrans(A) * A));
+    //this->set_Q(arma::inv(arma::htrans(A) * A));
 
     // check the consistency of the PVT solution
     if (((fabs(pos(3)) * 1000.0) / GPS_C_m_s) > GPS_STARTOFFSET_ms * 2)

src/algorithms/PVT/libs/nmea_printer.cc

@@ -125,7 +125,7 @@ void Nmea_Printer::close_serial()
 }
 
 
-bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<Pvt_Solution>& pvt_data, bool print_average_values)
+bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr<rtklib_solver>& pvt_data, bool print_average_values)
 {
     std::string GPRMC;
     std::string GPGGA;
@@ -432,9 +432,9 @@ std::string Nmea_Printer::get_GPGSA()
     // GSA-GNSS DOP and Active Satellites
     bool valid_fix = d_PVT_data->is_valid_position();
     int n_sats_used = d_PVT_data->get_num_valid_observations();
-    double pdop = d_PVT_data->get_PDOP();
+    double pdop = d_PVT_data->get_pdop();
-    double hdop = d_PVT_data->get_HDOP();
+    double hdop = d_PVT_data->get_hdop();
-    double vdop = d_PVT_data->get_VDOP();
+    double vdop = d_PVT_data->get_vdop();
 
     std::stringstream sentence_str;
     std::string sentence_header;
@@ -603,7 +603,7 @@ std::string Nmea_Printer::get_GPGGA()
     //boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
     bool valid_fix = d_PVT_data->is_valid_position();
     int n_channels = d_PVT_data->get_num_valid_observations();  //d_nchannels
-    double hdop = d_PVT_data->get_HDOP();
+    double hdop = d_PVT_data->get_hdop();
     double MSL_altitude;
 
     if (d_PVT_data->is_averaging() == true)

src/algorithms/PVT/libs/nmea_printer.h

@@ -36,7 +36,7 @@
 #ifndef GNSS_SDR_NMEA_PRINTER_H_
 #define GNSS_SDR_NMEA_PRINTER_H_
 
-#include "pvt_solution.h"
+#include "rtklib_solver.h"
 #include <fstream>
 #include <string>
 
@@ -58,7 +58,7 @@ public:
     /*!
      * \brief Print NMEA PVT and satellite info to the initialized device
      */
-    bool Print_Nmea_Line(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values);
+    bool Print_Nmea_Line(const std::shared_ptr<rtklib_solver>& position, bool print_average_values);
 
     /*!
      * \brief Default destructor.
@@ -70,7 +70,7 @@ private:
     std::ofstream nmea_file_descriptor;  // Output file stream for NMEA log file
     std::string nmea_devname;
     int nmea_dev_descriptor;  // NMEA serial device descriptor (i.e. COM port)
-    std::shared_ptr<Pvt_Solution> d_PVT_data;
+    std::shared_ptr<rtklib_solver> d_PVT_data;
     int init_serial(std::string serial_device);  //serial port control
     void close_serial();
     std::string get_GPGGA();  // fix data

src/algorithms/PVT/libs/pvt_solution.cc

@@ -46,11 +46,6 @@ Pvt_Solution::Pvt_Solution()
     d_avg_latitude_d = 0.0;
     d_avg_longitude_d = 0.0;
     d_avg_height_m = 0.0;
-    d_GDOP = 0.0;
-    d_PDOP = 0.0;
-    d_HDOP = 0.0;
-    d_VDOP = 0.0;
-    d_TDOP = 0.0;
     d_flag_averaging = false;
     b_valid_position = false;
     d_averaging_depth = 0;
@@ -445,50 +440,6 @@ int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &x
 }
 
 
-int Pvt_Solution::compute_DOP()
-{
-    // ###### Compute DOPs ########
-
-    // 1- Rotation matrix from ECEF coordinates to ENU coordinates
-    // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
-    arma::mat F = arma::zeros(3, 3);
-    F(0, 0) = -sin(GPS_TWO_PI * (d_longitude_d / 360.0));
-    F(0, 1) = -sin(GPS_TWO_PI * (d_latitude_d / 360.0)) * cos(GPS_TWO_PI * (d_longitude_d / 360.0));
-    F(0, 2) = cos(GPS_TWO_PI * (d_latitude_d / 360.0)) * cos(GPS_TWO_PI * (d_longitude_d / 360.0));
-
-    F(1, 0) = cos((GPS_TWO_PI * d_longitude_d) / 360.0);
-    F(1, 1) = -sin((GPS_TWO_PI * d_latitude_d) / 360.0) * sin((GPS_TWO_PI * d_longitude_d) / 360.0);
-    F(1, 2) = cos((GPS_TWO_PI * d_latitude_d / 360.0)) * sin((GPS_TWO_PI * d_longitude_d) / 360.0);
-
-    F(2, 0) = 0;
-    F(2, 1) = cos((GPS_TWO_PI * d_latitude_d) / 360.0);
-    F(2, 2) = sin((GPS_TWO_PI * d_latitude_d / 360.0));
-
-    // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
-    arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
-    arma::mat DOP_ENU = arma::zeros(3, 3);
-
-    try
-        {
-            DOP_ENU = arma::htrans(F) * Q_ECEF * F;
-            d_GDOP = sqrt(arma::trace(DOP_ENU));                           // Geometric DOP
-            d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2));  // PDOP
-            d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1));                  // HDOP
-            d_VDOP = sqrt(DOP_ENU(2, 2));                                  // VDOP
-            d_TDOP = sqrt(d_Q(3, 3));                                      // TDOP
-        }
-    catch (const std::exception &ex)
-        {
-            d_GDOP = -1;  // Geometric DOP
-            d_PDOP = -1;  // PDOP
-            d_HDOP = -1;  // HDOP
-            d_VDOP = -1;  // VDOP
-            d_TDOP = -1;  // TDOP
-        }
-    return 0;
-}
-
-
 void Pvt_Solution::set_averaging_depth(int depth)
 {
     d_averaging_depth = depth;
@@ -824,39 +775,3 @@ double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const
             return d_visible_satellites_CN0_dB[index];
         }
 }
-
-
-void Pvt_Solution::set_Q(const arma::mat &Q)
-{
-    d_Q = Q;
-}
-
-
-double Pvt_Solution::get_GDOP() const
-{
-    return d_GDOP;
-}
-
-
-double Pvt_Solution::get_PDOP() const
-{
-    return d_PDOP;
-}
-
-
-double Pvt_Solution::get_HDOP() const
-{
-    return d_HDOP;
-}
-
-
-double Pvt_Solution::get_VDOP() const
-{
-    return d_VDOP;
-}
-
-
-double Pvt_Solution::get_TDOP() const
-{
-    return d_TDOP;
-}

src/algorithms/PVT/libs/pvt_solution.h

@@ -70,13 +70,6 @@ private:
     boost::posix_time::ptime d_position_UTC_time;
     int d_valid_observations;
 
-    arma::mat d_Q;
-    double d_GDOP;
-    double d_PDOP;
-    double d_HDOP;
-    double d_VDOP;
-    double d_TDOP;
-
     int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {};          // Array with the IDs of the valid satellites
     double d_visible_satellites_El[PVT_MAX_CHANNELS] = {};        // Array with the LOS Elevation of the valid satellites
     double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {};        // Array with the LOS Azimuth of the valid satellites
@@ -130,16 +123,6 @@ public:
     bool is_averaging() const;
     void set_averaging_flag(bool flag);
 
-    // DOP estimations
-    void set_Q(const arma::mat &Q);
-    int compute_DOP();  //!< Compute Dilution Of Precision parameters
-
-    double get_GDOP() const;
-    double get_PDOP() const;
-    double get_HDOP() const;
-    double get_VDOP() const;
-    double get_TDOP() const;
-
     arma::vec rotateSatellite(double traveltime, const arma::vec &X_sat);
 
     /*!

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -70,7 +70,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
     count_valid_position = 0;
     this->set_averaging_flag(false);
     rtk_ = rtk;
-
+    for (unsigned int i = 0; i > 4; i++) dop_[i] = 0.0;
     pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0};
 
     // ############# ENABLE DATA FILE LOG #################
@@ -109,6 +109,30 @@ rtklib_solver::~rtklib_solver()
 }
 
 
+double rtklib_solver::get_gdop() const
+{
+    return dop_[0];
+}
+
+
+double rtklib_solver::get_pdop() const
+{
+    return dop_[1];
+}
+
+
+double rtklib_solver::get_hdop() const
+{
+    return dop_[2];
+}
+
+
+double rtklib_solver::get_vdop() const
+{
+    return dop_[3];
+}
+
+
 bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging)
 {
     std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
@@ -435,6 +459,26 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
                 {
                     this->set_num_valid_observations(rtk_.sol.ns);  //record the number of valid satellites used by the PVT solver
                     pvt_sol = rtk_.sol;
+                    // DOP computation
+                    unsigned int used_sats = 0;
+                    for (unsigned int i = 0; i < MAXSAT; i++)
+                        {
+                            if (int vsat = rtk_.ssat[i].vsat[0] == 1) used_sats++;
+                        }
+
+                    double azel[used_sats * 2];
+                    unsigned int index_aux = 0;
+                    for (unsigned int i = 0; i < MAXSAT; i++)
+                        {
+                            if (int vsat = rtk_.ssat[i].vsat[0] == 1)
+                                {
+                                    azel[2 * index_aux] = rtk_.ssat[i].azel[0];
+                                    azel[2 * index_aux + 1] = rtk_.ssat[i].azel[1];
+                                    index_aux++;
+                                }
+                        }
+                    if (index_aux > 0) dops(index_aux, azel, 0.0, dop_);
+
                     this->set_valid_position(true);
                     arma::vec rx_position_and_time(4);
                     rx_position_and_time(0) = pvt_sol.rr[0];

src/algorithms/PVT/libs/rtklib_solver.h

@@ -79,11 +79,17 @@ private:
     sol_t pvt_sol;
     bool d_flag_dump_enabled;
     int d_nchannels;  // Number of available channels for positioning
+    double dop_[4];
+
 public:
     rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk);
     ~rtklib_solver();
 
     bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging);
+    double get_hdop() const;
+    double get_vdop() const;
+    double get_pdop() const;
+    double get_gdop() const;
     
     std::map<int, Galileo_Ephemeris> galileo_ephemeris_map;            //!< Map storing new Galileo_Ephemeris
     std::map<int, Gps_Ephemeris> gps_ephemeris_map;                    //!< Map storing new GPS_Ephemeris

src/tests/CMakeLists.txt

@@ -308,6 +308,7 @@ include_directories(
      ${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp
      ${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-supl
      ${CMAKE_SOURCE_DIR}/src/algorithms/libs
+     ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib
      ${CMAKE_SOURCE_DIR}/src/algorithms/data_type_adapter/adapters
      ${CMAKE_SOURCE_DIR}/src/algorithms/data_type_adapter/gnuradio_blocks
      ${CMAKE_SOURCE_DIR}/src/algorithms/resampler/adapters

src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc

@@ -38,8 +38,10 @@
 TEST(NmeaPrinterTest, PrintLine)
 {
     std::string filename("nmea_test.nmea");
-
+    rtk_t rtk;
-    std::shared_ptr<Pvt_Solution> pvt_solution = std::make_shared<Pvt_Solution>();
+    prcopt_t rtklib_configuration_options;
+    rtkinit(&rtk, &rtklib_configuration_options);
+    std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, rtk);
 
     boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
         boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));  // example from http://aprs.gids.nl/nmea/#rmc
@@ -77,8 +79,10 @@ TEST(NmeaPrinterTest, PrintLine)
 TEST(NmeaPrinterTest, PrintLineLessthan10min)
 {
     std::string filename("nmea_test.nmea");
-
+    rtk_t rtk;
-    std::shared_ptr<Pvt_Solution> pvt_solution = std::make_shared<Pvt_Solution>();
+    prcopt_t rtklib_configuration_options;
+    rtkinit(&rtk, &rtklib_configuration_options);
+    std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, rtk);
 
     boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
         boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));  // example from http://aprs.gids.nl/nmea/#rmc