mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-17 15:24:56 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Carles Fernandez 2018-11-07 17:03:43 +01:00
commit 6d0ee84836
48 changed files with 3690 additions and 1073 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
docs/xml-schemas/gps_almanac_map.xsd
View File

@ -17,6 +17,7 @@
<xs:element type="xs:byte" name="i_satellite_PRN"/> <xs:element type="xs:byte" name="i_satellite_PRN"/>
<xs:element type="xs:float" name="d_Delta_i"/> <xs:element type="xs:float" name="d_Delta_i"/>
<xs:element type="xs:float" name="d_Toa"/> <xs:element type="xs:float" name="d_Toa"/>
<xs:element type="xs:byte" name="i_WNa"/>
<xs:element type="xs:float" name="d_M_0"/> <xs:element type="xs:float" name="d_M_0"/>
<xs:element type="xs:float" name="d_e_eccentricity"/> <xs:element type="xs:float" name="d_e_eccentricity"/>
<xs:element type="xs:float" name="d_sqrt_A"/> <xs:element type="xs:float" name="d_sqrt_A"/>
@ -24,6 +25,7 @@
<xs:element type="xs:float" name="d_OMEGA"/> <xs:element type="xs:float" name="d_OMEGA"/>
<xs:element type="xs:float" name="d_OMEGA_DOT"/> <xs:element type="xs:float" name="d_OMEGA_DOT"/>
<xs:element type="xs:byte" name="i_SV_health"/> <xs:element type="xs:byte" name="i_SV_health"/>
<xs:element type="xs:byte" name="i_AS_status"/>
<xs:element type="xs:float" name="d_A_f0"/> <xs:element type="xs:float" name="d_A_f0"/>
<xs:element type="xs:float" name="d_A_f1"/> <xs:element type="xs:float" name="d_A_f1"/>
</xs:sequence> </xs:sequence>

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

@ -48,6 +48,27 @@ namespace bc = boost::integer;
using google::LogMessage; 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, RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
std::string role, std::string role,
unsigned int in_streams, unsigned int in_streams,
@ -64,6 +85,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
pvt_output_parameters.dump = configuration->property(role + ".dump", false); pvt_output_parameters.dump = configuration->property(role + ".dump", false);
pvt_output_parameters.dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); pvt_output_parameters.dump_filename = configuration->property(role + ".dump_filename", default_dump_filename);
pvt_output_parameters.dump_mat = configuration->property(role + ".dump_mat", true);
// output rate // output rate
pvt_output_parameters.output_rate_ms = configuration->property(role + ".output_rate_ms", 500); pvt_output_parameters.output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
@ -178,24 +200,20 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
int glo_1G_count = configuration->property("Channels_1G.count", 0); int glo_1G_count = configuration->property("Channels_1G.count", 0);
int glo_2G_count = configuration->property("Channels_2G.count", 0); int glo_2G_count = configuration->property("Channels_2G.count", 0);
//unsigned int type_of_receiver = 0; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 1; // L1
// *******************WARNING!!!!!!!***********
// GPS L5 only configurable for single frequency, single system at the moment!!!!!!
if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 1;
if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 2; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 2;
if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 3; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 3; // L5
if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 4; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 4; // E1
if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 5; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 5; // E5a
if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 6; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 6;
if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 7; if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 7;
//if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) pvt_output_parameters.type_of_receiver = 8; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 8; // L1+L5
if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 9; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 9; // L1+E1
if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 10; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 10;
if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 11; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 11;
if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 12; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 12;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) pvt_output_parameters.type_of_receiver = 13; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 13; // L5+E5a
if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 14; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 14;
if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 15; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 15;
//if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) pvt_output_parameters.type_of_receiver = 16; //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) pvt_output_parameters.type_of_receiver = 16;
@ -214,11 +232,14 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0)) pvt_output_parameters.type_of_receiver = 29; if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0)) pvt_output_parameters.type_of_receiver = 29;
if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0)) pvt_output_parameters.type_of_receiver = 30; if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0)) pvt_output_parameters.type_of_receiver = 30;
if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0)) pvt_output_parameters.type_of_receiver = 31; if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0)) pvt_output_parameters.type_of_receiver = 31;
if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0)) pvt_output_parameters.type_of_receiver = 32; // L1+E1+L5+E5a
// RTKLIB PVT solver options // RTKLIB PVT solver options
// Settings 1 // Settings 1
int positioning_mode = -1; int positioning_mode = -1;
std::string default_pos_mode("Single"); std::string default_pos_mode("Single");
std::string positioning_mode_str = configuration->property(role + ".positioning_mode", default_pos_mode); /* (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */ std::string positioning_mode_str = configuration->property(role + ".positioning_mode", default_pos_mode); // (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h
if (positioning_mode_str.compare("Single") == 0) positioning_mode = PMODE_SINGLE; if (positioning_mode_str.compare("Single") == 0) positioning_mode = PMODE_SINGLE;
if (positioning_mode_str.compare("Static") == 0) positioning_mode = PMODE_STATIC; if (positioning_mode_str.compare("Static") == 0) positioning_mode = PMODE_STATIC;
if (positioning_mode_str.compare("Kinematic") == 0) positioning_mode = PMODE_KINEMA; if (positioning_mode_str.compare("Kinematic") == 0) positioning_mode = PMODE_KINEMA;
@ -515,6 +536,22 @@ RtklibPvt::~RtklibPvt()
rtkfree(&rtk); rtkfree(&rtk);
} }
std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris()
{
return pvt_->get_gps_ephemeris_map();
}
std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris()
{
return pvt_->get_galileo_ephemeris_map();
}
std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac()
{
return pvt_->get_gps_almanac_map();
}
std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac()
{
return pvt_->get_galileo_almanac_map();
}
void RtklibPvt::connect(gr::top_block_sptr top_block) void RtklibPvt::connect(gr::top_block_sptr top_block)
{ {

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

@ -63,6 +63,13 @@ public:
return "RTKLIB_PVT"; return "RTKLIB_PVT";
} }
void clear_ephemeris();
std::map<int, Gps_Ephemeris> get_gps_ephemeris();
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris();
std::map<int, Gps_Almanac> get_gps_almanac();
std::map<int, Galileo_Almanac> get_galileo_almanac();
void connect(gr::top_block_sptr top_block) override; void connect(gr::top_block_sptr top_block) override;
void disconnect(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_left_block() override;
@ -79,6 +86,13 @@ public:
return sizeof(gr_complex); 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);
private: private:
rtklib_pvt_cc_sptr pvt_; rtklib_pvt_cc_sptr pvt_;
rtk_t rtk; rtk_t rtk;

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

File diff suppressed because it is too large Load Diff

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

