mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-13 13:47:15 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Marc Majoral
2018-11-16 10:34:19 +01:00
parent 10676fd3cf 049b2d777d
commit 3e46f658f6
63 changed files with 2229 additions and 2038 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@@ -48,6 +48,27 @@ namespace bc = boost::integer;
using google::LogMessage;
bool RtklibPvt::get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time)
{
return pvt_->get_latest_PVT(longitude_deg,
latitude_deg,
height_m,
ground_speed_kmh,
course_over_ground_deg,
UTC_time);
}
void RtklibPvt::clear_ephemeris()
{
pvt_->clear_ephemeris();
}
RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
@@ -516,6 +537,30 @@ RtklibPvt::~RtklibPvt()
}
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() const
{
return pvt_->get_galileo_ephemeris_map();
}
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() const
{
return pvt_->get_galileo_almanac_map();
}
void RtklibPvt::connect(gr::top_block_sptr top_block)
{
if (top_block)

15
src/algorithms/PVT/adapters/rtklib_pvt.h
View File

@@ -57,12 +57,18 @@ public:
return role_;
}
//! Returns "RTKLIB_Pvt"
//! Returns "RTKLIB_PVT"
inline std::string implementation() override
{
return "RTKLIB_PVT";
}
void clear_ephemeris() override;
std::map<int, Gps_Ephemeris> get_gps_ephemeris() const override;
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() const override;
std::map<int, Gps_Almanac> get_gps_almanac() const 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;
gr::basic_block_sptr get_left_block() override;
@@ -79,6 +85,13 @@ public:
return sizeof(gr_complex);
}
bool get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time) override;
private:
rtklib_pvt_cc_sptr pvt_;
rtk_t rtk;

1490
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

File diff suppressed because it is too large Load Diff

41
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@@ -31,7 +31,7 @@
#ifndef GNSS_SDR_RTKLIB_PVT_CC_H
#define GNSS_SDR_RTKLIB_PVT_CC_H
#include "gps_ephemeris.h"
#include "nmea_printer.h"
#include "kml_printer.h"
#include "gpx_printer.h"
@@ -40,6 +40,8 @@
#include "rtcm_printer.h"
#include "pvt_conf.h"
#include "rtklib_solver.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <gnuradio/sync_block.h>
#include <sys/types.h>
#include <sys/ipc.h>
@@ -60,7 +62,7 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t n_channels,
rtk_t& rtk);
/*!
* \brief This class implements a block that computes the PVT solution with Galileo E1 signals
* \brief This class implements a block that computes the PVT solution using the RTKLIB integrated library
*/
class rtklib_pvt_cc : public gr::sync_block
{
@@ -109,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);
@@ -135,6 +138,11 @@ private:
bool d_xml_storage;
std::string xml_base_path;
inline std::time_t to_time_t(boost::posix_time::ptime pt)
{
return (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds();
}
public:
rtklib_pvt_cc(uint32_t nchannels,
@@ -142,11 +150,32 @@ public:
rtk_t& rtk);
/*!
* \brief Get latest set of GPS L1 ephemeris from PVT block
* \brief Get latest set of ephemeris from PVT block
*
* It is used to save the assistance data at the receiver shutdown
*/
std::map<int, Gps_Ephemeris> get_GPS_L1_ephemeris_map();
std::map<int, Gps_Ephemeris> get_gps_ephemeris_map() const;
std::map<int, Gps_Almanac> get_gps_almanac_map() const;
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map() const;
std::map<int, Galileo_Almanac> get_galileo_almanac_map() const;
/*!
* \brief Clear all ephemeris information and the almanacs for GPS and Galileo
*
*/
void clear_ephemeris();
/*!
* \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available
*/
bool get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time);
~rtklib_pvt_cc(); //!< Default destructor

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

