diff --git a/src/algorithms/PVT/libs/gpx_printer.cc b/src/algorithms/PVT/libs/gpx_printer.cc
index 2960e6e7c..65dda1636 100644
--- a/src/algorithms/PVT/libs/gpx_printer.cc
+++ b/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
<< "" << std::endl
- << "" << std::endl;
+ << indent << "Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "" << std::endl
+ << indent << "GNSS-SDR position log generated at " << pt << " (local time)" << std::endl
+ << indent << "" << 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& position, bool print_average_values)
+bool Gpx_Printer::print_position(const std::shared_ptr& position, bool print_average_values)
{
double latitude;
double longitude;
double height;
positions_printed = true;
+ std::shared_ptr position_ = position;
- std::shared_ptr 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& position,
if (gpx_file.is_open())
{
- gpx_file << "" << height << "" << std::endl;
+ gpx_file << indent << indent << "" << height << ""
+ << ""
+ << "" << hdop << "" << vdop << "" << pdop << "" << std::endl;
return true;
}
else
@@ -147,7 +157,7 @@ bool Gpx_Printer::close_file()
{
if (gpx_file.is_open())
{
- gpx_file << "" << std::endl
+ gpx_file << indent << "" << std::endl
<< "" << std::endl
<< "";
gpx_file.close();
@@ -163,6 +173,7 @@ bool Gpx_Printer::close_file()
Gpx_Printer::Gpx_Printer()
{
positions_printed = false;
+ indent = " ";
}
diff --git a/src/algorithms/PVT/libs/gpx_printer.h b/src/algorithms/PVT/libs/gpx_printer.h
index f7415cf71..347d95599 100644
--- a/src/algorithms/PVT/libs/gpx_printer.h
+++ b/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
#include
#include
@@ -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& position, bool print_average_values);
+ bool print_position(const std::shared_ptr& position, bool print_average_values);
bool close_file();
};
diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
index 3197c1882..4644352e5 100644
--- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
+++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc
@@ -350,9 +350,6 @@ bool hybrid_ls_pvt::get_PVT(std::map 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)
{
diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc
index 27be29287..af4f1e812 100644
--- a/src/algorithms/PVT/libs/ls_pvt.cc
+++ b/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)
diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc
index 305a89a07..ebff7b4ac 100644
--- a/src/algorithms/PVT/libs/nmea_printer.cc
+++ b/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_data, bool print_average_values)
+bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr& 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 hdop = d_PVT_data->get_HDOP();
- double vdop = d_PVT_data->get_VDOP();
+ double pdop = d_PVT_data->get_pdop();
+ double hdop = d_PVT_data->get_hdop();
+ 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)
diff --git a/src/algorithms/PVT/libs/nmea_printer.h b/src/algorithms/PVT/libs/nmea_printer.h
index c1b671d1a..857301d1c 100644
--- a/src/algorithms/PVT/libs/nmea_printer.h
+++ b/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
#include
@@ -58,7 +58,7 @@ public:
/*!
* \brief Print NMEA PVT and satellite info to the initialized device
*/
- bool Print_Nmea_Line(const std::shared_ptr& position, bool print_average_values);
+ bool Print_Nmea_Line(const std::shared_ptr& 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 d_PVT_data;
+ std::shared_ptr d_PVT_data;
int init_serial(std::string serial_device); //serial port control
void close_serial();
std::string get_GPGGA(); // fix data
diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc
index e38da70f7..dbfd76f1c 100644
--- a/src/algorithms/PVT/libs/pvt_solution.cc
+++ b/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;
-}
diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h
index e036b1547..c01e494a9 100644
--- a/src/algorithms/PVT/libs/pvt_solution.h
+++ b/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);
/*!
diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc
index 4dcee81a0..8dc81a90a 100644
--- a/src/algorithms/PVT/libs/rtklib_solver.cc
+++ b/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& gnss_observables_map, double Rx_time, bool flag_averaging)
{
std::map::const_iterator gnss_observables_iter;
@@ -435,6 +459,14 @@ bool rtklib_solver::get_PVT(const std::map& 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;
+ // TODO: Reduce the number of satellites in DOP computation
+ double azel[MAXSAT * 2] = {0.0};
+ for (unsigned int i = 0; i < MAXSAT; i++)
+ {
+ azel[2 * i] = rtk_.ssat[i].azel[0];
+ azel[2 * i + 1] = rtk_.ssat[i].azel[1];
+ }
+ dops(MAXSAT, 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];
diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h
index 3af8d2a76..ac180f617 100644
--- a/src/algorithms/PVT/libs/rtklib_solver.h
+++ b/src/algorithms/PVT/libs/rtklib_solver.h
@@ -79,12 +79,18 @@ 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& 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 galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
std::map gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris
diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt
index c43b47a07..ff2d91ee8 100644
--- a/src/tests/CMakeLists.txt
+++ b/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
diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc
index 0e260dd75..f259947e5 100644
--- a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc
+++ b/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");
-
- std::shared_ptr pvt_solution = std::make_shared();
+ rtk_t rtk;
+ prcopt_t rtklib_configuration_options;
+ rtkinit(&rtk, &rtklib_configuration_options);
+ std::shared_ptr pvt_solution = std::make_shared(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");
-
- std::shared_ptr pvt_solution = std::make_shared();
+ rtk_t rtk;
+ prcopt_t rtklib_configuration_options;
+ rtkinit(&rtk, &rtklib_configuration_options);
+ std::shared_ptr pvt_solution = std::make_shared(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