@ -31,7 +31,7 @@
#ifndef GNSS_SDR_RTKLIB_PVT_CC_H #ifndef GNSS_SDR_RTKLIB_PVT_CC_H
#define GNSS_SDR_RTKLIB_PVT_CC_H #define GNSS_SDR_RTKLIB_PVT_CC_H
#include "gps_ephemeris.h"
#include "nmea_printer.h" #include "nmea_printer.h"
#include "kml_printer.h" #include "kml_printer.h"
#include "gpx_printer.h" #include "gpx_printer.h"
@ -60,7 +60,7 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t n_channels,
rtk_t& rtk); 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 class rtklib_pvt_cc : public gr::sync_block
{ {
@ -72,6 +72,7 @@ private:
void msg_handler_telemetry(pmt::pmt_t msg); void msg_handler_telemetry(pmt::pmt_t msg);
bool d_dump; bool d_dump;
bool d_dump_mat;
bool b_rinex_output_enabled; bool b_rinex_output_enabled;
bool b_rinex_header_written; bool b_rinex_header_written;
bool b_rinex_header_updated; bool b_rinex_header_updated;
@ -80,6 +81,7 @@ private:
int32_t d_rinexnav_rate_ms; int32_t d_rinexnav_rate_ms;
bool b_rtcm_writing_started; bool b_rtcm_writing_started;
bool b_rtcm_enabled;
int32_t d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris int32_t d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris
int32_t d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits) int32_t d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits)
int32_t d_rtcm_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits) int32_t d_rtcm_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits)
@ -140,11 +142,33 @@ public:
rtk_t& rtk); 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();
std::map<int, Gps_Almanac> get_gps_almanac_map();
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map();
std::map<int, Galileo_Almanac> get_galileo_almanac_map();
/*!
* \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 ~rtklib_pvt_cc(); //!< Default destructor

9
src/algorithms/PVT/libs/CMakeLists.txt
View File

@ -59,6 +59,7 @@ include_directories(
${ARMADILLO_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS}
${MATIO_INCLUDE_DIRS}
) )
list(SORT PVT_LIB_HEADERS) list(SORT PVT_LIB_HEADERS)
@ -66,7 +67,12 @@ list(SORT PVT_LIB_SOURCES)
add_library(pvt_lib ${PVT_LIB_SOURCES} ${PVT_LIB_HEADERS}) add_library(pvt_lib ${PVT_LIB_SOURCES} ${PVT_LIB_HEADERS})
source_group(Headers FILES ${PVT_LIB_HEADERS}) source_group(Headers FILES ${PVT_LIB_HEADERS})
add_dependencies(pvt_lib rtklib_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE})
if(MATIO_FOUND)
add_dependencies(pvt_lib glog-${glog_RELEASE} armadillo-${armadillo_RELEASE})
else(MATIO_FOUND)
add_dependencies(pvt_lib glog-${glog_RELEASE} armadillo-${armadillo_RELEASE} matio-${GNSSSDR_MATIO_LOCAL_VERSION})
endif(MATIO_FOUND)
target_link_libraries( target_link_libraries(
pvt_lib pvt_lib
@ -77,4 +83,5 @@ target_link_libraries(
${ARMADILLO_LIBRARIES} ${ARMADILLO_LIBRARIES}
${BLAS} ${BLAS}
${LAPACK} ${LAPACK}
${MATIO_LIBRARIES}
) )

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

@ -34,6 +34,7 @@
*/ */
#include "nmea_printer.h" #include "nmea_printer.h"
#include "rtklib_solution.h"
#include <boost/date_time/posix_time/posix_time.hpp> #include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem/operations.hpp> // for create_directories, exists #include <boost/filesystem/operations.hpp> // for create_directories, exists
#include <boost/filesystem/path.hpp> // for path, operator<< #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() std::string Nmea_Printer::get_GPRMC()
{ {
// Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10 // 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; std::stringstream sentence_str;
unsigned char buff[1024] = {0};
// GPRMC (RMC-Recommended,Minimum Specific GNSS Data) outnmea_rmc(buff, &d_PVT_data->pvt_sol);
std::string sentence_header; sentence_str << buff;
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";
return sentence_str.str(); 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 // $GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
// GSA-GNSS DOP and Active Satellites // 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::stringstream sentence_str;
std::string sentence_header; unsigned char buff[1024] = {0};
sentence_header = "$GPGSA,"; outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
sentence_str << sentence_header; sentence_str << buff;
// 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";
return sentence_str.str(); return sentence_str.str();
} }
@ -560,199 +400,22 @@ std::string Nmea_Printer::get_GPGSA()
std::string Nmea_Printer::get_GPGSV() std::string Nmea_Printer::get_GPGSV()
{ {
// GSV-GNSS Satellites in View // 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 // $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() 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; std::stringstream sentence_str;
unsigned char buff[1024] = {0};
// GPGGA (Global Positioning System Fixed Data) outnmea_gga(buff, &d_PVT_data->pvt_sol);
std::string sentence_header; sentence_str << buff;
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";
return sentence_str.str(); 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 // $GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
} }

1
src/algorithms/PVT/libs/pvt_conf.cc
View File

@ -41,6 +41,7 @@ Pvt_Conf::Pvt_Conf()
rinexnav_rate_ms = 0; rinexnav_rate_ms = 0;
dump = false; dump = false;
dump_mat = true;
flag_nmea_tty_port = false; flag_nmea_tty_port = false;

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

@ -49,6 +49,7 @@ public:
std::map<int, int> rtcm_msg_rate_ms; std::map<int, int> rtcm_msg_rate_ms;
bool dump; bool dump;
bool dump_mat;
std::string dump_filename; std::string dump_filename;
bool flag_nmea_tty_port; bool flag_nmea_tty_port;

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

@ -43,6 +43,8 @@ Pvt_Solution::Pvt_Solution()
d_latitude_d = 0.0; d_latitude_d = 0.0;
d_longitude_d = 0.0; d_longitude_d = 0.0;
d_height_m = 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_latitude_d = 0.0;
d_avg_longitude_d = 0.0; d_avg_longitude_d = 0.0;
d_avg_height_m = 0.0; d_avg_height_m = 0.0;
@ -126,6 +128,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
d_latitude_d = phi * 180.0 / GPS_PI; d_latitude_d = phi * 180.0 / GPS_PI;
d_longitude_d = lambda * 180.0 / GPS_PI; d_longitude_d = lambda * 180.0 / GPS_PI;
d_height_m = h; d_height_m = h;
//todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
return 0; return 0;
} }
@ -516,6 +519,25 @@ double Pvt_Solution::get_height() const
return d_height_m; return d_height_m;
} }
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 double Pvt_Solution::get_avg_latitude() const
{ {

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

@ -52,6 +52,8 @@ private:
double d_latitude_d; // RX position Latitude WGS84 [deg] double d_latitude_d; // RX position Latitude WGS84 [deg]
double d_longitude_d; // RX position Longitude WGS84 [deg] double d_longitude_d; // RX position Longitude WGS84 [deg]
double d_height_m; // RX position height WGS84 [m] 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_latitude_d; // Averaged latitude in degrees
double d_avg_longitude_d; // Averaged longitude in degrees double d_avg_longitude_d; // Averaged longitude in degrees
@ -86,6 +88,12 @@ public:
double get_longitude() const; //!< Get RX position Longitude WGS84 [deg] double get_longitude() const; //!< Get RX position Longitude WGS84 [deg]
double get_height() const; //!< Get RX position height WGS84 [m] 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_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg]
double get_avg_longitude() const; //!< Get RX position averaged Longitude 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] double get_avg_height() const; //!< Get RX position averaged height WGS84 [m]

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

File diff suppressed because it is too large Load Diff

38
src/algorithms/PVT/libs/rinex_printer.h
View File

@ -113,6 +113,11 @@ public:
*/ */
void rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model); void rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model);
/*!
* \brief Generates the Mixed (GPS CNAV/Galileo) Navigation Data header
*/
void rinex_nav_header(std::fstream& out, const Gps_CNAV_Iono& iono, const Gps_CNAV_Utc_Model& utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model);
/*! /*!
* \brief Generates the GLONASS L1, L2 C/A Navigation Data header * \brief Generates the GLONASS L1, L2 C/A Navigation Data header
*/ */
@ -141,12 +146,12 @@ public:
/*! /*!
* \brief Generates the GPS L2 Observation data header * \brief Generates the GPS L2 Observation data header
*/ */
void rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const double d_TOW_first_observation); void rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const double d_TOW_first_observation, const std::string gps_bands = "2S");
/*! /*!
* \brief Generates the dual frequency GPS L1 & L2 Observation data header * \brief Generates the dual frequency GPS L1 & L2/L5 Observation data header
*/ */
void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph, const Gps_CNAV_Ephemeris& eph_cnav, const double d_TOW_first_observation); void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph, const Gps_CNAV_Ephemeris& eph_cnav, const double d_TOW_first_observation, const std::string gps_bands = "1C 2S");
/*! /*!
* \brief Generates the Galileo Observation data header. Example: bands("1B"), bands("1B 5X"), bands("5X"), ... Default: "1B". * \brief Generates the Galileo Observation data header. Example: bands("1B"), bands("1B 5X"), bands("5X"), ... Default: "1B".
@ -158,6 +163,16 @@ public:
*/ */
void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B"); void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B");
/*!
* \brief Generates the Mixed (GPS/Galileo) Observation data header. Example: galileo_bands("1B"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/
void rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& eph_cnav, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation, const std::string gps_bands = "1C 2S", const std::string galileo_bands = "1B");
/*!
* \brief Generates the Mixed (GPS/Galileo) Observation data header. Example: galileo_bands("1B"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/
void rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris& eph_cnav, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation, const std::string gps_bands = "2S", const std::string galileo_bands = "1B");
/*! /*!
* \brief Generates the GLONASS GNAV Observation data header. Example: bands("1C"), bands("1C 2C"), bands("2C"), ... Default: "1C". * \brief Generates the GLONASS GNAV Observation data header. Example: bands("1C"), bands("1C 2C"), bands("2C"), ... Default: "1C".
*/ */
@ -239,6 +254,11 @@ public:
*/ */
void log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps_Ephemeris>& gps_eph_map, const std::map<int32_t, Galileo_Ephemeris>& galileo_eph_map); void log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps_Ephemeris>& gps_eph_map, const std::map<int32_t, Galileo_Ephemeris>& galileo_eph_map);
/*!
* \brief Writes data from the Mixed (GPS/Galileo) navigation message into the RINEX file
*/
void log_rinex_nav(std::fstream& out, const std::map<int32_t, Gps_CNAV_Ephemeris>& gps_cnav_eph_map, const std::map<int32_t, Galileo_Ephemeris>& galileo_eph_map);
/*! /*!
* \brief Writes data from the GLONASS GNAV navigation message into the RINEX file * \brief Writes data from the GLONASS GNAV navigation message into the RINEX file
*/ */
@ -284,6 +304,16 @@ public:
*/ */
void log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double gps_obs_time, const std::map<int32_t, Gnss_Synchro>& observables); void log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double gps_obs_time, const std::map<int32_t, Gnss_Synchro>& observables);
/*!
* \brief Writes Mixed GPS / Galileo observables into the RINEX file
*/
void log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const Galileo_Ephemeris& galileo_eph, double gps_obs_time, const std::map<int32_t, Gnss_Synchro>& observables);
/*!
* \brief Writes Mixed GPS / Galileo observables into the RINEX file
*/
void log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& gps_cnav_eph, const Galileo_Ephemeris& galileo_eph, double gps_obs_time, const std::map<int32_t, Gnss_Synchro>& observables);
/*! /*!
* \brief Writes GLONASS GNAV observables into the RINEX file. Example: glonass_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". * \brief Writes GLONASS GNAV observables into the RINEX file. Example: glonass_bands("1C"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B".
*/ */
@ -320,6 +350,8 @@ public:
void update_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model); void update_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model);
void update_nav_header(std::fstream& out, const Gps_CNAV_Utc_Model& utc_model, const Gps_CNAV_Iono& iono, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model);
void update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& utc_model); void update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& utc_model);
void update_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac); void update_nav_header(std::fstream& out, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);

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