@@ -138,13 +138,15 @@ bool Gpx_Printer::set_headers(std::string filename, bool time_tag_name)
gpx_file << std::setprecision(14);
gpx_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<gpx version=\"1.1\" creator=\"GNSS-SDR\"" << std::endl
<< "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\"" << std::endl
<< "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
<< "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
<< "<trk>" << 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;
<< indent << "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd http://www.garmin.com/xmlschemas/GpxExtensions/v3 http://www.garmin.com/xmlschemas/GpxExtensionsv3.xsd http://www.garmin.com/xmlschemas/TrackPointExtension/v2 http://www.garmin.com/xmlschemas/TrackPointExtensionv2.xsd\"" << std::endl
<< indent << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
<< indent << "xmlns:gpxx=\"http://www.garmin.com/xmlschemas/GpxExtensions/v3\"" << std::endl
<< indent << "xmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"" << std::endl
<< indent << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
<< indent << "<trk>" << std::endl
<< indent << indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << std::endl
<< indent << indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << std::endl
<< indent << indent << "<trkseg>" << std::endl;
return true;
}
else
@@ -164,6 +166,9 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
positions_printed = true;
std::shared_ptr<rtklib_solver> position_ = position;
double speed_over_ground = position_->get_speed_over_ground(); // expressed in m/s
double course_over_ground = position_->get_course_over_ground(); // expressed in deg
double hdop = position_->get_hdop();
double vdop = position_->get_vdop();
double pdop = position_->get_pdop();
@@ -187,9 +192,13 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
if (gpx_file.is_open())
{
gpx_file << indent << indent << "<trkpt lon=\"" << longitude << "\" lat=\"" << latitude << "\"><ele>" << height << "</ele>"
gpx_file << indent << 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;
<< "<hdop>" << hdop << "</hdop><vdop>" << vdop << "</vdop><pdop>" << pdop << "</pdop>"
<< "<extensions><gpxtpx:TrackPointExtension>"
<< "<gpxtpx:speed>" << speed_over_ground << "</gpxtpx:speed>"
<< "<gpxtpx:course>" << course_over_ground << "</gpxtpx:course>"
<< "</gpxtpx:TrackPointExtension></extensions></trkpt>" << std::endl;
return true;
}
else
@@ -203,8 +212,8 @@ bool Gpx_Printer::close_file()
{
if (gpx_file.is_open())
{
gpx_file << indent << "</trkseg>" << std::endl
<< "</trk>" << std::endl
gpx_file << indent << indent << "</trkseg>" << std::endl
<< indent << "</trk>" << std::endl
<< "</gpx>";
gpx_file.close();
return true;

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

@@ -150,8 +150,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - this->get_time_offset_s() * GALILEO_C_m_s;
this->set_visible_satellites_ID(valid_obs, galileo_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
@@ -213,8 +211,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
double Code_bias_m = P1_P2 / (1.0 - Gamma);
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s - Code_bias_m - this->get_time_offset_s() * GPS_C_m_s;
this->set_visible_satellites_ID(valid_obs, gps_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
// SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
@@ -265,8 +261,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s;
this->set_visible_satellites_ID(valid_obs, gps_cnav_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
GPS_week = GPS_week % 1024; //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV)

171
src/algorithms/PVT/libs/kml_printer.cc
View File

@@ -2,6 +2,7 @@
* \file kml_printer.cc
* \brief Implementation of a class that prints PVT information to a kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
*
*
* -------------------------------------------------------------------------
@@ -43,6 +44,7 @@ using google::LogMessage;
Kml_Printer::Kml_Printer(const std::string& base_path)
{
positions_printed = false;
indent = " ";
kml_base_path = base_path;
boost::filesystem::path full_path(boost::filesystem::current_path());
const boost::filesystem::path p(kml_base_path);
@@ -74,6 +76,14 @@ Kml_Printer::Kml_Printer(const std::string& base_path)
}
kml_base_path = kml_base_path + boost::filesystem::path::preferred_separator;
boost::filesystem::path tmp_base_path = boost::filesystem::temp_directory_path();
boost::filesystem::path tmp_filename = boost::filesystem::unique_path();
boost::filesystem::path tmp_file = tmp_base_path / tmp_filename;
tmp_file_str = tmp_file.string();
point_id = 0;
}
@@ -127,36 +137,73 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
kml_filename = kml_base_path + kml_filename;
kml_file.open(kml_filename.c_str());
if (kml_file.is_open())
tmp_file.open(tmp_file_str.c_str());
if (kml_file.is_open() && tmp_file.is_open())
{
DLOG(INFO) << "KML printer writing on " << filename.c_str();
// Set iostream numeric format and precision
kml_file.setf(kml_file.fixed, kml_file.floatfield);
kml_file << std::setprecision(14);
tmp_file.setf(tmp_file.fixed, tmp_file.floatfield);
tmp_file << std::setprecision(14);
kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
<< " <Document>" << std::endl
<< " <name>GNSS Track</name>" << std::endl
<< " <description>GNSS-SDR Receiver position log file created at " << pt
<< " </description>" << std::endl
<< "<Style id=\"yellowLineGreenPoly\">" << std::endl
<< " <LineStyle>" << std::endl
<< " <color>7f00ffff</color>" << std::endl
<< " <width>1</width>" << std::endl
<< " </LineStyle>" << std::endl
<< "<PolyStyle>" << std::endl
<< " <color>7f00ff00</color>" << std::endl
<< "</PolyStyle>" << std::endl
<< "</Style>" << std::endl
<< "<Placemark>" << std::endl
<< "<name>GNSS-SDR PVT</name>" << std::endl
<< "<description>GNSS-SDR position log</description>" << std::endl
<< "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
<< "<LineString>" << std::endl
<< "<extrude>0</extrude>" << std::endl
<< "<tessellate>1</tessellate>" << std::endl
<< "<altitudeMode>absolute</altitudeMode>" << std::endl
<< "<coordinates>" << std::endl;
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:gx=\"http://www.google.com/kml/ext/2.2\">" << std::endl
<< indent << "<Document>" << std::endl
<< indent << indent << "<name>GNSS Track</name>" << std::endl
<< indent << indent << "<description><![CDATA[" << std::endl
<< indent << indent << indent << "<table>" << std::endl
<< indent << indent << indent << indent << "<tr><td>GNSS-SDR Receiver position log file created at " << pt << "</td></tr>" << std::endl
<< indent << indent << indent << indent << "<tr><td>https://gnss-sdr.org/</td></tr>" << std::endl
<< indent << indent << indent << "</table>" << std::endl
<< indent << indent << "]]></description>" << std::endl
<< indent << indent << "<!-- Normal track style -->" << std::endl
<< indent << indent << "<Style id=\"track_n\">" << std::endl
<< indent << indent << indent << "<IconStyle>" << std::endl
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << std::endl
<< indent << indent << indent << indent << "<scale>0.3</scale>" << std::endl
<< indent << indent << indent << indent << "<Icon>" << std::endl
<< indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << std::endl
<< indent << indent << indent << indent << "</Icon>" << std::endl
<< indent << indent << indent << "</IconStyle>" << std::endl
<< indent << indent << indent << "<LabelStyle>" << std::endl
<< indent << indent << indent << indent << "<scale>0</scale>" << std::endl
<< indent << indent << indent << "</LabelStyle>" << std::endl
<< indent << indent << "</Style>" << std::endl
<< indent << indent << "<!-- Highlighted track style -->" << std::endl
<< indent << indent << "<Style id=\"track_h\">" << std::endl
<< indent << indent << indent << "<IconStyle>" << std::endl
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << std::endl
<< indent << indent << indent << indent << "<scale>1</scale>" << std::endl
<< indent << indent << indent << indent << "<Icon>" << std::endl
<< indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << std::endl
<< indent << indent << indent << indent << "</Icon>" << std::endl
<< indent << indent << indent << "</IconStyle>" << std::endl
<< indent << indent << "</Style>" << std::endl
<< indent << indent << "<StyleMap id=\"track\">" << std::endl
<< indent << indent << indent << "<Pair>" << std::endl
<< indent << indent << indent << indent << "<key>normal</key>" << std::endl
<< indent << indent << indent << indent << "<styleUrl>#track_n</styleUrl>" << std::endl
<< indent << indent << indent << "</Pair>" << std::endl
<< indent << indent << indent << "<Pair>" << std::endl
<< indent << indent << indent << indent << "<key>highlight</key>" << std::endl
<< indent << indent << indent << indent << "<styleUrl>#track_h</styleUrl>" << std::endl
<< indent << indent << indent << "</Pair>" << std::endl
<< indent << indent << "</StyleMap>" << std::endl
<< indent << indent << "<Style id=\"yellowLineGreenPoly\">" << std::endl
<< indent << indent << indent << "<LineStyle>" << std::endl
<< indent << indent << indent << indent << "<color>7f00ffff</color>" << std::endl
<< indent << indent << indent << indent << "<width>1</width>" << std::endl
<< indent << indent << indent << "</LineStyle>" << std::endl
<< indent << indent << indent << "<PolyStyle>" << std::endl
<< indent << indent << indent << indent << "<color>7f00ff00</color>" << std::endl
<< indent << indent << indent << "</PolyStyle>" << std::endl
<< indent << indent << "</Style>" << std::endl
<< indent << indent << "<Folder>" << std::endl
<< indent << indent << indent << "<name>Points</name>" << std::endl;
return true;
}
else
@@ -167,7 +214,7 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
}
bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values)
bool Kml_Printer::print_position(const std::shared_ptr<rtklib_solver>& position, bool print_average_values)
{
double latitude;
double longitude;
@@ -175,7 +222,18 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
positions_printed = true;
std::shared_ptr<Pvt_Solution> position_ = position;
std::shared_ptr<rtklib_solver> position_ = position;
double speed_over_ground = position_->get_speed_over_ground(); // expressed in m/s
double course_over_ground = position_->get_course_over_ground(); // expressed in deg
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());
if (utc_time.length() < 23) utc_time += ".";
utc_time.resize(23, '0'); // time up to ms
utc_time.append("Z"); // UTC time zone
if (print_average_values == false)
{
@@ -190,9 +248,38 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
height = position_->get_avg_height();
}
if (kml_file.is_open())
if (kml_file.is_open() && tmp_file.is_open())
{
kml_file << longitude << "," << latitude << "," << height << std::endl;
point_id++;
kml_file << indent << indent << indent << "<Placemark>" << std::endl
<< indent << indent << indent << indent << "<name>" << point_id << "</name>" << std::endl
<< indent << indent << indent << indent << "<snippet/>" << std::endl
<< indent << indent << indent << indent << "<description><![CDATA[" << std::endl
<< indent << indent << indent << indent << indent << "<table>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Time:</td><td>" << utc_time << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Longitude:</td><td>" << longitude << "</td><td>deg</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Latitude:</td><td>" << latitude << "</td><td>deg</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Altitude:</td><td>" << height << "</td><td>m</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Speed:</td><td>" << speed_over_ground << "</td><td>m/s</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>Course:</td><td>" << course_over_ground << "</td><td>deg</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>HDOP:</td><td>" << hdop << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>VDOP:</td><td>" << vdop << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << indent << "<tr><td>PDOP:</td><td>" << pdop << "</td></tr>" << std::endl
<< indent << indent << indent << indent << indent << "</table>" << std::endl
<< indent << indent << indent << indent << "]]></description>" << std::endl
<< indent << indent << indent << indent << "<TimeStamp>" << std::endl
<< indent << indent << indent << indent << indent << "<when>" << utc_time << "</when>" << std::endl
<< indent << indent << indent << indent << "</TimeStamp>" << std::endl
<< indent << indent << indent << indent << "<styleUrl>#track</styleUrl>" << std::endl
<< indent << indent << indent << indent << "<Point>" << std::endl
<< indent << indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << std::endl
<< indent << indent << indent << indent << indent << "<coordinates>" << longitude << "," << latitude << "," << height << "</coordinates>" << std::endl
<< indent << indent << indent << indent << "</Point>" << std::endl
<< indent << indent << indent << "</Placemark>" << std::endl;
tmp_file << indent << indent << indent << indent << indent
<< longitude << "," << latitude << "," << height << std::endl;
return true;
}
else
@@ -204,14 +291,32 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
bool Kml_Printer::close_file()
{
if (kml_file.is_open())
if (kml_file.is_open() && tmp_file.is_open())
{
kml_file << "</coordinates>" << std::endl
<< "</LineString>" << std::endl
<< "</Placemark>" << std::endl
<< "</Document>" << std::endl
tmp_file.close();
kml_file << indent << indent << "</Folder>"
<< indent << indent << "<Placemark>" << std::endl
<< indent << indent << indent << "<name>Path</name>" << std::endl
<< indent << indent << indent << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
<< indent << indent << indent << "<LineString>" << std::endl
<< indent << indent << indent << indent << "<extrude>0</extrude>" << std::endl
<< indent << indent << indent << indent << "<tessellate>1</tessellate>" << std::endl
<< indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << std::endl
<< indent << indent << indent << indent << "<coordinates>" << std::endl;
// Copy the contents of tmp_file into kml_file
std::ifstream src(tmp_file_str, std::ios::binary);
kml_file << src.rdbuf();
kml_file << indent << indent << indent << indent << "</coordinates>" << std::endl
<< indent << indent << indent << "</LineString>" << std::endl
<< indent << indent << "</Placemark>" << std::endl
<< indent << "</Document>" << std::endl
<< "</kml>";
kml_file.close();
return true;
}
else

9
src/algorithms/PVT/libs/kml_printer.h
View File

@@ -2,7 +2,7 @@
* \file kml_printer.h
* \brief Interface of a class that prints PVT information to a kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
*
* -------------------------------------------------------------------------
*
@@ -34,6 +34,7 @@
#define GNSS_SDR_KML_PRINTER_H_
#include "pvt_solution.h"
#include "rtklib_solver.h"
#include <fstream>
#include <memory>
#include <string>
@@ -48,15 +49,19 @@ class Kml_Printer
{
private:
std::ofstream kml_file;
std::ofstream tmp_file;
bool positions_printed;
std::string kml_filename;
std::string kml_base_path;
std::string tmp_file_str;
unsigned int point_id;
std::string indent;
public:
Kml_Printer(const std::string& base_path = std::string("."));
~Kml_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();
};

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

@@ -31,6 +31,7 @@
#include "ls_pvt.h"
#include "GPS_L1_CA.h"
#include "geofunctions.h"
#include <glog/logging.h>
#include <exception>
#include <stdexcept>
@@ -235,15 +236,12 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
double* azim = 0;
double* elev = 0;
double* dist = 0;
Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
this->set_visible_satellites_Az(i, *azim);
this->set_visible_satellites_El(i, *elev);
this->set_visible_satellites_Distance(i, *dist);
topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
if (traveltime < 0.1 && nmbOfSatellites > 3)
{
//--- Find receiver's height
Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
// Add troposphere correction if the receiver is below the troposphere
if (h > 15000)
{
@@ -253,7 +251,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
else
{
//--- Find delay due to troposphere (in meters)
Ls_Pvt::tropo(&trop, sin(this->get_visible_satellites_El(i) * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
Ls_Pvt::tropo(&trop, sin(*elev * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if (trop > 5.0) trop = 0.0; //check for erratic values
}
}
@@ -280,9 +278,6 @@ 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));
// check the consistency of the PVT solution
if (((fabs(pos(3)) * 1000.0) / GPS_C_m_s) > GPS_STARTOFFSET_ms * 2)
{

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

@@ -34,6 +34,7 @@
*/
#include "nmea_printer.h"
#include "rtklib_solution.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem/operations.hpp> // for create_directories, exists
#include <boost/filesystem/path.hpp> // for path, operator<<
@@ -376,99 +377,10 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
std::string Nmea_Printer::get_GPRMC()
{
// Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10
bool valid_fix = d_PVT_data->is_valid_position();
// ToDo: Compute speed and course over ground
double speed_over_ground_knots = 0;
double course_over_ground_deg = 0;
// boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
std::stringstream sentence_str;
// GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
std::string sentence_header;
sentence_header = "$GPRMC,";
sentence_str << sentence_header;
// UTC Time: hhmmss.sss
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
// Status: A: data valid, V: data NOT valid
if (valid_fix == true)
{
sentence_str << ",A";
}
else
{
sentence_str << ",V";
};
if (print_avg_pos == true)
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
}
else
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
}
// Speed over ground (knots)
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.precision(2);
sentence_str << speed_over_ground_knots;
// course over ground (degrees)
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.precision(2);
sentence_str << course_over_ground_deg;
// Date ddmmyy
boost::gregorian::date sentence_date = d_PVT_data->get_position_UTC_time().date();
unsigned int year = sentence_date.year();
unsigned int day = sentence_date.day();
unsigned int month = sentence_date.month();
sentence_str << ",";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << day;
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << month;
std::stringstream year_strs;
year_strs << std::dec << year;
sentence_str << std::dec << year_strs.str().substr(2);
// Magnetic Variation (degrees)
// ToDo: Implement magnetic compass
sentence_str << ",";
// Magnetic Variation (E or W)
// ToDo: Implement magnetic compass
sentence_str << ",";
// Checksum
char checksum;
std::string tmpstr;
tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1));
sentence_str << "*";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
sentence_str << "\r\n";
unsigned char buff[1024] = {0};
outnmea_rmc(buff, &d_PVT_data->pvt_sol);
sentence_str << buff;
return sentence_str.str();
}
@@ -477,82 +389,10 @@ std::string Nmea_Printer::get_GPGSA()
{
// $GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
// 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();
std::stringstream sentence_str;
std::string sentence_header;
sentence_header = "$GPGSA,";
sentence_str << sentence_header;
// mode1:
// (M) Manual-forced to operate in 2D or 3D mode
// (A) Automatic-allowed to automatically switch 2D/3D
std::string mode1 = "M";
sentence_str << mode1;
// mode2:
// 1 fix not available
// 2 fix 2D
// 3 fix 3D
if (valid_fix == true)
{
sentence_str << ",3";
}
else
{
sentence_str << ",1";
};
// Used satellites
for (int i = 0; i < 12; i++)
{
sentence_str << ",";
if (i < n_sats_used)
{
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << d_PVT_data->get_visible_satellites_ID(i);
}
}
// PDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << pdop;
// HDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << hdop;
// VDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << vdop;
// Checksum
char checksum;
std::string tmpstr;
tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1));
sentence_str << "*";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
sentence_str << "\r\n";
unsigned char buff[1024] = {0};
outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
sentence_str << buff;
return sentence_str.str();
}
@@ -560,199 +400,22 @@ std::string Nmea_Printer::get_GPGSA()
std::string Nmea_Printer::get_GPGSV()
{
// GSV-GNSS Satellites in View
// Notice that NMEA 2.1 only supports 12 channels
int n_sats_used = d_PVT_data->get_num_valid_observations();
std::stringstream sentence_str;
std::stringstream frame_str;
std::string sentence_header;
sentence_header = "$GPGSV,";
char checksum;
std::string tmpstr;
// 1st step: How many GPGSV frames we need? (up to 3)
// Each frame contains up to 4 satellites
int n_frames;
n_frames = std::ceil((static_cast<double>(n_sats_used)) / 4.0);
// generate the frames
int current_satellite = 0;
for (int i = 1; i < (n_frames + 1); i++)
{
frame_str.str("");
frame_str << sentence_header;
// number of messages
frame_str << n_frames;
// message number
frame_str << ",";
frame_str << i;
// total number of satellites in view
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << n_sats_used;
// satellites info
for (int j = 0; j < 4; j++)
{
// write satellite info
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << d_PVT_data->get_visible_satellites_ID(current_satellite);
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_El(current_satellite));
frame_str << ",";
frame_str.width(3);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_Az(current_satellite));
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_CN0_dB(current_satellite));
current_satellite++;
if (current_satellite == n_sats_used)
{
break;
}
}
// frame checksum
tmpstr = frame_str.str();
checksum = checkSum(tmpstr.substr(1));
frame_str << "*";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
frame_str << "\r\n";
//add frame to sentence
sentence_str << frame_str.str();
}
return sentence_str.str();
// $GPGSV,2,1,07,07,79,048,42,02,51,062,43,26,36,256,42,27,27,138,42*71
// Notice that NMEA 2.1 only supports 12 channels
std::stringstream sentence_str;
unsigned char buff[1024] = {0};
outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
sentence_str << buff;
return sentence_str.str();
}
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 MSL_altitude;
if (d_PVT_data->is_averaging() == true)
{
MSL_altitude = d_PVT_data->get_avg_height();
}
else
{
MSL_altitude = d_PVT_data->get_height();
}
std::stringstream sentence_str;
// GPGGA (Global Positioning System Fixed Data)
std::string sentence_header;
sentence_header = "$GPGGA,";
sentence_str << sentence_header;
// UTC Time: hhmmss.sss
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
if (d_PVT_data->is_averaging() == true)
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
}
else
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
}
// Position fix indicator
// 0 - Fix not available or invalid
// 1 - GPS SPS Mode, fix valid
// 2 - Differential GPS, SPS Mode, fix valid
// 3-5 - Not supported
// 6 - Dead Reckoning Mode, fix valid
// ToDo: Update PVT module to identify the fix mode
if (valid_fix == true)
{
sentence_str << ",1";
}
else
{
sentence_str << ",0";
}
// Number of satellites used in PVT
sentence_str << ",";
if (n_channels < 10)
{
sentence_str << '0' << n_channels;
}
else
{
sentence_str << n_channels;
}
// HDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << hdop;
// MSL Altitude
sentence_str << ",";
sentence_str.precision(1);
sentence_str << MSL_altitude;
sentence_str << ",M";
// Geoid-to-ellipsoid separation. Ellipsoid altitude = MSL Altitude + Geoid Separation.
// ToDo: Compute this value
sentence_str << ",";
sentence_str << "0.0";
sentence_str << ",M";
// Age of Diff. Corr. (Seconds) Null fields when DGPS is not used
// Diff. Ref. Station ID (0000)
// ToDo: Implement this fields for Differential GPS
sentence_str << ",";
sentence_str << "0.0,0000";
// Checksum
char checksum;
std::string tmpstr;
tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1));
sentence_str << "*";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
sentence_str << "\r\n";
unsigned char buff[1024] = {0};
outnmea_gga(buff, &d_PVT_data->pvt_sol);
sentence_str << buff;
return sentence_str.str();
// $GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
}

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

