mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-26 04:57:40 +00:00
Code Issues Releases Wiki Activity

Fix DOP computation, print it in GPX and NMEA

Browse Source
This commit is contained in:
Carles Fernandez
2018-05-07 09:13:45 +02:00
parent 3bc6ce4f75
commit 632bceb653
12 changed files with 77 additions and 126 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/algorithms/PVT/libs/gpx_printer.cc
View File

@@ -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 = " ";
}

4
src/algorithms/PVT/libs/gpx_printer.h
View File

@@ -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();
};

3
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@@ -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)
{

2
src/algorithms/PVT/libs/ls_pvt.cc
View File

@@ -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)

10
src/algorithms/PVT/libs/nmea_printer.cc
View File

@@ -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 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)

6
src/algorithms/PVT/libs/nmea_printer.h
View File

@@ -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

85
src/algorithms/PVT/libs/pvt_solution.cc
View File

@@ -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;
}

17
src/algorithms/PVT/libs/pvt_solution.h
View File

@@ -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);
/*!

34
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@@ -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,14 @@ 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;
// 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];

8
src/algorithms/PVT/libs/rtklib_solver.h
View File

@@ -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<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
std::map<int, Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris

1
src/tests/CMakeLists.txt
View File

@@ -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

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

@@ -38,8 +38,10 @@
TEST(NmeaPrinterTest, PrintLine)
{
std::string filename("nmea_test.nmea");
std::shared_ptr<Pvt_Solution> pvt_solution = std::make_shared<Pvt_Solution>();
rtk_t rtk;
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");
std::shared_ptr<Pvt_Solution> pvt_solution = std::make_shared<Pvt_Solution>();
rtk_t rtk;
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