@ -53,26 +53,33 @@
#include "rtklib_solver.h" #include "rtklib_solver.h"
#include "rtklib_conversions.h" #include "rtklib_conversions.h"
#include "rtklib_solution.h"
#include "GPS_L1_CA.h" #include "GPS_L1_CA.h"
#include "Galileo_E1.h" #include "Galileo_E1.h"
#include "GLONASS_L1_L2_CA.h" #include "GLONASS_L1_L2_CA.h"
#include <matio.h>
#include <glog/logging.h> #include <glog/logging.h>
using google::LogMessage; using google::LogMessage;
rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk) rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, rtk_t &rtk)
{ {
// init empty ephemeris for all the available GNSS channels // init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels; d_nchannels = nchannels;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file; d_flag_dump_enabled = flag_dump_to_file;
d_flag_dump_mat_enabled = flag_dump_to_mat;
count_valid_position = 0; count_valid_position = 0;
this->set_averaging_flag(false); this->set_averaging_flag(false);
rtk_ = rtk; rtk_ = rtk;
for (unsigned int i = 0; i < 4; i++) dop_[i] = 0.0; 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}; 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 ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
{ {
@ -92,6 +99,301 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
} }
} }
bool rtklib_solver::save_matfile()
{
// READ DUMP FILE
std::string dump_filename = d_dump_filename;
std::ifstream::pos_type size;
int32_t number_of_double_vars = 21;
int32_t number_of_uint32_vars = 2;
int32_t number_of_uint8_vars = 3;
int32_t number_of_float_vars = 2;
int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
sizeof(uint32_t) * number_of_uint32_vars +
sizeof(uint8_t) * number_of_uint8_vars +
sizeof(float) * number_of_float_vars;
std::ifstream dump_file;
std::cout << "Generating .mat file for " << dump_filename << std::endl;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
{
dump_file.open(dump_filename.c_str(), std::ios::binary | std::ios::ate);
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem opening dump file:" << e.what() << std::endl;
return false;
}
// count number of epochs and rewind
int64_t num_epoch = 0LL;
if (dump_file.is_open())
{
size = dump_file.tellg();
num_epoch = static_cast<int64_t>(size) / static_cast<int64_t>(epoch_size_bytes);
dump_file.seekg(0, std::ios::beg);
}
else
{
return false;
}
uint32_t *TOW_at_current_symbol_ms = new uint32_t[num_epoch];
uint32_t *week = new uint32_t[num_epoch];
double *RX_time = new double[num_epoch];
double *user_clk_offset = new double[num_epoch];
double *pos_x = new double[num_epoch];
double *pos_y = new double[num_epoch];
double *pos_z = new double[num_epoch];
double *vel_x = new double[num_epoch];
double *vel_y = new double[num_epoch];
double *vel_z = new double[num_epoch];
double *cov_xx = new double[num_epoch];
double *cov_yy = new double[num_epoch];
double *cov_zz = new double[num_epoch];
double *cov_xy = new double[num_epoch];
double *cov_yz = new double[num_epoch];
double *cov_zx = new double[num_epoch];
double *latitude = new double[num_epoch];
double *longitude = new double[num_epoch];
double *height = new double[num_epoch];
uint8_t *valid_sats = new uint8_t[num_epoch];
uint8_t *solution_status = new uint8_t[num_epoch];
uint8_t *solution_type = new uint8_t[num_epoch];
float *AR_ratio_factor = new float[num_epoch];
float *AR_ratio_threshold = new float[num_epoch];
double *gdop = new double[num_epoch];
double *pdop = new double[num_epoch];
double *hdop = new double[num_epoch];
double *vdop = new double[num_epoch];
try
{
if (dump_file.is_open())
{
for (int64_t i = 0; i < num_epoch; i++)
{
dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_ms[i]), sizeof(uint32_t));
dump_file.read(reinterpret_cast<char *>(&week[i]), sizeof(uint32_t));
dump_file.read(reinterpret_cast<char *>(&RX_time[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&user_clk_offset[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pos_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_x[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_y[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vel_z[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_xx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_yy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_zz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_xy[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_yz[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&cov_zx[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&latitude[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&longitude[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&height[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&valid_sats[i]), sizeof(uint8_t));
dump_file.read(reinterpret_cast<char *>(&solution_status[i]), sizeof(uint8_t));
dump_file.read(reinterpret_cast<char *>(&solution_type[i]), sizeof(uint8_t));
dump_file.read(reinterpret_cast<char *>(&AR_ratio_factor[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&AR_ratio_threshold[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&gdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&pdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&hdop[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&vdop[i]), sizeof(double));
}
}
dump_file.close();
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] TOW_at_current_symbol_ms;
delete[] week;
delete[] RX_time;
delete[] user_clk_offset;
delete[] pos_x;
delete[] pos_y;
delete[] pos_z;
delete[] vel_x;
delete[] vel_y;
delete[] vel_z;
delete[] cov_xx;
delete[] cov_yy;
delete[] cov_zz;
delete[] cov_xy;
delete[] cov_yz;
delete[] cov_zx;
delete[] latitude;
delete[] longitude;
delete[] height;
delete[] valid_sats;
delete[] solution_status;
delete[] solution_type;
delete[] AR_ratio_factor;
delete[] AR_ratio_threshold;
delete[] gdop;
delete[] pdop;
delete[] hdop;
delete[] vdop;
return false;
}
// WRITE MAT FILE
mat_t *matfp;
matvar_t *matvar;
std::string filename = dump_filename;
filename.erase(filename.length() - 4, 4);
filename.append(".mat");
matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
if (reinterpret_cast<int64_t *>(matfp) != NULL)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims, TOW_at_current_symbol_ms, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims, week, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, user_clk_offset, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_x, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_y, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pos_z, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_x, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_y, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vel_z, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_xx, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_yy, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_zz, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_xy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_xy, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_yz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_yz, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("cov_zx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, cov_zx, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, latitude, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, longitude, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, height, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims, valid_sats, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("solution_status", MAT_C_UINT8, MAT_T_UINT8, 2, dims, solution_status, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("solution_type", MAT_C_UINT8, MAT_T_UINT8, 2, dims, solution_type, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("AR_ratio_factor", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, AR_ratio_factor, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("AR_ratio_threshold", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, AR_ratio_threshold, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, gdop, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, pdop, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, hdop, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, vdop, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] TOW_at_current_symbol_ms;
delete[] week;
delete[] RX_time;
delete[] user_clk_offset;
delete[] pos_x;
delete[] pos_y;
delete[] pos_z;
delete[] vel_x;
delete[] vel_y;
delete[] vel_z;
delete[] cov_xx;
delete[] cov_yy;
delete[] cov_zz;
delete[] cov_xy;
delete[] cov_yz;
delete[] cov_zx;
delete[] latitude;
delete[] longitude;
delete[] height;
delete[] valid_sats;
delete[] solution_status;
delete[] solution_type;
delete[] AR_ratio_factor;
delete[] AR_ratio_threshold;
delete[] gdop;
delete[] pdop;
delete[] hdop;
delete[] vdop;
return true;
}
rtklib_solver::~rtklib_solver() rtklib_solver::~rtklib_solver()
{ {
@ -106,6 +408,10 @@ rtklib_solver::~rtklib_solver()
LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what(); LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what();
} }
} }
if (d_flag_dump_mat_enabled)
{
save_matfile();
}
} }
@ -493,7 +799,11 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
unsigned int used_sats = 0; unsigned int used_sats = 0;
for (unsigned int i = 0; i < MAXSAT; i++) for (unsigned int i = 0; i < MAXSAT; i++)
{ {
if (rtk_.ssat[i].vs == 1) used_sats++; if (rtk_.ssat[i].vs == 1)
{
pvt_ssat[i] = rtk_.ssat[i];
used_sats++;
}
} }
std::vector<double> azel; std::vector<double> azel;
@ -508,8 +818,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
index_aux++; 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); this->set_valid_position(true);
arma::vec rx_position_and_time(4); arma::vec rx_position_and_time(4);
rx_position_and_time(0) = pvt_sol.rr[0]; // [m] rx_position_and_time(0) = pvt_sol.rr[0]; // [m]
@ -526,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] 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 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: //observable fix:
//double offset_s = this->get_time_offset_s(); //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] //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]

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

@ -77,14 +77,17 @@ private:
rtk_t rtk_; rtk_t rtk_;
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;
bool save_matfile();
bool d_flag_dump_enabled; bool d_flag_dump_enabled;
bool d_flag_dump_mat_enabled;
int d_nchannels; // Number of available channels for positioning int d_nchannels; // Number of available channels for positioning
double dop_[4]; double dop_[4];
public: public:
sol_t pvt_sol; sol_t pvt_sol;
rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk); 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(); ~rtklib_solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging); bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging);

2
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc
View File

@ -53,7 +53,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
Acq_Conf acq_parameters; Acq_Conf acq_parameters;
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;

2
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc
View File

@ -53,7 +53,7 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga(
pcpsconf_fpga_t acq_parameters; pcpsconf_fpga_t acq_parameters;
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;

2
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc
View File

@ -52,7 +52,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
Acq_Conf acq_parameters = Acq_Conf(); Acq_Conf acq_parameters = Acq_Conf();
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "../data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
DLOG(INFO) << "Role " << role; DLOG(INFO) << "Role " << role;

2
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc
View File

@ -52,7 +52,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf
pcpsconf_fpga_t acq_parameters; pcpsconf_fpga_t acq_parameters;
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "../data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
DLOG(INFO) << "Role " << role; DLOG(INFO) << "Role " << role;

2
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc
View File

@ -56,7 +56,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
Acq_Conf acq_parameters = Acq_Conf(); Acq_Conf acq_parameters = Acq_Conf();
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;

2
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc
View File

@ -51,7 +51,7 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler(
unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
{ {
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
Acq_Conf acq_parameters = Acq_Conf(); Acq_Conf acq_parameters = Acq_Conf();

2
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc
View File

@ -54,7 +54,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
Acq_Conf acq_parameters = Acq_Conf(); Acq_Conf acq_parameters = Acq_Conf();
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
LOG(INFO) << "role " << role; LOG(INFO) << "role " << role;

2
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc
View File

@ -55,7 +55,7 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga(
pcpsconf_fpga_t acq_parameters; pcpsconf_fpga_t acq_parameters;
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
LOG(INFO) << "role " << role; LOG(INFO) << "role " << role;

2
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc
View File

@ -54,7 +54,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
Acq_Conf acq_parameters = Acq_Conf(); Acq_Conf acq_parameters = Acq_Conf();
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
LOG(INFO) << "role " << role; LOG(INFO) << "role " << role;

2
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc
View File

@ -55,7 +55,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga(
pcpsconf_fpga_t acq_parameters; pcpsconf_fpga_t acq_parameters;
configuration_ = configuration; configuration_ = configuration;
std::string default_item_type = "gr_complex"; std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./data/acquisition.dat"; std::string default_dump_filename = "./acquisition.mat";
LOG(INFO) << "role " << role; LOG(INFO) << "role " << role;

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

@ -59,7 +59,6 @@ bool ChannelFsm::Event_stop_channel()
switch (d_state) switch (d_state)
{ {
case 0: //already in stanby case 0: //already in stanby
return true;
break; break;
case 1: //acquisition case 1: //acquisition
d_state = 0; d_state = 0;
@ -70,8 +69,9 @@ bool ChannelFsm::Event_stop_channel()
stop_tracking(); stop_tracking();
break; break;
default: default:
return true; break;
} }
return true;
} }
bool ChannelFsm::Event_start_acquisition() 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_sc.cc
conjugate_ic.cc conjugate_ic.cc
gnss_sdr_create_directory.cc gnss_sdr_create_directory.cc
geofunctions.cc
) )
set(GNSS_SPLIBS_HEADERS set(GNSS_SPLIBS_HEADERS
@ -63,6 +64,7 @@ set(GNSS_SPLIBS_HEADERS
conjugate_ic.h conjugate_ic.h
gnss_sdr_create_directory.h gnss_sdr_create_directory.h
gnss_circular_deque.h gnss_circular_deque.h
geofunctions.h
) )
@ -101,6 +103,7 @@ include_directories(
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS}
${ARMADILLO_INCLUDE_DIRS}
${GNURADIO_RUNTIME_INCLUDE_DIRS} ${GNURADIO_RUNTIME_INCLUDE_DIRS}
${GNURADIO_BLOCKS_INCLUDE_DIRS} ${GNURADIO_BLOCKS_INCLUDE_DIRS}
${VOLK_INCLUDE_DIRS} ${VOLK_INCLUDE_DIRS}
@ -128,6 +131,7 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
${VOLK_LIBRARIES} ${ORC_LIBRARIES} ${VOLK_LIBRARIES} ${ORC_LIBRARIES}
${VOLK_GNSSSDR_LIBRARIES} ${ORC_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES} ${ORC_LIBRARIES}
${GFlags_LIBS} ${GFlags_LIBS}
${ARMADILLO_LIBRARIES}
${GNURADIO_BLOCKS_LIBRARIES} ${GNURADIO_BLOCKS_LIBRARIES}
${GNURADIO_FFT_LIBRARIES} ${GNURADIO_FFT_LIBRARIES}
${GNURADIO_FILTER_LIBRARIES} ${GNURADIO_FILTER_LIBRARIES}
@ -136,7 +140,9 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
) )
if(NOT VOLK_GNSSSDR_FOUND) 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) endif(NOT VOLK_GNSSSDR_FOUND)
if(${GFLAGS_GREATER_20}) if(${GFLAGS_GREATER_20})

0
src/tests/system-tests/libs/geofunctions.cc → src/algorithms/libs/geofunctions.cc
View File

0
src/tests/system-tests/libs/geofunctions.h → src/algorithms/libs/geofunctions.h
View File

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

@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
return rtklib_sat; 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.d_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.d_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.d_WNa;
rtklib_alm.toa = gpst2time(gal_alm.d_WNa, gal_alm.d_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.d_Toa;
return rtklib_alm;
}

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

@ -38,10 +38,16 @@
#include "gps_cnav_ephemeris.h" #include "gps_cnav_ephemeris.h"
#include "glonass_gnav_ephemeris.h" #include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_utc_model.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 Galileo_Ephemeris& gal_eph);
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph); eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph);
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_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 * \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart
* \param glonass_gnav_eph GLONASS GNAV Ephemeris structure * \param glonass_gnav_eph GLONASS GNAV Ephemeris structure

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

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

16
src/core/interfaces/pvt_interface.h
View File

@ -38,6 +38,10 @@
#define GNSS_SDR_PVT_INTERFACE_H_ #define GNSS_SDR_PVT_INTERFACE_H_
#include "gnss_block_interface.h" #include "gnss_block_interface.h"
#include "gps_ephemeris.h"
#include "galileo_ephemeris.h"
#include "gps_almanac.h"
#include "galileo_almanac.h"
/*! /*!
* \brief This class represents an interface to a PVT block. * \brief This class represents an interface to a PVT block.
@ -52,6 +56,18 @@ class PvtInterface : public GNSSBlockInterface
{ {
public: public:
virtual void reset() = 0; virtual void reset() = 0;
virtual void clear_ephemeris() = 0;
virtual std::map<int, Gps_Ephemeris> get_gps_ephemeris() = 0;
virtual std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() = 0;
virtual std::map<int, Gps_Almanac> get_gps_almanac() = 0;
virtual std::map<int, Galileo_Almanac> get_galileo_almanac() = 0;
virtual 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) = 0;
}; };
#endif /* GNSS_SDR_PVT_INTERFACE_H_ */ #endif /* GNSS_SDR_PVT_INTERFACE_H_ */

2
src/core/libs/gnss_sdr_supl_client.cc
View File

@ -268,7 +268,7 @@ void gnss_sdr_supl_client::read_supl_data()
gps_almanac_iterator->second.d_sqrt_A = static_cast<double>(a->A_sqrt) * pow(2.0, -11); gps_almanac_iterator->second.d_sqrt_A = static_cast<double>(a->A_sqrt) * pow(2.0, -11);
gps_almanac_iterator->second.d_OMEGA_DOT = static_cast<double>(a->OMEGA_dot) * pow(2.0, -38); gps_almanac_iterator->second.d_OMEGA_DOT = static_cast<double>(a->OMEGA_dot) * pow(2.0, -38);
gps_almanac_iterator->second.d_Toa = static_cast<double>(a->toa) * pow(2.0, 12); gps_almanac_iterator->second.d_Toa = static_cast<double>(a->toa) * pow(2.0, 12);
gps_almanac_iterator->second.d_e_eccentricity = static_cast<double>(a->toa) * pow(2.0, -21); gps_almanac_iterator->second.d_e_eccentricity = static_cast<double>(a->e) * pow(2.0, -21);
gps_almanac_iterator->second.d_M_0 = static_cast<double>(a->M0) * pow(2.0, -23); gps_almanac_iterator->second.d_M_0 = static_cast<double>(a->M0) * pow(2.0, -23);
} }
} }