@@ -31,6 +31,7 @@
#include "pvt_solution.h"
#include "GPS_L1_CA.h"
#include "geofunctions.h"
#include <glog/logging.h>
#include <exception>
@@ -43,6 +44,8 @@ Pvt_Solution::Pvt_Solution()
d_latitude_d = 0.0;
d_longitude_d = 0.0;
d_height_m = 0.0;
d_speed_over_ground_m_s = 0.0;
d_course_over_ground_d = 0.0;
d_avg_latitude_d = 0.0;
d_avg_longitude_d = 0.0;
d_avg_height_m = 0.0;
@@ -126,125 +129,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
d_latitude_d = phi * 180.0 / GPS_PI;
d_longitude_d = lambda * 180.0 / GPS_PI;
d_height_m = h;
return 0;
}
int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
{
/* Subroutine to calculate geodetic coordinates latitude, longitude,
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
values semi-major axis (a) and the inverse of flattening (finv).
The output units of angular quantities will be in decimal degrees
(15.5 degrees not 15 deg 30 min). The output units of h will be the
same as the units of X,Y,Z,a.
Inputs:
a - semi-major axis of the reference ellipsoid
finv - inverse of flattening of the reference ellipsoid
X,Y,Z - Cartesian coordinates
Outputs:
dphi - latitude
dlambda - longitude
h - height above reference ellipsoid
Based in a Matlab function by Kai Borre
*/
*h = 0;
double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations
double rtd = 180.0 / GPS_PI;
// compute square of eccentricity
double esq;
if (finv < 1.0E-20)
{
esq = 0.0;
}
else
{
esq = (2.0 - 1.0 / finv) / finv;
}
// first guess
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
//direct calculation of longitude
if (P > 1.0E-20)
{
*dlambda = atan2(Y, X) * rtd;
}
else
{
*dlambda = 0.0;
}
// correct longitude bound
if (*dlambda < 0)
{
*dlambda = *dlambda + 360.0;
}
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
double sinphi;
if (r > 1.0E-20)
{
sinphi = Z / r;
}
else
{
sinphi = 0.0;
}
*dphi = asin(sinphi);
// initial value of height = distance from origin minus
// approximate distance from origin to surface of ellipsoid
if (r < 1.0E-20)
{
*h = 0;
return 1;
}
*h = r - a * (1 - sinphi * sinphi / finv);
// iterate
double cosphi;
double N_phi;
double dP;
double dZ;
double oneesq = 1.0 - esq;
for (int i = 0; i < maxit; i++)
{
sinphi = sin(*dphi);
cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq * sinphi * sinphi);
// compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
// update height and latitude
*h = *h + (sinphi * dZ + cosphi * dP);
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
// test for convergence
if ((dP * dP + dZ * dZ) < tolsq)
{
break;
}
if (i == (maxit - 1))
{
LOG(WARNING) << "The computation of geodetic coordinates did not converge";
}
}
*dphi = (*dphi) * rtd;
//todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
return 0;
}
@@ -356,73 +241,6 @@ int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb
}
int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
{
/* Transformation of vector dx into topocentric coordinate
system with origin at x
Inputs:
x - vector origin coordinates (in ECEF system [X; Y; Z;])
dx - vector ([dX; dY; dZ;]).
Outputs:
D - vector length. Units like the input
Az - azimuth from north positive clockwise, degrees
El - elevation angle, degrees
Based on a Matlab function by Kai Borre
*/
double lambda;
double phi;
double h;
double dtr = GPS_PI / 180.0;
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
// Transform x into geodetic coordinates
Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
arma::mat F = {{-sl, -sb * cl, cb * cl},
{cl, -sb * sl, cb * sl},
{0, 0, cb, sb}};
arma::vec local_vector;
local_vector = arma::htrans(F) * dx;
double E = local_vector(0);
double N = local_vector(1);
double U = local_vector(2);
double hor_dis;
hor_dis = sqrt(E * E + N * N);
if (hor_dis < 1.0E-20)
{
*Az = 0;
*El = 90;
}
else
{
*Az = atan2(E, N) / dtr;
*El = atan2(U, hor_dis) / dtr;
}
if (*Az < 0)
{
*Az = *Az + 360.0;
}
*D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
return 0;
}
void Pvt_Solution::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
@@ -517,6 +335,30 @@ double Pvt_Solution::get_height() const
}
double Pvt_Solution::get_speed_over_ground() const
{
return d_speed_over_ground_m_s;
}
void Pvt_Solution::set_speed_over_ground(double speed_m_s)
{
d_speed_over_ground_m_s = speed_m_s;
}
void Pvt_Solution::set_course_over_ground(double cog_deg)
{
d_course_over_ground_d = cog_deg;
}
double Pvt_Solution::get_course_over_ground() const
{
return d_course_over_ground_d;
}
double Pvt_Solution::get_avg_latitude() const
{
return d_avg_latitude_d;
@@ -540,6 +382,7 @@ bool Pvt_Solution::is_averaging() const
return d_flag_averaging;
}
bool Pvt_Solution::is_valid_position() const
{
return b_valid_position;
@@ -589,172 +432,3 @@ void Pvt_Solution::set_num_valid_observations(int num)
{
d_valid_observations = num;
}
bool Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat ID to channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
if (prn >= PVT_MAX_PRN)
{
LOG(WARNING) << "Setting to channel " << index << " a PRN of " << prn << " (the maximum is " << PVT_MAX_PRN << ")";
return false;
}
else
{
d_visible_satellites_IDs[index] = prn;
return true;
}
}
}
unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat ID for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0;
}
else
{
return d_visible_satellites_IDs[index];
}
}
bool Pvt_Solution::set_visible_satellites_El(size_t index, double el)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
if (el > 90.0)
{
LOG(WARNING) << "Setting a sat elevation > 90 [degrees]. Saturating to 90";
d_visible_satellites_El[index] = 90.0;
}
else
{
if (el < -90.0)
{
LOG(WARNING) << "Setting a sat elevation < -90 [degrees]. Saturating to -90";
d_visible_satellites_El[index] = -90.0;
}
else
{
d_visible_satellites_El[index] = el;
}
}
return true;
}
}
double Pvt_Solution::get_visible_satellites_El(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_El[index];
}
}
bool Pvt_Solution::set_visible_satellites_Az(size_t index, double az)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
d_visible_satellites_Az[index] = az;
return true;
}
}
double Pvt_Solution::get_visible_satellites_Az(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_Az[index];
}
}
bool Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
d_visible_satellites_Distance[index] = dist;
return true;
}
}
double Pvt_Solution::get_visible_satellites_Distance(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_Distance[index];
}
}
bool Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat Cn0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
d_visible_satellites_CN0_dB[index] = cn0;
return true;
}
}
double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting received CN0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_CN0_dB[index];
}
}

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

