From 632bceb653fa1988deddcd57a976ab0227aceb4c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 7 May 2018 09:13:45 +0200 Subject: [PATCH] Fix DOP computation, print it in GPX and NMEA --- src/algorithms/PVT/libs/gpx_printer.cc | 21 +++-- src/algorithms/PVT/libs/gpx_printer.h | 4 +- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 3 - src/algorithms/PVT/libs/ls_pvt.cc | 2 +- src/algorithms/PVT/libs/nmea_printer.cc | 10 +-- src/algorithms/PVT/libs/nmea_printer.h | 6 +- src/algorithms/PVT/libs/pvt_solution.cc | 85 ------------------- src/algorithms/PVT/libs/pvt_solution.h | 17 ---- src/algorithms/PVT/libs/rtklib_solver.cc | 34 +++++++- src/algorithms/PVT/libs/rtklib_solver.h | 8 +- src/tests/CMakeLists.txt | 1 + .../pvt/nmea_printer_test.cc | 12 ++- 12 files changed, 77 insertions(+), 126 deletions(-) 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