170
src/core/receiver/control_thread.cc
View File

@ -35,6 +35,7 @@
#include "control_thread.h" #include "control_thread.h"
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "concurrent_map.h" #include "concurrent_map.h"
#include "pvt_interface.h"
#include "control_message_factory.h" #include "control_message_factory.h"
#include "file_configuration.h" #include "file_configuration.h"
#include "gnss_flowgraph.h" #include "gnss_flowgraph.h"
@ -49,6 +50,10 @@
#include "gps_almanac.h" #include "gps_almanac.h"
#include "glonass_gnav_ephemeris.h" #include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_utc_model.h" #include "glonass_gnav_utc_model.h"
#include "geofunctions.h"
#include "rtklib_rtkcmn.h"
#include "rtklib_conversions.h"
#include "rtklib_ephemeris.h"
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <boost/chrono.hpp> #include <boost/chrono.hpp>
#include <glog/logging.h> #include <glog/logging.h>
@ -156,6 +161,7 @@ int ControlThread::run()
sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this); sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
//start the telecommand listener thread //start the telecommand listener thread
cmd_interface_.set_pvt(flowgraph_->get_pvt());
cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this); cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);
@ -693,6 +699,10 @@ void ControlThread::process_control_messages()
} }
else else
{ {
if (control_messages_->at(i)->who == 300) //some TC commands require also actions from controlthread
{
apply_action(control_messages_->at(i)->what);
}
flowgraph_->apply_action(control_messages_->at(i)->who, control_messages_->at(i)->what); flowgraph_->apply_action(control_messages_->at(i)->who, control_messages_->at(i)->what);
} }
processed_control_messages_++; processed_control_messages_++;
@ -704,25 +714,179 @@ void ControlThread::process_control_messages()
void ControlThread::apply_action(unsigned int what) void ControlThread::apply_action(unsigned int what)
{ {
std::shared_ptr<PvtInterface> pvt_ptr;
std::vector<std::pair<int, Gnss_Satellite>> visible_satellites;
switch (what) switch (what)
{ {
case 0: case 0:
DLOG(INFO) << "Received action STOP"; LOG(INFO) << "Received action STOP";
stop_ = true; stop_ = true;
applied_actions_++; applied_actions_++;
break; break;
case 1: case 1:
DLOG(INFO) << "Received action RESTART"; LOG(INFO) << "Received action RESTART";
stop_ = true; stop_ = true;
restart_ = true; restart_ = true;
applied_actions_++; applied_actions_++;
break; break;
case 11:
LOG(INFO) << "Receiver action COLDSTART";
//delete all ephemeris and almanac information from maps (also the PVT map queue)
pvt_ptr = flowgraph_->get_pvt();
pvt_ptr->clear_ephemeris();
//todo: reorder the satellite queues to the receiver default startup order.
//This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
break;
case 12:
LOG(INFO) << "Receiver action HOTSTART";
visible_satellites = get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
//reorder the satellite queue to acquire first those visible satellites
flowgraph_->priorize_satellites(visible_satellites);
//start again the satellite acquisitions (done in chained applyaction to flowgraph)
break;
case 13:
LOG(INFO) << "Receiver action WARMSTART";
//delete all ephemeris and almanac information from maps (also the PVT map queue)
pvt_ptr = flowgraph_->get_pvt();
pvt_ptr->clear_ephemeris();
//load the ephemeris and the almanac from XML files (receiver assistance)
read_assistance_from_XML();
//call here the function that computes the set of visible satellites and its elevation
//for the date and time specified by the warmstart command and the assisted position
get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
//reorder the satellite queue to acquire first those visible satellites
flowgraph_->priorize_satellites(visible_satellites);
//start again the satellite acquisitions (done in chained applyaction to flowgraph)
break;
default: default:
DLOG(INFO) << "Unrecognized action."; LOG(INFO) << "Unrecognized action.";
break; break;
} }
} }
std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, arma::vec LLH)
{
//1. Compute rx ECEF position from LLH WGS84
arma::vec LLH_rad = arma::vec{degtorad(LLH(0)), degtorad(LLH(1)), LLH(2)};
arma::mat C_tmp = arma::zeros(3, 3);
arma::vec r_eb_e = arma::zeros(3, 1);
arma::vec v_eb_e = arma::zeros(3, 1);
Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp);
//2. Compute rx GPS time from UTC time
gtime_t utc_gtime;
utc_gtime.time = rx_utc_time;
utc_gtime.sec = 0;
gtime_t gps_gtime = utc2gpst(utc_gtime);
//2. loop throught all the available ephemeris or almanac and compute satellite positions and elevations
// store visible satellites in a vector of pairs <int,Gnss_Satellite> to associate an elevation to the each satellite
std::vector<std::pair<int, Gnss_Satellite>> available_satellites;
std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
struct tm tstruct;
char buf[80];
tstruct = *gmtime(&rx_utc_time);
strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct);
std::string str_time = std::string(buf);
std::cout << "Get visible satellites at " << str_time
<< " UTC, assuming RX position " << LLH(0) << " [deg], " << LLH(1) << " [deg], " << LLH(2) << " [m]" << std::endl;
std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris();
for (std::map<int, Gps_Ephemeris>::iterator it = gps_eph_map.begin(); it != gps_eph_map.end(); ++it)
{
eph_t rtklib_eph = eph_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
&sat_pos_variance_m2);
double Az, El, dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
//push sat
if (El > 0)
{
std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
}
}
std::map<int, Galileo_Ephemeris> gal_eph_map = pvt_ptr->get_galileo_ephemeris();
for (std::map<int, Galileo_Ephemeris>::iterator it = gal_eph_map.begin(); it != gal_eph_map.end(); ++it)
{
eph_t rtklib_eph = eph_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
&sat_pos_variance_m2);
double Az, El, dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
//push sat
if (El > 0)
{
std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
}
}
std::map<int, Gps_Almanac> gps_alm_map = pvt_ptr->get_gps_almanac();
for (std::map<int, Gps_Almanac>::iterator it = gps_alm_map.begin(); it != gps_alm_map.end(); ++it)
{
alm_t rtklib_alm = alm_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
double Az, El, dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
//push sat
if (El > 0)
{
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
}
}
std::map<int, Galileo_Almanac> gal_alm_map = pvt_ptr->get_galileo_almanac();
for (std::map<int, Galileo_Almanac>::iterator it = gal_alm_map.begin(); it != gal_alm_map.end(); ++it)
{
alm_t rtklib_alm = alm_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
double Az, El, dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
std::cout << "Using Galileo Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
}
}
//sort the visible satellites in ascending order of elevation
std::sort(available_satellites.begin(), available_satellites.end(), [](const std::pair<int, Gnss_Satellite> &a, const std::pair<int, Gnss_Satellite> &b) { // use lambda. Cleaner and easier to read
return a.first < b.first;
});
//std::reverse(available_satellites.begin(), available_satellites.end());
return available_satellites;
}
void ControlThread::gps_acq_assist_data_collector() void ControlThread::gps_acq_assist_data_collector()
{ {

13
src/core/receiver/control_thread.h
View File

@ -35,16 +35,17 @@
#ifndef GNSS_SDR_CONTROL_THREAD_H_ #ifndef GNSS_SDR_CONTROL_THREAD_H_
#define GNSS_SDR_CONTROL_THREAD_H_ #define GNSS_SDR_CONTROL_THREAD_H_
#include "gnss_satellite.h"
#include "control_message_factory.h" #include "control_message_factory.h"
#include "gnss_sdr_supl_client.h" #include "gnss_sdr_supl_client.h"
#include "tcp_cmd_interface.h" #include "tcp_cmd_interface.h"
#include "gnss_flowgraph.h"
#include "configuration_interface.h"
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
#include <memory> #include <memory>
#include <vector> #include <vector>
#include <armadillo>
class GNSSFlowgraph;
class ConfigurationInterface;
/*! /*!
@ -143,6 +144,12 @@ private:
*/ */
void gps_acq_assist_data_collector(); void gps_acq_assist_data_collector();
/*
* Compute elevations for the specified time and position for all the available satellites in ephemeris and almanac queues
* returns a vector filled with the available satellites ordered from high elevation to low elevation angle.
*/
std::vector<std::pair<int, Gnss_Satellite>> get_visible_sats(time_t rx_utc_time, arma::vec LLH);
/* /*
* Read initial GNSS assistance from SUPL server or local XML files * Read initial GNSS assistance from SUPL server or local XML files
*/ */

74
src/core/receiver/gnss_flowgraph.cc
View File

@ -1103,11 +1103,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
acq_channels_count_ = 0; //all channels are in stanby now acq_channels_count_ = 0; //all channels are in stanby now
break; break;
case 11: //request coldstart mode case 11: //request coldstart mode
LOG(INFO) << "TC request coldstart"; LOG(INFO) << "TC request flowgraph coldstart";
//todo: delete all ephemeris and almanac information from maps (also the PVT map queue)
//todo: reorder the satellite queues to the receiver default startup order.
//This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
//start again the satellite acquisitions //start again the satellite acquisitions
for (unsigned int i = 0; i < channels_count_; i++) for (unsigned int i = 0; i < channels_count_; i++)
{ {
@ -1136,11 +1132,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
} }
break; break;
case 12: //request hotstart mode case 12: //request hotstart mode
LOG(INFO) << "TC request hotstart"; LOG(INFO) << "TC request flowgraph hotstart";
//todo: call here the function that computes the set of visible satellites and its elevation
//for the date and time specified by the hotstart command and the last available PVT
//todo: reorder the satellite queue to acquire first those visible satellites
//start again the satellite acquisitions
for (unsigned int i = 0; i < channels_count_; i++) for (unsigned int i = 0; i < channels_count_; i++)
{ {
unsigned int ch_index = (who + i + 1) % channels_count_; unsigned int ch_index = (who + i + 1) % channels_count_;
@ -1168,13 +1160,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
} }
break; break;
case 13: //request warmstart mode case 13: //request warmstart mode
LOG(INFO) << "TC request warmstart"; LOG(INFO) << "TC request flowgraph warmstart";
//todo: delete all ephemeris and almanac information from maps (also the PVT map queue)
//todo: load the ephemeris and the almanac from XML files (receiver assistance)
//todo: call here the function that computes the set of visible satellites and its elevation
//for the date and time specified by the warmstart command and the assisted position
//todo: reorder the satellite queue to acquire first those visible satellites
//start again the satellite acquisitions //start again the satellite acquisitions
for (unsigned int i = 0; i < channels_count_; i++) for (unsigned int i = 0; i < channels_count_; i++)
{ {
@ -1209,6 +1195,60 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
} }
void GNSSFlowgraph::priorize_satellites(std::vector<std::pair<int, Gnss_Satellite>> visible_satellites)
{
size_t old_size;
Gnss_Signal gs;
for (std::vector<std::pair<int, Gnss_Satellite>>::iterator it = visible_satellites.begin(); it != visible_satellites.end(); ++it)
{
if (it->second.get_system() == "GPS")
{
gs = Gnss_Signal(it->second, "1C");
old_size = available_GPS_1C_signals_.size();
available_GPS_1C_signals_.remove(gs);
if (old_size > available_GPS_1C_signals_.size())
{
available_GPS_1C_signals_.push_front(gs);
}
gs = Gnss_Signal(it->second, "2S");
old_size = available_GPS_2S_signals_.size();
available_GPS_2S_signals_.remove(gs);
if (old_size > available_GPS_2S_signals_.size())
{
available_GPS_2S_signals_.push_front(gs);
}
gs = Gnss_Signal(it->second, "L5");
old_size = available_GPS_L5_signals_.size();
available_GPS_L5_signals_.remove(gs);
if (old_size > available_GPS_L5_signals_.size())
{
available_GPS_L5_signals_.push_front(gs);
}
}
else if (it->second.get_system() == "Galileo")
{
gs = Gnss_Signal(it->second, "1B");
old_size = available_GAL_1B_signals_.size();
available_GAL_1B_signals_.remove(gs);
if (old_size > available_GAL_1B_signals_.size())
{
available_GAL_1B_signals_.push_front(gs);
}
gs = Gnss_Signal(it->second, "5X");
old_size = available_GAL_5X_signals_.size();
available_GAL_5X_signals_.remove(gs);
if (old_size > available_GAL_5X_signals_.size())
{
available_GAL_5X_signals_.push_front(gs);
}
}
}
}
void GNSSFlowgraph::set_configuration(std::shared_ptr<ConfigurationInterface> configuration) void GNSSFlowgraph::set_configuration(std::shared_ptr<ConfigurationInterface> configuration)
{ {
if (running_) if (running_)

22
src/core/receiver/gnss_flowgraph.h
View File

@ -41,6 +41,11 @@
#include "gnss_signal.h" #include "gnss_signal.h"
#include "gnss_sdr_sample_counter.h" #include "gnss_sdr_sample_counter.h"
#include "gnss_synchro_monitor.h" #include "gnss_synchro_monitor.h"
#include "gnss_block_interface.h"
#include "pvt_interface.h"
#include "channel_interface.h"
#include "configuration_interface.h"
#include "gnss_block_factory.h"
#include <gnuradio/top_block.h> #include <gnuradio/top_block.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
#include <list> #include <list>
@ -55,10 +60,6 @@
#include "gnss_sdr_fpga_sample_counter.h" #include "gnss_sdr_fpga_sample_counter.h"
#endif #endif
class GNSSBlockInterface;
class ChannelInterface;
class ConfigurationInterface;
class GNSSBlockFactory;
/*! \brief This class represents a GNSS flow graph. /*! \brief This class represents a GNSS flow graph.
* *
@ -129,6 +130,19 @@ public:
*/ */
bool send_telemetry_msg(pmt::pmt_t msg); bool send_telemetry_msg(pmt::pmt_t msg);
/*!
* \brief Returns a smart pointer to the PVT object
*/
std::shared_ptr<PvtInterface> get_pvt()
{
return std::dynamic_pointer_cast<PvtInterface>(pvt_);
}
/*!
* \brief Priorize visible satellites in the specified vector
*/
void priorize_satellites(std::vector<std::pair<int, Gnss_Satellite>> visible_satellites);
private: private:
void init(); // Populates the SV PRN list available for acquisition and tracking void init(); // Populates the SV PRN list available for acquisition and tracking
void set_signals_list(); void set_signals_list();

123
src/core/receiver/tcp_cmd_interface.cc
View File

@ -32,8 +32,22 @@
#include "tcp_cmd_interface.h" #include "tcp_cmd_interface.h"
#include "control_message_factory.h" #include "control_message_factory.h"
#include <functional> #include <functional>
#include <sstream>
void TcpCmdInterface::set_pvt(std::shared_ptr<PvtInterface> PVT_sptr)
{
PVT_sptr_ = PVT_sptr;
}
time_t TcpCmdInterface::get_utc_time()
{
return receiver_utc_time_;
}
arma::vec TcpCmdInterface::get_LLH()
{
return arma::vec{rx_latitude_, rx_longitude_, rx_altitude_};
}
std::string TcpCmdInterface::reset(const std::vector<std::string> &commandLine) std::string TcpCmdInterface::reset(const std::vector<std::string> &commandLine)
{ {
std::string response; std::string response;
@ -69,17 +83,75 @@ std::string TcpCmdInterface::standby(const std::vector<std::string> &commandLine
std::string TcpCmdInterface::status(const std::vector<std::string> &commandLine) std::string TcpCmdInterface::status(const std::vector<std::string> &commandLine)
{ {
std::string response; std::stringstream str_stream;
//todo: implement the receiver status report //todo: implement the receiver status report
response = "Not implemented\n";
return response; // str_stream << "-------------------------------------------------------\n";
// str_stream << "ch | sys | sig | mode | Tlm | Eph | Doppler | CN0 |\n";
// str_stream << " | | | | | | [Hz] | [dB - Hz] |\n";
// str_stream << "-------------------------------------------------------\n";
// int n_ch = 10;
// for (int n = 0; n < n_ch; n++)
// {
// str_stream << n << "GPS | L1CA | TRK | YES | YES | 23412.4 | 44.3 |\n";
// }
// str_stream << "--------------------------------------------------------\n";
double longitude_deg, latitude_deg, height_m, ground_speed_kmh, course_over_ground_deg;
time_t UTC_time;
if (PVT_sptr_->get_latest_PVT(&longitude_deg,
&latitude_deg,
&height_m,
&ground_speed_kmh,
&course_over_ground_deg,
&UTC_time) == true)
{
struct tm tstruct;
char buf1[80];
tstruct = *gmtime(&UTC_time);
strftime(buf1, sizeof(buf1), "%d/%m/%Y %H:%M:%S", &tstruct);
std::string str_time = std::string(buf1);
str_stream << "- Receiver UTC Time: " << str_time << std::endl;
str_stream << std::setprecision(9);
str_stream << "- Receiver Position WGS84 [Lat, Long, H]: "
<< latitude_deg << ", "
<< longitude_deg << ", ";
str_stream << std::setprecision(3);
str_stream << height_m << std::endl;
str_stream << std::setprecision(1);
str_stream << "- Receiver Speed over Ground [km/h]: " << ground_speed_kmh << std::endl;
str_stream << "- Receiver Course over ground [deg]: " << course_over_ground_deg << std::endl;
}
else
{
str_stream << "No PVT information available.\n";
}
return str_stream.str();
} }
std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine) std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine)
{ {
std::string response; std::string response;
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS if (commandLine.size() > 5)
//todo: store it in a member variable {
//read commandline time parameter
struct tm tm;
strptime(commandLine.at(1).c_str(), "%d/%m/%Y %H:%M:%S", &tm);
receiver_utc_time_ = timegm(&tm);
//read latitude, longitude, and height
rx_latitude_ = std::stod(commandLine.at(3).c_str());
rx_longitude_ = std::stod(commandLine.at(4).c_str());
rx_altitude_ = std::stod(commandLine.at(5).c_str());
if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_))
{
response = "ERROR: position malformed\n";
}
else
{
std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory()); std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (control_queue_ != nullptr) if (control_queue_ != nullptr)
{ {
@ -90,14 +162,37 @@ std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLin
{ {
response = "ERROR\n"; response = "ERROR\n";
} }
}
}
else
{
response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n";
}
return response; return response;
} }
std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLine) std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLine)
{ {
std::string response; std::string response;
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS if (commandLine.size() > 5)
//todo: store it in a member variable {
std::string tmp_str;
//read commandline time parameter
struct tm tm;
tmp_str = commandLine.at(1) + commandLine.at(2);
strptime(tmp_str.c_str(), "%d/%m/%Y %H:%M:%S", &tm);
receiver_utc_time_ = timegm(&tm);
//read latitude, longitude, and height
rx_latitude_ = std::stod(commandLine.at(3).c_str());
rx_longitude_ = std::stod(commandLine.at(4).c_str());
rx_altitude_ = std::stod(commandLine.at(5).c_str());
if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_))
{
response = "ERROR: position malformed\n";
}
else
{
std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory()); std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (control_queue_ != nullptr) if (control_queue_ != nullptr)
{ {
@ -108,6 +203,12 @@ std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLi
{ {
response = "ERROR\n"; response = "ERROR\n";
} }
}
}
else
{
response = "ERROR: time parameter not found, please use warmstart %d/%m/%Y %H:%M:%S Lat Long H\n";
}
return response; return response;
} }
@ -154,6 +255,10 @@ TcpCmdInterface::TcpCmdInterface()
register_functions(); register_functions();
keep_running_ = true; keep_running_ = true;
control_queue_ = nullptr; control_queue_ = nullptr;
rx_latitude_ = 0;
rx_longitude_ = 0;
rx_altitude_ = 0;
receiver_utc_time_ = 0;
} }
@ -218,6 +323,10 @@ void TcpCmdInterface::run_cmd_server(int tcp_port)
response = functions[cmd_vector.at(0)](cmd_vector); response = functions[cmd_vector.at(0)](cmd_vector);
} }
} }
catch (const std::bad_function_call &ex)
{
response = "ERROR: command not found \n ";
}
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
response = "ERROR: command execution error: " + std::string(ex.what()) + "\n"; response = "ERROR: command execution error: " + std::string(ex.what()) + "\n";