@@ -49,9 +49,11 @@ class Pvt_Solution
private:
double d_rx_dt_s; // RX time offset [s]
double d_latitude_d; // RX position Latitude WGS84 [deg]
double d_longitude_d; // RX position Longitude WGS84 [deg]
double d_height_m; // RX position height WGS84 [m]
double d_latitude_d; // RX position Latitude WGS84 [deg]
double d_longitude_d; // RX position Longitude WGS84 [deg]
double d_height_m; // RX position height WGS84 [m]
double d_speed_over_ground_m_s; // RX speed over ground [m/s]
double d_course_over_ground_d; // RX course over ground [deg]
double d_avg_latitude_d; // Averaged latitude in degrees
double d_avg_longitude_d; // Averaged longitude in degrees
@@ -70,12 +72,6 @@ private:
boost::posix_time::ptime d_position_UTC_time;
int d_valid_observations;
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
double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {}; // Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites
public:
Pvt_Solution();
@@ -86,6 +82,12 @@ public:
double get_longitude() const; //!< Get RX position Longitude WGS84 [deg]
double get_height() const; //!< Get RX position height WGS84 [m]
double get_speed_over_ground() const; //!< Get RX speed over ground [m/s]
void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s]
double get_course_over_ground() const; //!< Get RX course over ground [deg]
void set_course_over_ground(double cog_deg); //!< Set RX course over ground [deg]
double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg]
double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg]
double get_avg_height() const; //!< Get RX position averaged height WGS84 [m]
@@ -102,21 +104,6 @@ public:
int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites)
void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites)
bool set_visible_satellites_ID(size_t index, unsigned int prn); //!< Set the ID of the visible satellite index channel
unsigned int get_visible_satellites_ID(size_t index) const; //!< Get the ID of the visible satellite index channel
bool set_visible_satellites_El(size_t index, double el); //!< Set the LOS Elevation, in degrees, of the visible satellite index channel
double get_visible_satellites_El(size_t index) const; //!< Get the LOS Elevation, in degrees, of the visible satellite index channel
bool set_visible_satellites_Az(size_t index, double az); //!< Set the LOS Azimuth, in degrees, of the visible satellite index channel
double get_visible_satellites_Az(size_t index) const; //!< Get the LOS Azimuth, in degrees, of the visible satellite index channel
bool set_visible_satellites_Distance(size_t index, double dist); //!< Set the LOS Distance of the visible satellite index channel
double get_visible_satellites_Distance(size_t index) const; //!< Get the LOS Distance of the visible satellite index channel
bool set_visible_satellites_CN0_dB(size_t index, double cn0); //!< Set the CN0 in dB of the visible satellite index channel
double get_visible_satellites_CN0_dB(size_t index) const; //!< Get the CN0 in dB of the visible satellite index channel
//averaging
void perform_pos_averaging();
void set_averaging_depth(int depth); //!< Set length of averaging window
@@ -142,41 +129,6 @@ public:
*/
int cart2geo(double X, double Y, double Z, int elipsoid_selection);
/*!
* \brief Transformation of vector dx into topocentric coordinate system with origin at x
*
* \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;])
* \param[in] dx Vector ([dX; dY; dZ;]).
*
* \param[out] D Vector length. Units like the input
* \param[out] Az Azimuth from north positive clockwise, degrees
* \param[out] El Elevation angle, degrees
*
* Based on a Matlab function by Kai Borre
*/
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
/*!
* \brief Subroutine to calculate geodetic coordinates latitude, longitude,
* height given Cartesian coordinates X,Y,Z, and reference ellipsoid
* values semi-major axis (a) and the inverse of flattening (finv).
*
* The output units of angular quantities will be in decimal degrees
* (15.5 degrees not 15 deg 30 min). The output units of h will be the
* same as the units of X,Y,Z,a.
*
* \param[in] a - semi-major axis of the reference ellipsoid
* \param[in] finv - inverse of flattening of the reference ellipsoid
* \param[in] X,Y,Z - Cartesian coordinates
*
* \param[out] dphi - latitude
* \param[out] dlambda - longitude
* \param[out] h - height above reference ellipsoid
*
* Based in a Matlab function by Kai Borre
*/
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
/*!
* \brief Tropospheric correction
*

88
src/algorithms/PVT/libs/rinex_printer.cc
View File

@@ -3296,8 +3296,8 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Glo
std::string line;
std::map<int32_t, Glonass_Gnav_Ephemeris>::const_iterator glonass_gnav_ephemeris_iter;
for (glonass_gnav_ephemeris_iter = eph_map.begin();
glonass_gnav_ephemeris_iter != eph_map.end();
for (glonass_gnav_ephemeris_iter = eph_map.cbegin();
glonass_gnav_ephemeris_iter != eph_map.cend();
glonass_gnav_ephemeris_iter++)
{
// -------- SV / EPOCH / SV CLK
@@ -7114,15 +7114,15 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
//Number of satellites observed in current epoch
int32_t numSatellitesObserved = 0;
std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
numSatellitesObserved++;
}
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(numSatellitesObserved), 3);
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
line += satelliteSystem["GLONASS"];
@@ -7135,8 +7135,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
std::string lineObs;
@@ -7218,8 +7218,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
//Number of satellites observed in current epoch
int32_t numSatellitesObserved = 0;
std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
numSatellitesObserved++;
@@ -7233,8 +7233,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
std::string lineObs;
@@ -7390,8 +7390,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
std::map<int32_t, Gnss_Synchro> observablesR2C;
std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
std::string system_(&observables_iter->second.System, 1);
@@ -7413,8 +7413,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
std::multimap<uint32_t, Gnss_Synchro> total_glo_map;
std::set<uint32_t> available_glo_prns;
std::set<uint32_t>::iterator it;
for (observables_iter = observablesR1C.begin();
observables_iter != observablesR1C.end();
for (observables_iter = observablesR1C.cbegin();
observables_iter != observablesR1C.cend();
observables_iter++)
{
uint32_t prn_ = observables_iter->second.PRN;
@@ -7426,8 +7426,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
}
}
for (observables_iter = observablesR2C.begin();
observables_iter != observablesR2C.end();
for (observables_iter = observablesR2C.cbegin();
observables_iter != observablesR2C.cend();
observables_iter++)
{
uint32_t prn_ = observables_iter->second.PRN;
@@ -7446,8 +7446,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
if (version == 2)
{
// Add list of GPS satellites
for (observables_iter = observablesG1C.begin();
observables_iter != observablesG1C.end();
for (observables_iter = observablesG1C.cbegin();
observables_iter != observablesG1C.cend();
observables_iter++)
{
line += satelliteSystem["GPS"];
@@ -7455,8 +7455,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
line += boost::lexical_cast<std::string>(static_cast<int32_t>(observables_iter->second.PRN));
}
// Add list of GLONASS L1 satellites
for (observables_iter = observablesR1C.begin();
observables_iter != observablesR1C.end();
for (observables_iter = observablesR1C.cbegin();
observables_iter != observablesR1C.cend();
observables_iter++)
{
line += satelliteSystem["GLONASS"];
@@ -7464,8 +7464,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
line += boost::lexical_cast<std::string>(static_cast<int32_t>(observables_iter->second.PRN));
}
// Add list of GLONASS L2 satellites
for (observables_iter = observablesR2C.begin();
observables_iter != observablesR2C.end();
for (observables_iter = observablesR2C.cbegin();
observables_iter != observablesR2C.cend();
observables_iter++)
{
line += satelliteSystem["GLONASS"];
@@ -7480,8 +7480,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
// -------- OBSERVATION record
std::string s;
std::string lineObs;
for (observables_iter = observablesG1C.begin();
observables_iter != observablesG1C.end();
for (observables_iter = observablesG1C.cbegin();
observables_iter != observablesG1C.cend();
observables_iter++)
{
lineObs.clear();
@@ -7664,8 +7664,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
std::map<int32_t, Gnss_Synchro> observablesR2C;
std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
std::string system_(&observables_iter->second.System, 1);
@@ -7687,8 +7687,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
std::multimap<uint32_t, Gnss_Synchro> total_glo_map;
std::set<uint32_t> available_glo_prns;
std::set<uint32_t>::iterator it;
for (observables_iter = observablesR1C.begin();
observables_iter != observablesR1C.end();
for (observables_iter = observablesR1C.cbegin();
observables_iter != observablesR1C.cend();
observables_iter++)
{
uint32_t prn_ = observables_iter->second.PRN;
@@ -7700,8 +7700,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
}
}
for (observables_iter = observablesR2C.begin();
observables_iter != observablesR2C.end();
for (observables_iter = observablesR2C.cbegin();
observables_iter != observablesR2C.cend();
observables_iter++)
{
uint32_t prn_ = observables_iter->second.PRN;
@@ -7725,8 +7725,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
// -------- OBSERVATION record
std::string s;
std::string lineObs;
for (observables_iter = observablesG2S.begin();
observables_iter != observablesG2S.end();
for (observables_iter = observablesG2S.cbegin();
observables_iter != observablesG2S.cend();
observables_iter++)
{
lineObs.clear();
@@ -7904,8 +7904,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
std::map<int32_t, Gnss_Synchro> observablesR2C;
std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
for (observables_iter = observables.begin();
observables_iter != observables.end();
for (observables_iter = observables.cbegin();
observables_iter != observables.cend();
observables_iter++)
{
std::string system_(&observables_iter->second.System, 1);
@@ -7927,8 +7927,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
std::multimap<uint32_t, Gnss_Synchro> total_glo_map;
std::set<uint32_t> available_glo_prns;
std::set<uint32_t>::iterator it;
for (observables_iter = observablesR1C.begin();
observables_iter != observablesR1C.end();
for (observables_iter = observablesR1C.cbegin();
observables_iter != observablesR1C.cend();
observables_iter++)
{
uint32_t prn_ = observables_iter->second.PRN;
@@ -7939,8 +7939,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
available_glo_prns.insert(prn_);
}
}
for (observables_iter = observablesR2C.begin();
observables_iter != observablesR2C.end();
for (observables_iter = observablesR2C.cbegin();
observables_iter != observablesR2C.cend();
observables_iter++)
{
uint32_t prn_ = observables_iter->second.PRN;
@@ -7966,8 +7966,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
std::string s;
std::string lineObs;
for (observables_iter = observablesE1B.begin();
observables_iter != observablesE1B.end();
for (observables_iter = observablesE1B.cbegin();
observables_iter != observablesE1B.cend();
observables_iter++)
{
lineObs.clear();
@@ -8747,8 +8747,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep
std::set<uint32_t>::iterator it;
if (found_1B != std::string::npos)
{
for (observables_iter = observablesE1B.begin();
observables_iter != observablesE1B.end();
for (observables_iter = observablesE1B.cbegin();
observables_iter != observablesE1B.cend();
observables_iter++)
{
uint32_t prn_ = observables_iter->second.PRN;

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

@@ -53,6 +53,7 @@
#include "rtklib_solver.h"
#include "rtklib_conversions.h"
#include "rtklib_solution.h"
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"
#include "GLONASS_L1_L2_CA.h"
@@ -74,7 +75,11 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
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};
ssat_t ssat0 = {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}}}, {{}, {}}};
for (unsigned int i = 0; i < MAXSAT; i++)
{
pvt_ssat[i] = ssat0;
}
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
@@ -772,9 +777,9 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
for (int i = 0; i < MAXSAT; i++)
{
nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */
nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */
nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; // L1/E1
nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; // L2
nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; // L5/E5
}
result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data);
@@ -783,18 +788,22 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
LOG(INFO) << "RTKLIB rtkpos error";
DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
this->set_time_offset_s(0.0); //reset rx time estimation
this->set_time_offset_s(0.0); // reset rx time estimation
this->set_num_valid_observations(0);
}
else
{
this->set_num_valid_observations(rtk_.sol.ns); //record the number of valid satellites used by the PVT solver
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 (rtk_.ssat[i].vs == 1) used_sats++;
pvt_ssat[i] = rtk_.ssat[i];
if (rtk_.ssat[i].vs == 1)
{
used_sats++;
}
}
std::vector<double> azel;
@@ -809,8 +818,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
index_aux++;
}
}
if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
this->set_valid_position(true);
arma::vec rx_position_and_time(4);
rx_position_and_time(0) = pvt_sol.rr[0]; // [m]
@@ -827,6 +836,22 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
rx_position_and_time(3) = pvt_sol.dtr[0] / GPS_C_m_s; // the receiver clock offset is expressed in [meters], so we convert it into [s]
}
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
//compute Ground speed and COG
double ground_speed_ms = 0.0;
double pos[3];
double enuv[3];
ecef2pos(pvt_sol.rr, pos);
ecef2enu(pos, &pvt_sol.rr[3], enuv);
this->set_speed_over_ground(norm_rtk(enuv, 2));
double new_cog;
if (ground_speed_ms >= 1.0)
{
new_cog = atan2(enuv[0], enuv[1]) * R2D;
if (new_cog < 0.0) new_cog += 360.0;
this->set_course_over_ground(new_cog);
}
//observable fix:
//double offset_s = this->get_time_offset_s();
//this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds]

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

@@ -86,6 +86,7 @@ private:
public:
sol_t pvt_sol;
ssat_t pvt_ssat[MAXSAT];
rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, rtk_t& rtk);
~rtklib_solver();

4
src/algorithms/channel/libs/channel_fsm.cc
View File

@@ -59,7 +59,6 @@ bool ChannelFsm::Event_stop_channel()
switch (d_state)
{
case 0: //already in stanby
return true;
break;
case 1: //acquisition
d_state = 0;
@@ -70,8 +69,9 @@ bool ChannelFsm::Event_stop_channel()
stop_tracking();
break;
default:
return true;
break;
}
return true;
}
bool ChannelFsm::Event_start_acquisition()

8
src/algorithms/libs/CMakeLists.txt
View File

@@ -39,6 +39,7 @@ set(GNSS_SPLIBS_SOURCES
conjugate_sc.cc
conjugate_ic.cc
gnss_sdr_create_directory.cc
geofunctions.cc
)
set(GNSS_SPLIBS_HEADERS
@@ -63,6 +64,7 @@ set(GNSS_SPLIBS_HEADERS
conjugate_ic.h
gnss_sdr_create_directory.h
gnss_circular_deque.h
geofunctions.h
)
@@ -101,6 +103,7 @@ include_directories(
${Boost_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
${ARMADILLO_INCLUDE_DIRS}
${GNURADIO_RUNTIME_INCLUDE_DIRS}
${GNURADIO_BLOCKS_INCLUDE_DIRS}
${VOLK_INCLUDE_DIRS}
@@ -128,6 +131,7 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
${VOLK_LIBRARIES} ${ORC_LIBRARIES}
${VOLK_GNSSSDR_LIBRARIES} ${ORC_LIBRARIES}
${GFlags_LIBS}
${ARMADILLO_LIBRARIES}
${GNURADIO_BLOCKS_LIBRARIES}
${GNURADIO_FFT_LIBRARIES}
${GNURADIO_FILTER_LIBRARIES}
@@ -136,7 +140,9 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
)
if(NOT VOLK_GNSSSDR_FOUND)
add_dependencies(gnss_sp_libs volk_gnsssdr_module)
add_dependencies(gnss_sp_libs volk_gnsssdr_module armadillo-${armadillo_RELEASE})
else(NOT VOLK_GNSSSDR_FOUND)
add_dependencies(gnss_sp_libs armadillo-${armadillo_RELEASE})
endif(NOT VOLK_GNSSSDR_FOUND)
if(${GFLAGS_GREATER_20})

775
src/algorithms/libs/geofunctions.cc Normal file
View File

@@ -0,0 +1,775 @@
/*!
* \file geofunctions.cc
* \brief A set of coordinate transformations functions and helpers,
* some of them migrated from MATLAB, for geographic information systems.
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "geofunctions.h"
const double STRP_PI = 3.1415926535898; // Pi as defined in IS-GPS-200E
arma::mat Skew_symmetric(const arma::vec &a)
{
arma::mat A = arma::zeros(3, 3);
A << 0.0 << -a(2) << a(1) << arma::endr
<< a(2) << 0.0 << -a(0) << arma::endr
<< -a(1) << a(0) << 0 << arma::endr;
// {{0, -a(2), a(1)},
// {a(2), 0, -a(0)},
// {-a(1), a(0), 0}};
return A;
}
double WGS84_g0(double Lat_rad)
{
const double k = 0.001931853; // normal gravity constant
const double e2 = 0.00669438002290; // the square of the first numerical eccentricity
const double nge = 9.7803253359; // normal gravity value on the equator (m/sec^2)
double b = sin(Lat_rad); // Lat in degrees
b = b * b;
double g0 = nge * (1 + k * b) / (sqrt(1 - e2 * b));
return g0;
}
double WGS84_geocentric_radius(double Lat_geodetic_rad)
{
// WGS84 earth model Geocentric radius (Eq. 2.88)
const double WGS84_A = 6378137.0; // Semi-major axis of the Earth, a [m]
const double WGS84_IF = 298.257223563; // Inverse flattening of the Earth
const double WGS84_F = (1.0 / WGS84_IF); // The flattening of the Earth
// double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m]
double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); // Eccentricity of the Earth
// transverse radius of curvature
double R_E = WGS84_A / sqrt(1 - WGS84_E * WGS84_E * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.66)
// geocentric radius at the Earth surface
double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) +
(1 - WGS84_E * WGS84_E) * (1 - WGS84_E * WGS84_E) * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.88)
return r_eS;
}
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
{
double lambda;
double phi;
double h;
const double dtr = STRP_PI / 180.0;
const double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
const double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
// Transform x into geodetic coordinates
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
arma::mat F = {{-sl, -sb * cl, cb * cl},
{cl, -sb * sl, cb * sl},
{0.0, cb, sb}};
arma::vec local_vector;
local_vector = arma::htrans(F) * dx;
double E = local_vector(0);
double N = local_vector(1);
double U = local_vector(2);
double hor_dis;
hor_dis = sqrt(E * E + N * N);
if (hor_dis < 1.0E-20)
{
*Az = 0.0;
*El = 90.0;
}
else
{
*Az = atan2(E, N) / dtr;
*El = atan2(U, hor_dis) / dtr;
}
if (*Az < 0)
{
*Az = *Az + 360.0;
}
*D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
return 0;
}
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
{
*h = 0.0;
const double tolsq = 1.e-10; // tolerance to accept convergence
const int maxit = 10; // max number of iterations
const double rtd = 180.0 / STRP_PI;
// compute square of eccentricity
double esq;
if (finv < 1.0E-20)
{
esq = 0.0;
}
else
{
esq = (2.0 - 1.0 / finv) / finv;
}
// first guess
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
// direct calculation of longitude
if (P > 1.0E-20)
{
*dlambda = atan2(Y, X) * rtd;
}
else
{
*dlambda = 0.0;
}
// correct longitude bound
if (*dlambda < 0)
{
*dlambda = *dlambda + 360.0;
}
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
double sinphi;
if (r > 1.0E-20)
{
sinphi = Z / r;
}
else
{
sinphi = 0.0;
}
*dphi = asin(sinphi);
// initial value of height = distance from origin minus
// approximate distance from origin to surface of ellipsoid
if (r < 1.0E-20)
{
*h = 0.0;
return 1;
}
*h = r - a * (1 - sinphi * sinphi / finv);
// iterate
double cosphi;
double N_phi;
double dP;
double dZ;
double oneesq = 1.0 - esq;
for (int i = 0; i < maxit; i++)
{
sinphi = sin(*dphi);
cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1.0 - esq * sinphi * sinphi);
// compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
// update height and latitude
*h = *h + (sinphi * dZ + cosphi * dP);
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
// test for convergence
if ((dP * dP + dZ * dZ) < tolsq)
{
break;
}
if (i == (maxit - 1))
{
// LOG(WARNING) << "The computation of geodetic coordinates did not converge";
}
}
*dphi = (*dphi) * rtd;
return 0;
}
arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
{
// Parameters
const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2)
const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant
const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
// Calculate distance from center of the Earth
double mag_r = sqrt(arma::as_scalar(r_eb_e.t() * r_eb_e));
// If the input position is 0,0,0, produce a dummy output
arma::vec g = arma::zeros(3, 1);
if (mag_r != 0)
{
// Calculate gravitational acceleration using (2.142)
double z_scale = 5 * pow((r_eb_e(2) / mag_r), 2);
arma::vec tmp_vec = {(1 - z_scale) * r_eb_e(0),
(1 - z_scale) * r_eb_e(1),
(3 - z_scale) * r_eb_e(2)};
arma::vec gamma_ = (-mu / pow(mag_r, 3)) * (r_eb_e + 1.5 * J_2 * pow(R_0 / mag_r, 2) * tmp_vec);
// Add centripetal acceleration using (2.133)
g(0) = gamma_(0) + pow(omega_ie, 2) * r_eb_e(0);
g(1) = gamma_(1) + pow(omega_ie, 2) * r_eb_e(1);
g(2) = gamma_(2);
}
return g;
}
arma::vec LLH_to_deg(const arma::vec &LLH)
{
const double rtd = 180.0 / STRP_PI;
arma::vec deg = arma::zeros(3, 1);
deg(0) = LLH(0) * rtd;
deg(1) = LLH(1) * rtd;
deg(2) = LLH(2);
return deg;
}
double degtorad(double angleInDegrees)
{
double angleInRadians = (STRP_PI / 180.0) * angleInDegrees;
return angleInRadians;
}
double radtodeg(double angleInRadians)
{
double angleInDegrees = (180.0 / STRP_PI) * angleInRadians;
return angleInDegrees;
}
double mstoknotsh(double MetersPerSeconds)
{
double knots = mstokph(MetersPerSeconds) * 0.539957;
return knots;
}
double mstokph(double MetersPerSeconds)
{
double kph = 3600.0 * MetersPerSeconds / 1e3;
return kph;
}
arma::vec CTM_to_Euler(const arma::mat &C)
{
// Calculate Euler angles using (2.23)
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;
if (CTM(0, 2) > 1.0) CTM(0, 2) = 1.0;
eul(1) = -asin(CTM(0, 2)); // pitch
eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw
return eul;
}
arma::mat Euler_to_CTM(const arma::vec &eul)
{
// Eq.2.15
// Euler angles to Attitude matrix is equivalent to rotate the body
// in the three axes:
// arma::mat Ax= {{1,0,0}, {0,cos(Att_phi),sin(Att_phi)} ,{0,-sin(Att_phi),cos(Att_phi)}};
// arma::mat Ay= {{cos(Att_theta), 0, -sin(Att_theta)}, {0,1,0} , {sin(Att_theta), 0, cos(Att_theta)}};
// arma::mat Az= {{cos(Att_psi), sin(Att_psi), 0}, {-sin(Att_psi), cos(Att_psi), 0},{0,0,1}};
// arma::mat C_b_n=Ax*Ay*Az; // Attitude expressed in the LOCAL FRAME (NED)
// C_b_n=C_b_n.t();
// Precalculate sines and cosines of the Euler angles
double sin_phi = sin(eul(0));
double cos_phi = cos(eul(0));
double sin_theta = sin(eul(1));
double cos_theta = cos(eul(1));
double sin_psi = sin(eul(2));
double cos_psi = cos(eul(2));
// Calculate coordinate transformation matrix using (2.22)
arma::mat C = {{cos_theta * cos_psi, cos_theta * sin_psi, -sin_theta},
{-cos_phi * sin_psi + sin_phi * sin_theta * cos_psi, cos_phi * cos_psi + sin_phi * sin_theta * sin_psi, sin_phi * cos_theta},
{sin_phi * sin_psi + cos_phi * sin_theta * cos_psi, -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi, cos_phi * cos_theta}};
return C;
}
arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection)
{
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(XYZ[1], XYZ[0]);
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(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (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.0 + ex2 * (cos(phi) * cos(phi)));
phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N;
iterations = iterations + 1;
if (iterations > 100)
{
// std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break;
}
}
while (std::fabs(h - oldh) > 1.0e-12);
arma::vec LLH = {{phi, lambda, h}}; // radians
return LLH;
}
void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::mat &C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n)
{
// Compute the Latitude of the ECEF position
LLH = cart2geo(r_eb_e, 4); // ECEF -> WGS84 geographical
// Calculate ECEF to Geographical coordinate transformation matrix using (2.150)
double cos_lat = cos(LLH(0));
double sin_lat = sin(LLH(0));
double cos_long = cos(LLH(1));
double sin_long = sin(LLH(1));
// C++11 and arma >= 5.2
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
// {-sin_long, cos_long, 0},
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; //ECEF to Geo
arma::mat C_e_n = arma::zeros(3, 3);
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
<< -sin_long << cos_long << 0 << arma::endr
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; // ECEF to Geo
// Transform velocity using (2.73)
v_eb_n = C_e_n * v_eb_e;
C_b_n = C_e_n * C_b_e; // Attitude conversion from ECEF to NED
}
void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
{
// Parameters
double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
double e = 0.0818191908425; // WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105)
double R_E = R_0 / sqrt(1.0 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
// Convert position using (2.112)
double cos_lat = cos(LLH(0));
double sin_lat = sin(LLH(0));
double cos_long = cos(LLH(1));
double sin_long = sin(LLH(1));
r_eb_e = {(R_E + LLH(2)) * cos_lat * cos_long,
(R_E + LLH(2)) * cos_lat * sin_long,
((1 - e * e) * R_E + LLH(2)) * sin_lat};
// Calculate ECEF to Geo coordinate transformation matrix using (2.150)
// C++11 and arma>=5.2
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
// {-sin_long, cos_long, 0},
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}};
arma::mat C_e_n = arma::zeros(3, 3);
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
<< -sin_long << cos_long << 0 << arma::endr
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
// Transform velocity using (2.73)
v_eb_e = C_e_n.t() * v_eb_n;
// Transform attitude using (2.15)
C_b_e = C_e_n.t() * C_b_n;
}
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
{
// Parameters
const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters
const double e = 0.0818191908425; // WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105)
double R_E = R_0 / sqrt(1 - pow(e * sin(L_b), 2));
// Convert position using (2.112)
double cos_lat = cos(L_b);
double sin_lat = sin(L_b);
double cos_long = cos(lambda_b);
double sin_long = sin(lambda_b);
r_eb_e = {(R_E + h_b) * cos_lat * cos_long,
(R_E + h_b) * cos_lat * sin_long,
((1 - pow(e, 2)) * R_E + h_b) * sin_lat};
// Calculate ECEF to Geo coordinate transformation matrix using (2.150)
arma::mat C_e_n = arma::zeros(3, 3);
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
<< -sin_long << cos_long << 0 << arma::endr
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
// Transform velocity using (2.73)
v_eb_e = C_e_n.t() * v_eb_n;
}
double great_circle_distance(double lat1, double lon1, double lat2, double lon2)
{
// The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
// generally used geo measurement function
double R = 6378.137; // Radius of earth in KM
double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0;
double dLon = lon2 * STRP_PI / 180.0 - lon1 * STRP_PI / 180.0;
double a = sin(dLat / 2.0) * sin(dLat / 2.0) +
cos(lat1 * STRP_PI / 180.0) * cos(lat2 * STRP_PI / 180.0) *
sin(dLon / 2) * sin(dLon / 2.0);
double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
double d = R * c;
return d * 1000.0; // meters
}
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu)
{
// Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'
//
// Inputs:
// r_eb_e - Cartesian coordinates. Coordinates are referenced
// with respect to the International Terrestrial Reference
// Frame 1996 (ITRF96)
// zone - UTM zone of the given position
//
// Outputs:
// r_enu - UTM coordinates (Easting, Northing, Uping)
//
// Originally written in Matlab by Kai Borre, Nov. 1994
// Implemented in C++ by J.Arribas
//
// This implementation is based upon
// O. Andersson & K. Poder (1981) Koordinattransformationer
// ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
// Vol. 30: 552--571 and Vol. 31: 76
//
// An excellent, general reference (KW) is
// R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
// h\"oheren Geod\"asie und Kartographie.
// Erster Band, Springer Verlag
//
// Explanation of variables used:
// f flattening of ellipsoid
// a semi major axis in m
// m0 1 - scale at central meridian; for UTM 0.0004
// Q_n normalized meridian quadrant
// E0 Easting of central meridian
// L0 Longitude of central meridian
// bg constants for ellipsoidal geogr. to spherical geogr.
// gb constants for spherical geogr. to ellipsoidal geogr.
// gtu constants for ellipsoidal N, E to spherical N, E
// utg constants for spherical N, E to ellipoidal N, E
// tolutm tolerance for utm, 1.2E-10*meridian quadrant
// tolgeo tolerance for geographical, 0.00040 second of arc
//
// B, L refer to latitude and longitude. Southern latitude is negative
// International ellipsoid of 1924, valid for ED50
double a = 6378388.0;
double f = 1.0 / 297.0;
double ex2 = (2.0 - f) * f / ((1.0 - f) * (1.0 - f));
double c = a * sqrt(1.0 + ex2);
arma::vec vec = r_eb_e;
vec(2) = vec(2) - 4.5;
double alpha = 0.756e-6;
arma::mat R = {{1.0, -alpha, 0.0}, {alpha, 1.0, 0.0}, {0.0, 0.0, 1.0}};
arma::vec trans = {89.5, 93.8, 127.6};
double scale = 0.9999988;
arma::vec v = scale * R * vec + trans; // coordinate vector in ED50
double L = atan2(v(1), v(0));
double N1 = 6395000.0; // preliminary value
double B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value
double U = 0.1;
double oldU = 0.0;
int iterations = 0;
while (fabs(U - oldU) > 1.0E-4)
{
oldU = U;
N1 = c / sqrt(1.0 + ex2 * (cos(B) * cos(B)));
B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1 + U), arma::norm(v.subvec(0, 1)) / (N1 + U));
U = arma::norm(v.subvec(0, 1)) / cos(B) - N1;
iterations = iterations + 1;
if (iterations > 100)
{
std::cout << "Failed to approximate U with desired precision. U-oldU:" << U - oldU << std::endl;
break;
}
}
// Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
double m0 = 0.0004;
double n = f / (2.0 - f);
double m = n * n * (1.0 / 4.0 + n * n / 64.0);
double w = (a * (-n - m0 + m * (1.0 - m0))) / (1.0 + n);
double Q_n = a + w;
// Easting and longitude of central meridian
double E0 = 500000.0;
double L0 = (zone - 30) * 6.0 - 3.0;
// Check tolerance for reverse transformation
// double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n;
// double tolgeo = 0.000040;
// Coefficients of trigonometric series
//
// ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52)
// bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45))));
// bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9)));
// bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21);
// bg[4] = n ^ 4 * 1237 / 630;
//
// spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45)));
// gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45)));
// gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35));
// gb[4] = n ^ 4 * 4279 / 630;
//
// spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180)));
// gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440));
// gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140));
// gtu[4] = n ^ 4 * 49561 / 161280;
//
// ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360)));
// utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440));
// utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840);
// utg[4] = n ^ 4 * (-4397 / 161280);
//
// With f = 1 / 297 we get
arma::colvec bg = {-3.37077907e-3,
4.73444769e-6,
-8.29914570e-9,
1.58785330e-11};
arma::colvec gb = {3.37077588e-3,
6.62769080e-6,
1.78718601e-8,
5.49266312e-11};
arma::colvec gtu = {8.41275991e-4,
7.67306686e-7,
1.21291230e-9,
2.48508228e-12};
arma::colvec utg = {-8.41276339e-4,
-5.95619298e-8,
-1.69485209e-10,
-2.20473896e-13};
// Ellipsoidal latitude, longitude to spherical latitude, longitude
bool neg_geo = false;
if (B < 0.0) neg_geo = true;
double Bg_r = fabs(B);
double res_clensin = clsin(bg, 4, 2.0 * Bg_r);
Bg_r = Bg_r + res_clensin;
L0 = L0 * STRP_PI / 180.0;
double Lg_r = L - L0;
// Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E
double cos_BN = cos(Bg_r);
double Np = atan2(sin(Bg_r), cos(Lg_r) * cos_BN);
double Ep = atanh(sin(Lg_r) * cos_BN);
// Spherical normalized N, E to ellipsoidal N, E
Np = 2.0 * Np;
Ep = 2.0 * Ep;
double dN;
double dE;
clksin(gtu, 4, Np, Ep, &dN, &dE);
Np = Np / 2.0;
Ep = Ep / 2.0;
Np = Np + dN;
Ep = Ep + dE;
double N = Q_n * Np;
double E = Q_n * Ep + E0;
if (neg_geo)
{
N = -N + 20000000.0;
}
r_enu(0) = E;
r_enu(1) = N;
r_enu(2) = U;
}
double clsin(const arma::colvec &ar, int degree, double argument)
{
// Clenshaw summation of sinus of argument.
//
// result = clsin(ar, degree, argument);
//
// Originally written in Matlab by Kai Borre
// Implemented in C++ by J.Arribas
double cos_arg = 2.0 * cos(argument);
double hr1 = 0.0;
double hr = 0.0;
double hr2;
for (int t = degree; t > 0; t--)
{
hr2 = hr1;
hr1 = hr;
hr = ar(t - 1) + cos_arg * hr1 - hr2;
}
return (hr * sin(argument));
}
void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im)
{
// Clenshaw summation of sinus with complex argument
// [re, im] = clksin(ar, degree, arg_real, arg_imag);
//
// Originally written in Matlab by Kai Borre
// Implemented in C++ by J.Arribas
double sin_arg_r = sin(arg_real);
double cos_arg_r = cos(arg_real);
double sinh_arg_i = sinh(arg_imag);
double cosh_arg_i = cosh(arg_imag);
double r = 2.0 * cos_arg_r * cosh_arg_i;
double i = -2.0 * sin_arg_r * sinh_arg_i;
double hr1 = 0.0;
double hr = 0.0;
double hi1 = 0.0;
double hi = 0.0;
double hi2;
double hr2;
for (int t = degree; t > 0; t--)
{
hr2 = hr1;
hr1 = hr;
hi2 = hi1;
hi1 = hi;
double z = ar(t - 1) + r * hr1 - i * hi - hr2;
hi = i * hr1 + r * hi1 - hi2;
hr = z;
}
r = sin_arg_r * cosh_arg_i;
i = cos_arg_r * sinh_arg_i;
*re = r * hr - i * hi;
*im = r * hi + i * hr;
}
int findUtmZone(double latitude_deg, double longitude_deg)
{
// Function finds the UTM zone number for given longitude and latitude.
// The longitude value must be between -180 (180 degree West) and 180 (180
// degree East) degree. The latitude must be within -80 (80 degree South) and
// 84 (84 degree North).
//
// utmZone = findUtmZone(latitude, longitude);
//
// Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
// 15 deg 30 min).
//
// Originally written in Matlab by Darius Plausinaitis
// Implemented in C++ by J.Arribas
// Check value bounds
if ((longitude_deg > 180.0) || (longitude_deg < -180.0))
std::cout << "Longitude value exceeds limits (-180:180).\n";
if ((latitude_deg > 84.0) || (latitude_deg < -80.0))
std::cout << "Latitude value exceeds limits (-80:84).\n";
//
// Find zone
//
// Start at 180 deg west = -180 deg
int utmZone = floor((180 + longitude_deg) / 6) + 1;
// Correct zone numbers for particular areas
if (latitude_deg > 72.0)
{
// Corrections for zones 31 33 35 37
if ((longitude_deg >= 0.0) && (longitude_deg < 9.0))
{
utmZone = 31;
}
else if ((longitude_deg >= 9.0) && (longitude_deg < 21.0))
{
utmZone = 33;
}
else if ((longitude_deg >= 21.0) && (longitude_deg < 33.0))
{
utmZone = 35;
}
else if ((longitude_deg >= 33.0) && (longitude_deg < 42.0))
{
utmZone = 37;
}
}
else if ((latitude_deg >= 56.0) && (latitude_deg < 64.0))
{
// Correction for zone 32
if ((longitude_deg >= 3.0) && (longitude_deg < 12.0))
utmZone = 32;
}
return utmZone;
}

184
src/algorithms/libs/geofunctions.h Normal file
View File

@@ -0,0 +1,184 @@
/*!
* \file geofunctions.h
* \brief A set of coordinate transformations functions and helpers,
* some of them migrated from MATLAB, for geographic information systems.
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GEOFUNCTIONS_H
#define GNSS_SDR_GEOFUNCTIONS_H
#include <armadillo>
arma::mat Skew_symmetric(const arma::vec &a); //!< Calculates skew-symmetric matrix
double WGS84_g0(double Lat_rad);
double WGS84_geocentric_radius(double Lat_geodetic_rad);
/*!
* \brief Transformation of vector dx into topocentric coordinate
* system with origin at x
* Inputs:
* x - vector origin coordinates (in ECEF system [X; Y; Z;])
* dx - vector ([dX; dY; dZ;]).
*
* Outputs:
* D - vector length. Units like the input
* Az - azimuth from north positive clockwise, degrees
* El - elevation angle, degrees
*
* Based on a Matlab function by Kai Borre
*/
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
/*!
* \brief Subroutine to calculate geodetic coordinates latitude, longitude,
* height given Cartesian coordinates X,Y,Z, and reference ellipsoid
* values semi-major axis (a) and the inverse of flattening (finv).
*
* The output units of angular quantities will be in decimal degrees
* (15.5 degrees not 15 deg 30 min). The output units of h will be the
* same as the units of X,Y,Z,a.
*
* Inputs:
* a - semi-major axis of the reference ellipsoid
* finv - inverse of flattening of the reference ellipsoid
* X,Y,Z - Cartesian coordinates
*
* Outputs:
* dphi - latitude
* dlambda - longitude
* h - height above reference ellipsoid
*
* Based in a Matlab function by Kai Borre
*/
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
arma::mat Gravity_ECEF(const arma::vec &r_eb_e); //!< Calculates acceleration due to gravity resolved about ECEF-frame
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
* coordinates (latitude, longitude, h) on a selected reference ellipsoid.
*
* Choices of Reference Ellipsoid for Geographical Coordinates
* 0. International Ellipsoid 1924
* 1. International Ellipsoid 1967
* 2. World Geodetic System 1972
* 3. Geodetic Reference System 1980
* 4. World Geodetic System 1984
*/
arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection);
arma::vec LLH_to_deg(const arma::vec &LLH);
double degtorad(double angleInDegrees);
double radtodeg(double angleInRadians);
double mstoknotsh(double MetersPerSeconds);
double mstokph(double Kph);
arma::vec CTM_to_Euler(const arma::mat &C);
arma::mat Euler_to_CTM(const arma::vec &eul);
void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::mat &C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n);
/*!
* \brief From Geographic to ECEF coordinates
*
* Inputs:
* LLH latitude (rad), longitude (rad), height (m)
* v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
* north, east, and down (m/s)
* C_b_n body-to-NED coordinate transformation matrix
*
* Outputs:
* r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
* along ECEF-frame axes (m)
* v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
* ECEF-frame axes (m/s)
* C_b_e body-to-ECEF-frame coordinate transformation matrix
*
*/
void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e);
/*!
* \brief Converts curvilinear to Cartesian position and velocity
* resolving axes from NED to ECEF
* This function created 11/4/2012 by Paul Groves
*
* Inputs:
* L_b latitude (rad)
* lambda_b longitude (rad)
* h_b height (m)
* v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
* north, east, and down (m/s)
*
* Outputs:
* r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
* along ECEF-frame axes (m)
* v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
* ECEF-frame axes (m/s)
*/
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
/*!
* \brief The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes.
*/
double great_circle_distance(double lat1, double lon1, double lat2, double lon2);
/*!
* \brief Transformation of ECEF (X,Y,Z) to (E,N,U) in UTM, zone 'zone'.
*/
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu);
/*!
* \brief Function finds the UTM zone number for given longitude and latitude.
*/
int findUtmZone(double latitude_deg, double longitude_deg);
/*!
* \brief Clenshaw summation of sinus of argument.
*/
double clsin(const arma::colvec &ar, int degree, double argument);
/*!
* \brief Clenshaw summation of sinus with complex argument.
*/
void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im);
#endif