21
src/core/receiver/tcp_cmd_interface.h
View File

@ -31,6 +31,7 @@
#ifndef GNSS_SDR_TCPCMDINTERFACE_H_ #ifndef GNSS_SDR_TCPCMDINTERFACE_H_
#define GNSS_SDR_TCPCMDINTERFACE_H_ #define GNSS_SDR_TCPCMDINTERFACE_H_
#include "pvt_interface.h"
#include <functional> #include <functional>
#include <iostream> #include <iostream>
#include <string> #include <string>
@ -42,7 +43,8 @@
#include <cstdint> #include <cstdint>
#include <gnuradio/message.h> #include <gnuradio/message.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
#include <armadillo>
#include <time.h>
class TcpCmdInterface class TcpCmdInterface
{ {
@ -51,7 +53,16 @@ public:
virtual ~TcpCmdInterface(); virtual ~TcpCmdInterface();
void run_cmd_server(int tcp_port); void run_cmd_server(int tcp_port);
void set_msg_queue(gr::msg_queue::sptr control_queue); void set_msg_queue(gr::msg_queue::sptr control_queue);
/*!
* \brief gets the UTC time parsed from the last TC command issued
*/
time_t get_utc_time();
/*!
* \brief gets the Latitude, Longitude and Altitude vector from the last TC command issued
*/
arma::vec get_LLH();
void set_pvt(std::shared_ptr<PvtInterface> PVT_sptr);
private: private:
std::unordered_map<std::string, std::function<std::string(const std::vector<std::string> &)>> std::unordered_map<std::string, std::function<std::string(const std::vector<std::string> &)>>
@ -68,6 +79,14 @@ private:
gr::msg_queue::sptr control_queue_; gr::msg_queue::sptr control_queue_;
bool keep_running_; bool keep_running_;
time_t receiver_utc_time_;
double rx_latitude_;
double rx_longitude_;
double rx_altitude_;
std::shared_ptr<PvtInterface> PVT_sptr_;
}; };
#endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */ #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */

2
src/core/system_parameters/gps_almanac.cc
View File

@ -37,6 +37,7 @@ Gps_Almanac::Gps_Almanac()
i_satellite_PRN = 0U; i_satellite_PRN = 0U;
d_Delta_i = 0.0; d_Delta_i = 0.0;
d_Toa = 0.0; d_Toa = 0.0;
i_WNa = 0;
d_M_0 = 0.0; d_M_0 = 0.0;
d_e_eccentricity = 0.0; d_e_eccentricity = 0.0;
d_sqrt_A = 0.0; d_sqrt_A = 0.0;
@ -44,6 +45,7 @@ Gps_Almanac::Gps_Almanac()
d_OMEGA = 0.0; d_OMEGA = 0.0;
d_OMEGA_DOT = 0.0; d_OMEGA_DOT = 0.0;
i_SV_health = 0; i_SV_health = 0;
i_AS_status = 0;
d_A_f0 = 0.0; d_A_f0 = 0.0;
d_A_f1 = 0.0; d_A_f1 = 0.0;
} }

8
src/core/system_parameters/gps_almanac.h
View File

@ -44,15 +44,17 @@ class Gps_Almanac
{ {
public: public:
uint32_t i_satellite_PRN; //!< SV PRN NUMBER uint32_t i_satellite_PRN; //!< SV PRN NUMBER
double d_Delta_i; double d_Delta_i; //!< Inclination Angle at Reference Time (relative to i_0 = 0.30 semi-circles)
double d_Toa; //!< Almanac data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] double d_Toa; //!< Almanac data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s]
int32_t i_WNa; //!< Almanac week number
double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles] double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles]
double d_e_eccentricity; //!< Eccentricity [dimensionless] double d_e_eccentricity; //!< Eccentricity [dimensionless]
double d_sqrt_A; //!< Square Root of the Semi-Major Axis [sqrt(m)] double d_sqrt_A; //!< Square Root of the Semi-Major Axis [sqrt(m)]
double d_OMEGA0; //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] double d_OMEGA0; //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
double d_OMEGA; //!< Argument of Perigee [semi-cicles] double d_OMEGA; //!< Argument of Perigee [semi-cicles]
double d_OMEGA_DOT; //!< Rate of Right Ascension [semi-circles/s] double d_OMEGA_DOT; //!< Rate of Right Ascension [semi-circles/s]
int32_t i_SV_health; // SV Health int32_t i_SV_health; //!< SV Health
int32_t i_AS_status; //!< Anti-Spoofing Flags and SV Configuration
double d_A_f0; //!< Coefficient 0 of code phase offset model [s] double d_A_f0; //!< Coefficient 0 of code phase offset model [s]
double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s] double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s]
@ -71,6 +73,7 @@ public:
ar& BOOST_SERIALIZATION_NVP(i_satellite_PRN); ar& BOOST_SERIALIZATION_NVP(i_satellite_PRN);
ar& BOOST_SERIALIZATION_NVP(d_Delta_i); ar& BOOST_SERIALIZATION_NVP(d_Delta_i);
ar& BOOST_SERIALIZATION_NVP(d_Toa); ar& BOOST_SERIALIZATION_NVP(d_Toa);
ar& BOOST_SERIALIZATION_NVP(i_WNa);
ar& BOOST_SERIALIZATION_NVP(d_M_0); ar& BOOST_SERIALIZATION_NVP(d_M_0);
ar& BOOST_SERIALIZATION_NVP(d_e_eccentricity); ar& BOOST_SERIALIZATION_NVP(d_e_eccentricity);
ar& BOOST_SERIALIZATION_NVP(d_sqrt_A); ar& BOOST_SERIALIZATION_NVP(d_sqrt_A);
@ -78,6 +81,7 @@ public:
ar& BOOST_SERIALIZATION_NVP(d_OMEGA); ar& BOOST_SERIALIZATION_NVP(d_OMEGA);
ar& BOOST_SERIALIZATION_NVP(d_OMEGA_DOT); ar& BOOST_SERIALIZATION_NVP(d_OMEGA_DOT);
ar& BOOST_SERIALIZATION_NVP(i_SV_health); ar& BOOST_SERIALIZATION_NVP(i_SV_health);
ar& BOOST_SERIALIZATION_NVP(i_AS_status);
ar& BOOST_SERIALIZATION_NVP(d_A_f0); ar& BOOST_SERIALIZATION_NVP(d_A_f0);
ar& BOOST_SERIALIZATION_NVP(d_A_f1); ar& BOOST_SERIALIZATION_NVP(d_A_f1);
} }