70
src/algorithms/libs/gnss_sdr_valve.cc
View File

@@ -2,6 +2,7 @@
* \file gnss_sdr_valve.cc
* \brief Implementation of a GNU Radio block that sends a STOP message to the
* control queue right after a specific number of samples have passed through it.
* \author Javier Arribas, 2018. jarribas(at)cttc.es
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
*
*
@@ -39,42 +40,65 @@
gnss_sdr_valve::gnss_sdr_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue) : gr::sync_block("valve",
gr::io_signature::make(1, 1, sizeof_stream_item),
gr::io_signature::make(1, 1, sizeof_stream_item)),
d_nitems(nitems),
d_ncopied_items(0),
d_queue(queue)
gr::msg_queue::sptr queue, bool stop_flowgraph) : gr::sync_block("valve",
gr::io_signature::make(1, 1, sizeof_stream_item),
gr::io_signature::make(1, 1, sizeof_stream_item)),
d_nitems(nitems),
d_ncopied_items(0),
d_queue(queue),
d_stop_flowgraph(stop_flowgraph)
{
d_open_valve = false;
}
boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue)
boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue, bool stop_flowgraph)
{
boost::shared_ptr<gnss_sdr_valve> valve_(new gnss_sdr_valve(sizeof_stream_item, nitems, queue));
boost::shared_ptr<gnss_sdr_valve> valve_(new gnss_sdr_valve(sizeof_stream_item, nitems, queue, stop_flowgraph));
return valve_;
}
boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue)
{
boost::shared_ptr<gnss_sdr_valve> valve_(new gnss_sdr_valve(sizeof_stream_item, nitems, queue, false));
return valve_;
}
void gnss_sdr_valve::open_valve()
{
d_open_valve = true;
}
int gnss_sdr_valve::work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
if (d_ncopied_items >= d_nitems)
if (d_open_valve == false)
{
ControlMessageFactory *cmf = new ControlMessageFactory();
d_queue->handle(cmf->GetQueueMessage(200, 0));
LOG(INFO) << "Stopping receiver, " << d_ncopied_items << " samples processed";
delete cmf;
return -1; // Done!
if (d_ncopied_items >= d_nitems)
{
ControlMessageFactory *cmf = new ControlMessageFactory();
d_queue->handle(cmf->GetQueueMessage(200, 0));
LOG(INFO) << "Stopping receiver, " << d_ncopied_items << " samples processed";
delete cmf;
if (d_stop_flowgraph)
{
return -1; // Done!
}
else
{
usleep(1000000);
return 0; //do not produce or consume
}
}
unsigned long long n = std::min(d_nitems - d_ncopied_items, static_cast<long long unsigned int>(noutput_items));
if (n == 0) return 0;
memcpy(output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0));
d_ncopied_items += n;
return n;
}
else
{
memcpy(output_items[0], input_items[0], noutput_items * input_signature()->sizeof_stream_item(0));
return noutput_items;
}
unsigned long long n = std::min(d_nitems - d_ncopied_items, static_cast<long long unsigned int>(noutput_items));
if (n == 0) return 0;
memcpy(output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0));
//for(long long i = 0; i++; i<n)
// {
// output_items[i] = input_items[i];
// }
d_ncopied_items += n;
return n;
}

20
src/algorithms/libs/gnss_sdr_valve.h
View File

@@ -2,6 +2,7 @@
* \file gnss_sdr_valve.h
* \brief Interface of a GNU Radio block that sends a STOP message to the
* control queue right after a specific number of samples have passed through it.
* \author Javier Arribas, 2018. jarribas(at)cttc.es
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
*
* -------------------------------------------------------------------------
@@ -37,10 +38,13 @@
#include <gnuradio/msg_queue.h>
#include <boost/shared_ptr.hpp>
boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue,
bool stop_flowgraph);
/*!
* \brief Implementation of a GNU Radio block that sends a STOP message to the
* control queue right after a specific number of samples have passed through it.
@@ -50,14 +54,24 @@ class gnss_sdr_valve : public gr::sync_block
friend boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
gnss_sdr_valve(size_t sizeof_stream_item,
friend boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue);
gr::msg_queue::sptr queue,
bool stop_flowgraph);
unsigned long long d_nitems;
unsigned long long d_ncopied_items;
gr::msg_queue::sptr d_queue;
bool d_stop_flowgraph;
bool d_open_valve;
public:
gnss_sdr_valve(size_t sizeof_stream_item,
unsigned long long nitems,
gr::msg_queue::sptr queue, bool stop_flowgraph);
void open_valve();
int work(int noutput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);

58
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@@ -116,7 +116,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glona
eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph)
{
eph_t rtklib_sat = {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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
//Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris
rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO;
rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1;
@@ -174,7 +174,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph)
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph)
{
eph_t rtklib_sat = {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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
rtklib_sat.sat = gps_eph.i_satellite_PRN;
rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A;
rtklib_sat.M0 = gps_eph.d_M_0;
@@ -231,7 +231,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph)
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
{
eph_t rtklib_sat = {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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN;
const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170
rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A;
@@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
return rtklib_sat;
}
alm_t alm_to_rtklib(const Gps_Almanac& gps_alm)
{
alm_t rtklib_alm;
rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
rtklib_alm.sat = gps_alm.i_satellite_PRN;
rtklib_alm.svh = gps_alm.i_SV_health;
rtklib_alm.svconf = gps_alm.i_AS_status;
rtklib_alm.week = gps_alm.i_WNa;
rtklib_alm.toa = gpst2time(gps_alm.i_WNa, gps_alm.i_Toa);
rtklib_alm.A = gps_alm.d_sqrt_A * gps_alm.d_sqrt_A;
rtklib_alm.e = gps_alm.d_e_eccentricity;
rtklib_alm.i0 = gps_alm.d_Delta_i + 0.3;
rtklib_alm.OMG0 = gps_alm.d_OMEGA0;
rtklib_alm.OMGd = gps_alm.d_OMEGA_DOT;
rtklib_alm.omg = gps_alm.d_OMEGA;
rtklib_alm.M0 = gps_alm.d_M_0;
rtklib_alm.f0 = gps_alm.d_A_f0;
rtklib_alm.f1 = gps_alm.d_A_f1;
rtklib_alm.toas = gps_alm.i_Toa;
return rtklib_alm;
}
alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm)
{
alm_t rtklib_alm;
rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
rtklib_alm.sat = gal_alm.i_satellite_PRN;
rtklib_alm.svh = gal_alm.E1B_HS;
rtklib_alm.svconf = gal_alm.E1B_HS;
rtklib_alm.week = gal_alm.i_WNa;
rtklib_alm.toa = gpst2time(gal_alm.i_WNa, gal_alm.i_Toa);
rtklib_alm.A = 5440.588203494 + gal_alm.d_Delta_sqrt_A;
rtklib_alm.A = rtklib_alm.A * rtklib_alm.A;
rtklib_alm.e = gal_alm.d_e_eccentricity;
rtklib_alm.i0 = gal_alm.d_Delta_i + 0.31111;
rtklib_alm.OMG0 = gal_alm.d_OMEGA0;
rtklib_alm.OMGd = gal_alm.d_OMEGA_DOT;
rtklib_alm.omg = gal_alm.d_OMEGA;
rtklib_alm.M0 = gal_alm.d_M_0;
rtklib_alm.f0 = gal_alm.d_A_f0;
rtklib_alm.f1 = gal_alm.d_A_f1;
rtklib_alm.toas = gal_alm.i_Toa;
return rtklib_alm;
}

6
src/algorithms/libs/rtklib/rtklib_conversions.h
View File

@@ -38,10 +38,16 @@
#include "gps_cnav_ephemeris.h"
#include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_utc_model.h"
#include "gps_almanac.h"
#include "galileo_almanac.h"
eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph);
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph);
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph);
alm_t alm_to_rtklib(const Gps_Almanac& gps_alm);
alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm);
/*!
* \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart
* \param glonass_gnav_eph GLONASS GNAV Ephemeris structure

153
src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc
View File

@@ -59,7 +59,7 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface*
adc_bits_ = configuration->property(role + ".adc_bits", 4);
n_channels_ = configuration->property(role + ".total_channels", 1);
sel_ch_ = configuration->property(role + ".sel_ch", 1);
item_size_ = sizeof(int);
item_size_ = sizeof(int32_t);
int64_t bytes_seek = configuration->property(role + ".bytes_to_skip", 65536);
double sample_size_byte = static_cast<double>(adc_bits_) / 4.0;
@@ -69,17 +69,22 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface*
}
if (n_channels_ > 1)
{
for (uint32_t i = 0; i < (n_channels_ - 1); i++)
for (uint32_t i = 0; i < n_channels_; i++)
{
null_sinks_.push_back(gr::blocks::null_sink::make(item_size_));
null_sinks_.push_back(gr::blocks::null_sink::make(sizeof(gr_complex)));
unpack_spir_vec_.push_back(make_unpack_spir_gss6450_samples(adc_bits_));
if (endian_swap_)
{
endian_vec_.push_back(gr::blocks::endian_swap::make(item_size_));
}
}
DLOG(INFO) << "NUMBER OF NULL SINKS = " << null_sinks_.size();
}
try
{
file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_);
file_source_->seek(bytes_seek / item_size_, SEEK_SET);
unpack_spir_ = make_unpack_spir_gss6450_samples(adc_bits_);
deint_ = gr::blocks::deinterleave::make(item_size_);
}
catch (const std::exception& e)
@@ -143,22 +148,19 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface*
LOG(INFO) << "Total number samples to be processed= " << samples_ << " GNSS signal duration= " << signal_duration_s << " [s]";
std::cout << "GNSS signal recorded time to be processed: " << signal_duration_s << " [s]" << std::endl;
valve_ = gnss_sdr_make_valve(sizeof(gr_complex), samples_, queue_);
DLOG(INFO) << "valve(" << valve_->unique_id() << ")";
for (uint32_t i = 0; i < (n_channels_); i++)
{
valve_vec_.push_back(gnss_sdr_make_valve(sizeof(gr_complex), samples_, queue_));
if (dump_)
{
sink_vec_.push_back(gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str()));
}
if (enable_throttle_control_)
{
throttle_vec_.push_back(gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_));
}
}
if (dump_)
{
sink_ = gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str());
DLOG(INFO) << "file_sink(" << sink_->unique_id() << ")";
}
if (enable_throttle_control_)
{
throttle_ = gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_);
}
if (endian_swap_)
{
endian_ = gr::blocks::endian_swap::make(item_size_);
}
DLOG(INFO) << "File source filename " << filename_;
DLOG(INFO) << "Samples " << samples_;
DLOG(INFO) << "Sampling frequency " << sampling_frequency_;
@@ -188,15 +190,17 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block)
if (samples_ > 0)
{
top_block->connect(file_source_, 0, deint_, 0);
if (endian_swap_)
{
top_block->connect(deint_, sel_ch_ - 1, endian_, 0);
top_block->connect(endian_, 0, unpack_spir_, 0);
top_block->connect(deint_, sel_ch_ - 1, endian_vec_.at(sel_ch_ - 1), 0);
top_block->connect(endian_vec_.at(sel_ch_ - 1), 0, unpack_spir_vec_.at(sel_ch_ - 1), 0);
}
else
{
top_block->connect(deint_, sel_ch_ - 1, unpack_spir_, 0);
top_block->connect(deint_, sel_ch_ - 1, unpack_spir_vec_.at(sel_ch_ - 1), 0);
}
if (n_channels_ > 1)
{
uint32_t aux = 0;
@@ -204,23 +208,37 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block)
{
if (i != (sel_ch_ - 1))
{
top_block->connect(deint_, i, null_sinks_.at(aux), 0);
if (endian_swap_)
{
top_block->connect(deint_, i, endian_vec_.at(i), 0);
top_block->connect(endian_vec_.at(i), 0, unpack_spir_vec_.at(i), 0);
}
else
{
top_block->connect(deint_, i, unpack_spir_vec_.at(i), 0);
}
aux++;
}
}
}
if (enable_throttle_control_)
for (uint32_t i = 0; i < n_channels_; i++)
{
top_block->connect(unpack_spir_, 0, throttle_, 0);
top_block->connect(throttle_, 0, valve_, 0);
}
else
{
top_block->connect(unpack_spir_, 0, valve_, 0);
}
if (dump_)
{
top_block->connect(valve_, 0, sink_, 0);
if (enable_throttle_control_)
{
top_block->connect(unpack_spir_vec_.at(i), 0, throttle_vec_.at(i), 0);
top_block->connect(throttle_vec_.at(i), 0, valve_vec_.at(i), 0);
}
else
{
top_block->connect(unpack_spir_vec_.at(i), 0, valve_vec_.at(i), 0);
}
if (dump_)
{
top_block->connect(valve_vec_.at(i), 0, sink_vec_.at(i), 0);
}
top_block->connect(valve_vec_.at(i), 0, null_sinks_.at(i), 0);
}
}
else
@@ -237,12 +255,12 @@ void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block)
top_block->disconnect(file_source_, 0, deint_, 0);
if (endian_swap_)
{
top_block->disconnect(deint_, sel_ch_ - 1, endian_, 0);
top_block->disconnect(endian_, 0, unpack_spir_, 0);
top_block->disconnect(deint_, sel_ch_ - 1, endian_vec_.at(sel_ch_ - 1), 0);
top_block->disconnect(endian_vec_.at(sel_ch_ - 1), 0, unpack_spir_vec_.at(sel_ch_ - 1), 0);
}
else
{
top_block->disconnect(deint_, sel_ch_ - 1, unpack_spir_, 0);
top_block->disconnect(deint_, sel_ch_ - 1, unpack_spir_vec_.at(sel_ch_ - 1), 0);
}
if (n_channels_ > 1)
{
@@ -251,23 +269,38 @@ void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block)
{
if (i != (sel_ch_ - 1))
{
top_block->disconnect(deint_, i, null_sinks_.at(aux), 0);
if (endian_swap_)
{
top_block->disconnect(deint_, i, endian_vec_.at(i), 0);
top_block->disconnect(endian_vec_.at(i), 0, unpack_spir_vec_.at(i), 0);
}
else
{
top_block->disconnect(deint_, i, unpack_spir_vec_.at(i), 0);
}
aux++;
}
}
}
if (enable_throttle_control_)
for (uint32_t i = 0; i < (n_channels_); i++)
{
top_block->disconnect(unpack_spir_, 0, throttle_, 0);
top_block->disconnect(throttle_, 0, valve_, 0);
}
else
{
top_block->disconnect(unpack_spir_, 0, valve_, 0);
}
if (dump_)
{
top_block->disconnect(valve_, 0, sink_, 0);
if (enable_throttle_control_)
{
top_block->disconnect(unpack_spir_vec_.at(i), 0, throttle_vec_.at(i), 0);
top_block->disconnect(throttle_vec_.at(i), 0, valve_vec_.at(i), 0);
}
else
{
top_block->disconnect(unpack_spir_vec_.at(i), 0, valve_vec_.at(i), 0);
}
if (dump_)
{
top_block->disconnect(valve_vec_.at(i), 0, sink_vec_.at(i), 0);
}
top_block->disconnect(valve_vec_.at(i), 0, null_sinks_.at(i), 0);
}
}
else
@@ -283,22 +316,12 @@ gr::basic_block_sptr SpirGSS6450FileSignalSource::get_left_block()
return gr::blocks::file_source::sptr();
}
gr::basic_block_sptr SpirGSS6450FileSignalSource::get_right_block(int RF_channel)
{
return valve_vec_.at(RF_channel);
}
gr::basic_block_sptr SpirGSS6450FileSignalSource::get_right_block()
{
if (samples_ > 0)
{
return valve_;
}
else
{
if (enable_throttle_control_)
{
return throttle_;
}
else
{
return unpack_spir_;
}
}
return valve_vec_.at(0);
}

11
src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h
View File

@@ -79,6 +79,7 @@ public:
void connect(gr::top_block_sptr top_block) override;
void disconnect(gr::top_block_sptr top_block) override;
gr::basic_block_sptr get_left_block() override;
gr::basic_block_sptr get_right_block(int RF_channel) override;
gr::basic_block_sptr get_right_block() override;
inline std::string filename() const
@@ -124,12 +125,12 @@ private:
uint32_t sel_ch_;
gr::blocks::file_source::sptr file_source_;
gr::blocks::deinterleave::sptr deint_;
gr::blocks::endian_swap::sptr endian_;
std::vector<gr::blocks::endian_swap::sptr> endian_vec_;
std::vector<gr::blocks::null_sink::sptr> null_sinks_;
unpack_spir_gss6450_samples_sptr unpack_spir_;
boost::shared_ptr<gr::block> valve_;
gr::blocks::file_sink::sptr sink_;
gr::blocks::throttle::sptr throttle_;
std::vector<unpack_spir_gss6450_samples_sptr> unpack_spir_vec_;
std::vector<boost::shared_ptr<gr::block>> valve_vec_;
std::vector<gr::blocks::file_sink::sptr> sink_vec_;
std::vector<gr::blocks::throttle::sptr> throttle_vec_;
gr::msg_queue::sptr queue_;
size_t item_size_;
};

2
src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc
View File

@@ -40,7 +40,7 @@ unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int a
unpack_spir_gss6450_samples::unpack_spir_gss6450_samples(unsigned int adc_nbit) : gr::sync_interpolator("unpack_spir_gss6450_samples",
gr::io_signature::make(1, 1, sizeof(int)),
gr::io_signature::make(1, 1, sizeof(int32_t)),
gr::io_signature::make(1, 1, sizeof(gr_complex)), 16 / adc_nbit)
{
adc_bits = adc_nbit;

6
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@@ -50,7 +50,7 @@
#include "GPS_L5.h"
#include "gps_l5_signal.h"
#include "gnss_sdr_create_directory.h"
#include <boost/lexical_cast.hpp>
#include <boost/filesystem/path.hpp>
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
#include <matio.h>
@@ -1091,7 +1091,7 @@ int32_t dll_pll_veml_tracking::save_matfile()
std::ifstream dump_file;
std::string dump_filename_ = d_dump_filename;
// add channel number to the filename
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
dump_filename_.append(std::to_string(d_channel));
// add extension
dump_filename_.append(".dat");
std::cout << "Generating .mat file for " << dump_filename_ << std::endl;
@@ -1335,7 +1335,7 @@ void dll_pll_veml_tracking::set_channel(uint32_t channel)
{
std::string dump_filename_ = d_dump_filename;
// add channel number to the filename
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
dump_filename_.append(std::to_string(d_channel));
// add extension
dump_filename_.append(".dat");

6
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc
View File

@@ -48,7 +48,7 @@
#include "GPS_L5.h"
#include "gps_l5_signal.h"
#include "gnss_sdr_create_directory.h"
#include <boost/lexical_cast.hpp>
#include <boost/filesystem/path.hpp>
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
#include <matio.h>
@@ -1011,7 +1011,7 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile()
std::ifstream dump_file;
std::string dump_filename_ = d_dump_filename;
// add channel number to the filename
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
dump_filename_.append(std::to_string(d_channel));
// add extension
dump_filename_.append(".dat");
std::cout << "Generating .mat file for " << dump_filename_ << std::endl;
@@ -1239,7 +1239,7 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel)
{
std::string dump_filename_ = d_dump_filename;
// add channel number to the filename
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
dump_filename_.append(std::to_string(d_channel));
// add extension
dump_filename_.append(".dat");