1
src/main/CMakeLists.txt
View File

@ -50,6 +50,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/core/system_parameters ${CMAKE_SOURCE_DIR}/src/core/system_parameters
${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/interfaces
${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/core/receiver
${CMAKE_SOURCE_DIR}/src/core/monitor
${CMAKE_SOURCE_DIR}/src/core/libs ${CMAKE_SOURCE_DIR}/src/core/libs
${CMAKE_SOURCE_DIR}/src/core/libs/supl ${CMAKE_SOURCE_DIR}/src/core/libs/supl
${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp ${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp

1
src/tests/system-tests/libs/CMakeLists.txt
View File

@ -18,7 +18,6 @@
set(SYSTEM_TESTING_LIB_SOURCES set(SYSTEM_TESTING_LIB_SOURCES
geofunctions.cc
spirent_motion_csv_dump_reader.cc spirent_motion_csv_dump_reader.cc
rtklib_solver_dump_reader.cc rtklib_solver_dump_reader.cc
) )

2
src/tests/system-tests/libs/position_test_flags.h
View File

@ -47,5 +47,5 @@ DEFINE_double(static_3D_error_m, 5.0, "Static scenario 3D (East, North, Up) posi
DEFINE_double(accuracy_CEP, 2.0, "Static scenario 2D (East, North) accuracy Circular Error Position (CEP) threshold [meters]"); DEFINE_double(accuracy_CEP, 2.0, "Static scenario 2D (East, North) accuracy Circular Error Position (CEP) threshold [meters]");
DEFINE_double(precision_SEP, 10.0, "Static scenario 3D (East, North, Up) precision Spherical Error Position (SEP) threshold [meters]"); DEFINE_double(precision_SEP, 10.0, "Static scenario 3D (East, North, Up) precision Spherical Error Position (SEP) threshold [meters]");
DEFINE_double(dynamic_3D_position_RMSE, 10.0, "Dynamic scenario 3D (ECEF) accuracy RMSE threshold [meters]"); DEFINE_double(dynamic_3D_position_RMSE, 10.0, "Dynamic scenario 3D (ECEF) accuracy RMSE threshold [meters]");
DEFINE_double(dynamic_3D_velocity_RMSE, 5.0, "Dynamic scenario 3D (ECEF) accuracy RMSE threshold [meters/second]"); DEFINE_double(dynamic_3D_velocity_RMSE, 5.0, "Dynamic scenario 3D (ECEF) velocity accuracy RMSE threshold [meters/second]");
#endif #endif

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

@ -157,7 +157,7 @@ void NmeaPrinterTest::conf()
TEST_F(NmeaPrinterTest, PrintLine) TEST_F(NmeaPrinterTest, PrintLine)
{ {
std::string filename("nmea_test.nmea"); std::string filename("nmea_test.nmea");
std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, rtk); std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, false, rtk);
boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc
@ -196,7 +196,7 @@ TEST_F(NmeaPrinterTest, PrintLine)
TEST_F(NmeaPrinterTest, PrintLineLessthan10min) TEST_F(NmeaPrinterTest, PrintLineLessthan10min)
{ {
std::string filename("nmea_test.nmea"); std::string filename("nmea_test.nmea");
std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, rtk); std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, false, rtk);
boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc

3
src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc
View File

@ -330,9 +330,10 @@ TEST(RTKLibSolverTest, test1)
int nchannels = 8; int nchannels = 8;
std::string dump_filename = ".rtklib_solver_dump.dat"; std::string dump_filename = ".rtklib_solver_dump.dat";
bool flag_dump_to_file = false; bool flag_dump_to_file = false;
bool save_to_mat = false;
rtk_t rtk = configure_rtklib_options(); rtk_t rtk = configure_rtklib_options();
std::unique_ptr<rtklib_solver> d_ls_pvt(new rtklib_solver(nchannels, dump_filename, flag_dump_to_file, rtk)); std::unique_ptr<rtklib_solver> d_ls_pvt(new rtklib_solver(nchannels, dump_filename, flag_dump_to_file, save_to_mat, rtk));
d_ls_pvt->set_averaging_depth(1); d_ls_pvt->set_averaging_depth(1);
// load ephemeris // load ephemeris