mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-17 07:14:55 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
6d0ee84836
@ -17,6 +17,7 @@
|
||||
<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_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_e_eccentricity"/>
|
||||
<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_DOT"/>
|
||||
<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_f1"/>
|
||||
</xs:sequence>
|
||||
|
||||
@ -48,6 +48,27 @@ namespace bc = boost::integer;
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
|
||||
bool RtklibPvt::get_latest_PVT(double* longitude_deg,
|
||||
double* latitude_deg,
|
||||
double* height_m,
|
||||
double* ground_speed_kmh,
|
||||
double* course_over_ground_deg,
|
||||
time_t* UTC_time)
|
||||
{
|
||||
return pvt_->get_latest_PVT(longitude_deg,
|
||||
latitude_deg,
|
||||
height_m,
|
||||
ground_speed_kmh,
|
||||
course_over_ground_deg,
|
||||
UTC_time);
|
||||
}
|
||||
|
||||
void RtklibPvt::clear_ephemeris()
|
||||
{
|
||||
pvt_->clear_ephemeris();
|
||||
}
|
||||
|
||||
RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
@ -64,6 +85,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
|
||||
DLOG(INFO) << "role " << role;
|
||||
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_mat = configuration->property(role + ".dump_mat", true);
|
||||
|
||||
// output rate
|
||||
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_2G_count = configuration->property("Channels_2G.count", 0);
|
||||
|
||||
//unsigned int type_of_receiver = 0;
|
||||
|
||||
// *******************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 = 1; // L1
|
||||
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 = 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 = 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 = 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; // 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; // 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 = 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 = 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 = 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; // 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 = 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) && (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 = 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;
|
||||
@ -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 = 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 = 32; // L1+E1+L5+E5a
|
||||
|
||||
// RTKLIB PVT solver options
|
||||
// Settings 1
|
||||
int positioning_mode = -1;
|
||||
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("Static") == 0) positioning_mode = PMODE_STATIC;
|
||||
if (positioning_mode_str.compare("Kinematic") == 0) positioning_mode = PMODE_KINEMA;
|
||||
@ -515,6 +536,22 @@ RtklibPvt::~RtklibPvt()
|
||||
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)
|
||||
{
|
||||
|
||||
@ -63,6 +63,13 @@ public:
|
||||
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 disconnect(gr::top_block_sptr top_block) override;
|
||||
gr::basic_block_sptr get_left_block() override;
|
||||
@ -79,6 +86,13 @@ public:
|
||||
return sizeof(gr_complex);
|
||||
}
|
||||
|
||||
bool get_latest_PVT(double* longitude_deg,
|
||||
double* latitude_deg,
|
||||
double* height_m,
|
||||
double* ground_speed_kmh,
|
||||
double* course_over_ground_deg,
|
||||
time_t* UTC_time);
|
||||
|
||||
private:
|
||||
rtklib_pvt_cc_sptr pvt_;
|
||||
rtk_t rtk;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -31,7 +31,7 @@
|
||||
#ifndef GNSS_SDR_RTKLIB_PVT_CC_H
|
||||
#define GNSS_SDR_RTKLIB_PVT_CC_H
|
||||
|
||||
|
||||
#include "gps_ephemeris.h"
|
||||
#include "nmea_printer.h"
|
||||
#include "kml_printer.h"
|
||||
#include "gpx_printer.h"
|
||||
@ -60,7 +60,7 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t n_channels,
|
||||
rtk_t& rtk);
|
||||
|
||||
/*!
|
||||
* \brief This class implements a block that computes the PVT solution with Galileo E1 signals
|
||||
* \brief This class implements a block that computes the PVT solution using the RTKLIB integrated library
|
||||
*/
|
||||
class rtklib_pvt_cc : public gr::sync_block
|
||||
{
|
||||
@ -72,6 +72,7 @@ private:
|
||||
void msg_handler_telemetry(pmt::pmt_t msg);
|
||||
|
||||
bool d_dump;
|
||||
bool d_dump_mat;
|
||||
bool b_rinex_output_enabled;
|
||||
bool b_rinex_header_written;
|
||||
bool b_rinex_header_updated;
|
||||
@ -80,6 +81,7 @@ private:
|
||||
int32_t d_rinexnav_rate_ms;
|
||||
|
||||
bool b_rtcm_writing_started;
|
||||
bool b_rtcm_enabled;
|
||||
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_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits)
|
||||
@ -140,11 +142,33 @@ public:
|
||||
rtk_t& rtk);
|
||||
|
||||
/*!
|
||||
* \brief Get latest set of GPS L1 ephemeris from PVT block
|
||||
* \brief Get latest set of ephemeris from PVT block
|
||||
*
|
||||
* It is used to save the assistance data at the receiver shutdown
|
||||
*/
|
||||
std::map<int, Gps_Ephemeris> get_GPS_L1_ephemeris_map();
|
||||
std::map<int, Gps_Ephemeris> get_gps_ephemeris_map();
|
||||
|
||||
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
|
||||
|
||||
|
||||
@ -59,6 +59,7 @@ include_directories(
|
||||
${ARMADILLO_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${MATIO_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
list(SORT PVT_LIB_HEADERS)
|
||||
@ -66,7 +67,12 @@ list(SORT PVT_LIB_SOURCES)
|
||||
|
||||
add_library(pvt_lib ${PVT_LIB_SOURCES} ${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(
|
||||
pvt_lib
|
||||
@ -77,4 +83,5 @@ target_link_libraries(
|
||||
${ARMADILLO_LIBRARIES}
|
||||
${BLAS}
|
||||
${LAPACK}
|
||||
${MATIO_LIBRARIES}
|
||||
)
|
||||
|
||||
@ -34,6 +34,7 @@
|
||||
*/
|
||||
|
||||
#include "nmea_printer.h"
|
||||
#include "rtklib_solution.h"
|
||||
#include <boost/date_time/posix_time/posix_time.hpp>
|
||||
#include <boost/filesystem/operations.hpp> // for create_directories, exists
|
||||
#include <boost/filesystem/path.hpp> // for path, operator<<
|
||||
@ -376,99 +377,10 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
|
||||
std::string Nmea_Printer::get_GPRMC()
|
||||
{
|
||||
// Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10
|
||||
bool valid_fix = d_PVT_data->is_valid_position();
|
||||
|
||||
// ToDo: Compute speed and course over ground
|
||||
double speed_over_ground_knots = 0;
|
||||
double course_over_ground_deg = 0;
|
||||
|
||||
// boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
|
||||
|
||||
std::stringstream sentence_str;
|
||||
|
||||
// GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
|
||||
std::string sentence_header;
|
||||
sentence_header = "$GPRMC,";
|
||||
sentence_str << sentence_header;
|
||||
|
||||
// UTC Time: hhmmss.sss
|
||||
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
|
||||
|
||||
// Status: A: data valid, V: data NOT valid
|
||||
if (valid_fix == true)
|
||||
{
|
||||
sentence_str << ",A";
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << ",V";
|
||||
};
|
||||
|
||||
if (print_avg_pos == true)
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
|
||||
}
|
||||
else
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
|
||||
}
|
||||
|
||||
// Speed over ground (knots)
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.precision(2);
|
||||
sentence_str << speed_over_ground_knots;
|
||||
|
||||
// course over ground (degrees)
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.precision(2);
|
||||
sentence_str << course_over_ground_deg;
|
||||
|
||||
// Date ddmmyy
|
||||
boost::gregorian::date sentence_date = d_PVT_data->get_position_UTC_time().date();
|
||||
unsigned int year = sentence_date.year();
|
||||
unsigned int day = sentence_date.day();
|
||||
unsigned int month = sentence_date.month();
|
||||
|
||||
sentence_str << ",";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << day;
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << month;
|
||||
|
||||
std::stringstream year_strs;
|
||||
year_strs << std::dec << year;
|
||||
sentence_str << std::dec << year_strs.str().substr(2);
|
||||
|
||||
// Magnetic Variation (degrees)
|
||||
// ToDo: Implement magnetic compass
|
||||
sentence_str << ",";
|
||||
|
||||
// Magnetic Variation (E or W)
|
||||
// ToDo: Implement magnetic compass
|
||||
sentence_str << ",";
|
||||
|
||||
// Checksum
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
tmpstr = sentence_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str << "\r\n";
|
||||
unsigned char buff[1024] = {0};
|
||||
outnmea_rmc(buff, &d_PVT_data->pvt_sol);
|
||||
sentence_str << buff;
|
||||
return sentence_str.str();
|
||||
}
|
||||
|
||||
@ -477,82 +389,10 @@ std::string Nmea_Printer::get_GPGSA()
|
||||
{
|
||||
// $GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
|
||||
// GSA-GNSS DOP and Active Satellites
|
||||
bool valid_fix = d_PVT_data->is_valid_position();
|
||||
int n_sats_used = d_PVT_data->get_num_valid_observations();
|
||||
double pdop = d_PVT_data->get_pdop();
|
||||
double hdop = d_PVT_data->get_hdop();
|
||||
double vdop = d_PVT_data->get_vdop();
|
||||
|
||||
std::stringstream sentence_str;
|
||||
std::string sentence_header;
|
||||
sentence_header = "$GPGSA,";
|
||||
sentence_str << sentence_header;
|
||||
|
||||
// mode1:
|
||||
// (M) Manual-forced to operate in 2D or 3D mode
|
||||
// (A) Automatic-allowed to automatically switch 2D/3D
|
||||
std::string mode1 = "M";
|
||||
sentence_str << mode1;
|
||||
|
||||
// mode2:
|
||||
// 1 fix not available
|
||||
// 2 fix 2D
|
||||
// 3 fix 3D
|
||||
if (valid_fix == true)
|
||||
{
|
||||
sentence_str << ",3";
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << ",1";
|
||||
};
|
||||
|
||||
// Used satellites
|
||||
for (int i = 0; i < 12; i++)
|
||||
{
|
||||
sentence_str << ",";
|
||||
if (i < n_sats_used)
|
||||
{
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << d_PVT_data->get_visible_satellites_ID(i);
|
||||
}
|
||||
}
|
||||
|
||||
// PDOP
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.width(2);
|
||||
sentence_str.precision(1);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << pdop;
|
||||
// HDOP
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.width(2);
|
||||
sentence_str.precision(1);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << hdop;
|
||||
// VDOP
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.width(2);
|
||||
sentence_str.precision(1);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << vdop;
|
||||
|
||||
// Checksum
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
tmpstr = sentence_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str << "\r\n";
|
||||
unsigned char buff[1024] = {0};
|
||||
outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
|
||||
sentence_str << buff;
|
||||
return sentence_str.str();
|
||||
}
|
||||
|
||||
@ -560,199 +400,22 @@ std::string Nmea_Printer::get_GPGSA()
|
||||
std::string Nmea_Printer::get_GPGSV()
|
||||
{
|
||||
// GSV-GNSS Satellites in View
|
||||
// Notice that NMEA 2.1 only supports 12 channels
|
||||
int n_sats_used = d_PVT_data->get_num_valid_observations();
|
||||
std::stringstream sentence_str;
|
||||
std::stringstream frame_str;
|
||||
std::string sentence_header;
|
||||
sentence_header = "$GPGSV,";
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
|
||||
// 1st step: How many GPGSV frames we need? (up to 3)
|
||||
// Each frame contains up to 4 satellites
|
||||
int n_frames;
|
||||
n_frames = std::ceil((static_cast<double>(n_sats_used)) / 4.0);
|
||||
|
||||
// generate the frames
|
||||
int current_satellite = 0;
|
||||
for (int i = 1; i < (n_frames + 1); i++)
|
||||
{
|
||||
frame_str.str("");
|
||||
frame_str << sentence_header;
|
||||
|
||||
// number of messages
|
||||
frame_str << n_frames;
|
||||
|
||||
// message number
|
||||
frame_str << ",";
|
||||
frame_str << i;
|
||||
|
||||
// total number of satellites in view
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str << std::dec << n_sats_used;
|
||||
|
||||
// satellites info
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
// write satellite info
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str << std::dec << d_PVT_data->get_visible_satellites_ID(current_satellite);
|
||||
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_El(current_satellite));
|
||||
|
||||
frame_str << ",";
|
||||
frame_str.width(3);
|
||||
frame_str.fill('0');
|
||||
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_Az(current_satellite));
|
||||
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_CN0_dB(current_satellite));
|
||||
|
||||
current_satellite++;
|
||||
|
||||
if (current_satellite == n_sats_used)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// frame checksum
|
||||
tmpstr = frame_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
frame_str << "*";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
frame_str << "\r\n";
|
||||
|
||||
//add frame to sentence
|
||||
sentence_str << frame_str.str();
|
||||
}
|
||||
return sentence_str.str();
|
||||
// $GPGSV,2,1,07,07,79,048,42,02,51,062,43,26,36,256,42,27,27,138,42*71
|
||||
// Notice that NMEA 2.1 only supports 12 channels
|
||||
std::stringstream sentence_str;
|
||||
unsigned char buff[1024] = {0};
|
||||
outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
|
||||
sentence_str << buff;
|
||||
return sentence_str.str();
|
||||
}
|
||||
|
||||
|
||||
std::string Nmea_Printer::get_GPGGA()
|
||||
{
|
||||
// boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
|
||||
bool valid_fix = d_PVT_data->is_valid_position();
|
||||
int n_channels = d_PVT_data->get_num_valid_observations(); //d_nchannels
|
||||
double hdop = d_PVT_data->get_hdop();
|
||||
double MSL_altitude;
|
||||
|
||||
if (d_PVT_data->is_averaging() == true)
|
||||
{
|
||||
MSL_altitude = d_PVT_data->get_avg_height();
|
||||
}
|
||||
else
|
||||
{
|
||||
MSL_altitude = d_PVT_data->get_height();
|
||||
}
|
||||
|
||||
std::stringstream sentence_str;
|
||||
|
||||
// GPGGA (Global Positioning System Fixed Data)
|
||||
std::string sentence_header;
|
||||
sentence_header = "$GPGGA,";
|
||||
sentence_str << sentence_header;
|
||||
|
||||
// UTC Time: hhmmss.sss
|
||||
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
|
||||
|
||||
if (d_PVT_data->is_averaging() == true)
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
|
||||
}
|
||||
else
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
|
||||
}
|
||||
|
||||
// Position fix indicator
|
||||
// 0 - Fix not available or invalid
|
||||
// 1 - GPS SPS Mode, fix valid
|
||||
// 2 - Differential GPS, SPS Mode, fix valid
|
||||
// 3-5 - Not supported
|
||||
// 6 - Dead Reckoning Mode, fix valid
|
||||
// ToDo: Update PVT module to identify the fix mode
|
||||
|
||||
if (valid_fix == true)
|
||||
{
|
||||
sentence_str << ",1";
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << ",0";
|
||||
}
|
||||
|
||||
// Number of satellites used in PVT
|
||||
sentence_str << ",";
|
||||
if (n_channels < 10)
|
||||
{
|
||||
sentence_str << '0' << n_channels;
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << n_channels;
|
||||
}
|
||||
|
||||
// HDOP
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.width(2);
|
||||
sentence_str.precision(1);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << hdop;
|
||||
|
||||
// MSL Altitude
|
||||
sentence_str << ",";
|
||||
sentence_str.precision(1);
|
||||
sentence_str << MSL_altitude;
|
||||
sentence_str << ",M";
|
||||
|
||||
// Geoid-to-ellipsoid separation. Ellipsoid altitude = MSL Altitude + Geoid Separation.
|
||||
// ToDo: Compute this value
|
||||
sentence_str << ",";
|
||||
sentence_str << "0.0";
|
||||
sentence_str << ",M";
|
||||
|
||||
// Age of Diff. Corr. (Seconds) Null fields when DGPS is not used
|
||||
// Diff. Ref. Station ID (0000)
|
||||
// ToDo: Implement this fields for Differential GPS
|
||||
sentence_str << ",";
|
||||
sentence_str << "0.0,0000";
|
||||
|
||||
// Checksum
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
tmpstr = sentence_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str << "\r\n";
|
||||
unsigned char buff[1024] = {0};
|
||||
outnmea_gga(buff, &d_PVT_data->pvt_sol);
|
||||
sentence_str << buff;
|
||||
return sentence_str.str();
|
||||
// $GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
|
||||
}
|
||||
|
||||
@ -41,6 +41,7 @@ Pvt_Conf::Pvt_Conf()
|
||||
rinexnav_rate_ms = 0;
|
||||
|
||||
dump = false;
|
||||
dump_mat = true;
|
||||
|
||||
flag_nmea_tty_port = false;
|
||||
|
||||
|
||||
@ -49,6 +49,7 @@ public:
|
||||
std::map<int, int> rtcm_msg_rate_ms;
|
||||
|
||||
bool dump;
|
||||
bool dump_mat;
|
||||
std::string dump_filename;
|
||||
|
||||
bool flag_nmea_tty_port;
|
||||
|
||||
@ -43,6 +43,8 @@ Pvt_Solution::Pvt_Solution()
|
||||
d_latitude_d = 0.0;
|
||||
d_longitude_d = 0.0;
|
||||
d_height_m = 0.0;
|
||||
d_speed_over_ground_m_s = 0.0;
|
||||
d_course_over_ground_d = 0.0;
|
||||
d_avg_latitude_d = 0.0;
|
||||
d_avg_longitude_d = 0.0;
|
||||
d_avg_height_m = 0.0;
|
||||
@ -126,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_longitude_d = lambda * 180.0 / GPS_PI;
|
||||
d_height_m = h;
|
||||
//todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -516,6 +519,25 @@ double Pvt_Solution::get_height() const
|
||||
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
|
||||
{
|
||||
|
||||
@ -52,6 +52,8 @@ private:
|
||||
double d_latitude_d; // RX position Latitude WGS84 [deg]
|
||||
double d_longitude_d; // RX position Longitude WGS84 [deg]
|
||||
double d_height_m; // RX position height WGS84 [m]
|
||||
double d_speed_over_ground_m_s; // RX speed over ground [m/s]
|
||||
double d_course_over_ground_d; // RX course over ground [deg]
|
||||
|
||||
double d_avg_latitude_d; // Averaged latitude in degrees
|
||||
double d_avg_longitude_d; // Averaged longitude in degrees
|
||||
@ -86,6 +88,12 @@ public:
|
||||
double get_longitude() const; //!< Get RX position Longitude WGS84 [deg]
|
||||
double get_height() const; //!< Get RX position height WGS84 [m]
|
||||
|
||||
double get_speed_over_ground() const; //!< Get RX speed over ground [m/s]
|
||||
void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s]
|
||||
|
||||
double get_course_over_ground() const; //!< Get RX course over ground [deg]
|
||||
void set_course_over_ground(double cog_deg); //!< Set RX course over ground [deg]
|
||||
|
||||
double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg]
|
||||
double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg]
|
||||
double get_avg_height() const; //!< Get RX position averaged height WGS84 [m]
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -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);
|
||||
|
||||
/*!
|
||||
* \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
|
||||
*/
|
||||
@ -141,12 +146,12 @@ public:
|
||||
/*!
|
||||
* \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".
|
||||
@ -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");
|
||||
|
||||
/*!
|
||||
* \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".
|
||||
*/
|
||||
@ -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);
|
||||
|
||||
/*!
|
||||
* \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
|
||||
*/
|
||||
@ -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);
|
||||
|
||||
/*!
|
||||
* \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".
|
||||
*/
|
||||
@ -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_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 Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac);
|
||||
|
||||
@ -53,26 +53,33 @@
|
||||
|
||||
#include "rtklib_solver.h"
|
||||
#include "rtklib_conversions.h"
|
||||
#include "rtklib_solution.h"
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "Galileo_E1.h"
|
||||
#include "GLONASS_L1_L2_CA.h"
|
||||
#include <matio.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
|
||||
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
|
||||
d_nchannels = nchannels;
|
||||
d_dump_filename = dump_filename;
|
||||
d_flag_dump_enabled = flag_dump_to_file;
|
||||
d_flag_dump_mat_enabled = flag_dump_to_mat;
|
||||
count_valid_position = 0;
|
||||
this->set_averaging_flag(false);
|
||||
rtk_ = rtk;
|
||||
for (unsigned int i = 0; i < 4; i++) dop_[i] = 0.0;
|
||||
pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0};
|
||||
|
||||
ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{{0, 0}}, {{0, 0}}}, {{}, {}}};
|
||||
for (unsigned int i = 0; i < MAXSAT; i++)
|
||||
{
|
||||
pvt_ssat[i] = ssat0;
|
||||
}
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_flag_dump_enabled == true)
|
||||
{
|
||||
@ -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()
|
||||
{
|
||||
@ -106,6 +408,10 @@ rtklib_solver::~rtklib_solver()
|
||||
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;
|
||||
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;
|
||||
@ -508,8 +818,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
|
||||
index_aux++;
|
||||
}
|
||||
}
|
||||
if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
|
||||
|
||||
if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
|
||||
this->set_valid_position(true);
|
||||
arma::vec rx_position_and_time(4);
|
||||
rx_position_and_time(0) = pvt_sol.rr[0]; // [m]
|
||||
@ -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]
|
||||
}
|
||||
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
|
||||
|
||||
//compute Ground speed and COG
|
||||
double ground_speed_ms = 0.0;
|
||||
double pos[3];
|
||||
double enuv[3];
|
||||
ecef2pos(pvt_sol.rr, pos);
|
||||
ecef2enu(pos, &pvt_sol.rr[3], enuv);
|
||||
this->set_speed_over_ground(norm_rtk(enuv, 2));
|
||||
double new_cog;
|
||||
if (ground_speed_ms >= 1.0)
|
||||
{
|
||||
new_cog = atan2(enuv[0], enuv[1]) * R2D;
|
||||
if (new_cog < 0.0) new_cog += 360.0;
|
||||
this->set_course_over_ground(new_cog);
|
||||
}
|
||||
|
||||
//observable fix:
|
||||
//double offset_s = this->get_time_offset_s();
|
||||
//this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds]
|
||||
|
||||
@ -77,14 +77,17 @@ private:
|
||||
rtk_t rtk_;
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
bool save_matfile();
|
||||
|
||||
bool d_flag_dump_enabled;
|
||||
bool d_flag_dump_mat_enabled;
|
||||
int d_nchannels; // Number of available channels for positioning
|
||||
double dop_[4];
|
||||
|
||||
public:
|
||||
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();
|
||||
|
||||
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging);
|
||||
|
||||
@ -53,7 +53,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
|
||||
Acq_Conf acq_parameters;
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -53,7 +53,7 @@ GalileoE1PcpsAmbiguousAcquisitionFpga::GalileoE1PcpsAmbiguousAcquisitionFpga(
|
||||
pcpsconf_fpga_t acq_parameters;
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -52,7 +52,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
|
||||
Acq_Conf acq_parameters = Acq_Conf();
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -52,7 +52,7 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf
|
||||
pcpsconf_fpga_t acq_parameters;
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -56,7 +56,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
|
||||
Acq_Conf acq_parameters = Acq_Conf();
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -51,7 +51,7 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler(
|
||||
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_dump_filename = "./data/acquisition.dat";
|
||||
std::string default_dump_filename = "./acquisition.mat";
|
||||
|
||||
DLOG(INFO) << "role " << role;
|
||||
Acq_Conf acq_parameters = Acq_Conf();
|
||||
|
||||
@ -54,7 +54,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
|
||||
Acq_Conf acq_parameters = Acq_Conf();
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -55,7 +55,7 @@ GpsL2MPcpsAcquisitionFpga::GpsL2MPcpsAcquisitionFpga(
|
||||
pcpsconf_fpga_t acq_parameters;
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -54,7 +54,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
|
||||
Acq_Conf acq_parameters = Acq_Conf();
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -55,7 +55,7 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga(
|
||||
pcpsconf_fpga_t acq_parameters;
|
||||
configuration_ = configuration;
|
||||
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;
|
||||
|
||||
|
||||
@ -59,7 +59,6 @@ bool ChannelFsm::Event_stop_channel()
|
||||
switch (d_state)
|
||||
{
|
||||
case 0: //already in stanby
|
||||
return true;
|
||||
break;
|
||||
case 1: //acquisition
|
||||
d_state = 0;
|
||||
@ -70,8 +69,9 @@ bool ChannelFsm::Event_stop_channel()
|
||||
stop_tracking();
|
||||
break;
|
||||
default:
|
||||
return true;
|
||||
break;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ChannelFsm::Event_start_acquisition()
|
||||
|
||||
@ -39,6 +39,7 @@ set(GNSS_SPLIBS_SOURCES
|
||||
conjugate_sc.cc
|
||||
conjugate_ic.cc
|
||||
gnss_sdr_create_directory.cc
|
||||
geofunctions.cc
|
||||
)
|
||||
|
||||
set(GNSS_SPLIBS_HEADERS
|
||||
@ -63,6 +64,7 @@ set(GNSS_SPLIBS_HEADERS
|
||||
conjugate_ic.h
|
||||
gnss_sdr_create_directory.h
|
||||
gnss_circular_deque.h
|
||||
geofunctions.h
|
||||
)
|
||||
|
||||
|
||||
@ -101,6 +103,7 @@ include_directories(
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
${ARMADILLO_INCLUDE_DIRS}
|
||||
${GNURADIO_RUNTIME_INCLUDE_DIRS}
|
||||
${GNURADIO_BLOCKS_INCLUDE_DIRS}
|
||||
${VOLK_INCLUDE_DIRS}
|
||||
@ -128,6 +131,7 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
|
||||
${VOLK_LIBRARIES} ${ORC_LIBRARIES}
|
||||
${VOLK_GNSSSDR_LIBRARIES} ${ORC_LIBRARIES}
|
||||
${GFlags_LIBS}
|
||||
${ARMADILLO_LIBRARIES}
|
||||
${GNURADIO_BLOCKS_LIBRARIES}
|
||||
${GNURADIO_FFT_LIBRARIES}
|
||||
${GNURADIO_FILTER_LIBRARIES}
|
||||
@ -136,7 +140,9 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
|
||||
)
|
||||
|
||||
if(NOT VOLK_GNSSSDR_FOUND)
|
||||
add_dependencies(gnss_sp_libs volk_gnsssdr_module)
|
||||
add_dependencies(gnss_sp_libs volk_gnsssdr_module armadillo-${armadillo_RELEASE})
|
||||
else(NOT VOLK_GNSSSDR_FOUND)
|
||||
add_dependencies(gnss_sp_libs armadillo-${armadillo_RELEASE})
|
||||
endif(NOT VOLK_GNSSSDR_FOUND)
|
||||
|
||||
if(${GFLAGS_GREATER_20})
|
||||
|
||||
@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
|
||||
|
||||
return rtklib_sat;
|
||||
}
|
||||
|
||||
alm_t alm_to_rtklib(const Gps_Almanac& gps_alm)
|
||||
{
|
||||
alm_t rtklib_alm;
|
||||
|
||||
rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
|
||||
rtklib_alm.sat = gps_alm.i_satellite_PRN;
|
||||
rtklib_alm.svh = gps_alm.i_SV_health;
|
||||
rtklib_alm.svconf = gps_alm.i_AS_status;
|
||||
rtklib_alm.week = gps_alm.i_WNa;
|
||||
rtklib_alm.toa = gpst2time(gps_alm.i_WNa, gps_alm.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;
|
||||
}
|
||||
|
||||
@ -38,10 +38,16 @@
|
||||
#include "gps_cnav_ephemeris.h"
|
||||
#include "glonass_gnav_ephemeris.h"
|
||||
#include "glonass_gnav_utc_model.h"
|
||||
#include "gps_almanac.h"
|
||||
#include "galileo_almanac.h"
|
||||
|
||||
eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph);
|
||||
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph);
|
||||
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph);
|
||||
|
||||
alm_t alm_to_rtklib(const Gps_Almanac& gps_alm);
|
||||
alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm);
|
||||
|
||||
/*!
|
||||
* \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart
|
||||
* \param glonass_gnav_eph GLONASS GNAV Ephemeris structure
|
||||
|
||||
@ -50,7 +50,7 @@
|
||||
#include "GPS_L5.h"
|
||||
#include "gps_l5_signal.h"
|
||||
#include "gnss_sdr_create_directory.h"
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/filesystem/path.hpp>
|
||||
#include <glog/logging.h>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <matio.h>
|
||||
@ -1091,7 +1091,7 @@ int32_t dll_pll_veml_tracking::save_matfile()
|
||||
std::ifstream dump_file;
|
||||
std::string dump_filename_ = d_dump_filename;
|
||||
// add channel number to the filename
|
||||
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
|
||||
dump_filename_.append(std::to_string(d_channel));
|
||||
// add extension
|
||||
dump_filename_.append(".dat");
|
||||
std::cout << "Generating .mat file for " << dump_filename_ << std::endl;
|
||||
@ -1335,7 +1335,7 @@ void dll_pll_veml_tracking::set_channel(uint32_t channel)
|
||||
{
|
||||
std::string dump_filename_ = d_dump_filename;
|
||||
// add channel number to the filename
|
||||
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
|
||||
dump_filename_.append(std::to_string(d_channel));
|
||||
// add extension
|
||||
dump_filename_.append(".dat");
|
||||
|
||||
|
||||
@ -48,7 +48,7 @@
|
||||
#include "GPS_L5.h"
|
||||
#include "gps_l5_signal.h"
|
||||
#include "gnss_sdr_create_directory.h"
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/filesystem/path.hpp>
|
||||
#include <glog/logging.h>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <matio.h>
|
||||
@ -999,7 +999,7 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile()
|
||||
std::ifstream dump_file;
|
||||
std::string dump_filename_ = d_dump_filename;
|
||||
// add channel number to the filename
|
||||
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
|
||||
dump_filename_.append(std::to_string(d_channel));
|
||||
// add extension
|
||||
dump_filename_.append(".dat");
|
||||
std::cout << "Generating .mat file for " << dump_filename_ << std::endl;
|
||||
@ -1227,7 +1227,7 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel)
|
||||
{
|
||||
std::string dump_filename_ = d_dump_filename;
|
||||
// add channel number to the filename
|
||||
dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
|
||||
dump_filename_.append(std::to_string(d_channel));
|
||||
// add extension
|
||||
dump_filename_.append(".dat");
|
||||
|
||||
|
||||
@ -38,6 +38,10 @@
|
||||
#define GNSS_SDR_PVT_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.
|
||||
@ -52,6 +56,18 @@ class PvtInterface : public GNSSBlockInterface
|
||||
{
|
||||
public:
|
||||
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_ */
|
||||
|
||||
@ -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_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_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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -35,6 +35,7 @@
|
||||
#include "control_thread.h"
|
||||
#include "concurrent_queue.h"
|
||||
#include "concurrent_map.h"
|
||||
#include "pvt_interface.h"
|
||||
#include "control_message_factory.h"
|
||||
#include "file_configuration.h"
|
||||
#include "gnss_flowgraph.h"
|
||||
@ -49,6 +50,10 @@
|
||||
#include "gps_almanac.h"
|
||||
#include "glonass_gnav_ephemeris.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/chrono.hpp>
|
||||
#include <glog/logging.h>
|
||||
@ -156,6 +161,7 @@ int ControlThread::run()
|
||||
sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
|
||||
|
||||
//start the telecommand listener thread
|
||||
cmd_interface_.set_pvt(flowgraph_->get_pvt());
|
||||
cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);
|
||||
|
||||
|
||||
@ -693,6 +699,10 @@ void ControlThread::process_control_messages()
|
||||
}
|
||||
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);
|
||||
}
|
||||
processed_control_messages_++;
|
||||
@ -704,25 +714,179 @@ void ControlThread::process_control_messages()
|
||||
|
||||
void ControlThread::apply_action(unsigned int what)
|
||||
{
|
||||
std::shared_ptr<PvtInterface> pvt_ptr;
|
||||
std::vector<std::pair<int, Gnss_Satellite>> visible_satellites;
|
||||
switch (what)
|
||||
{
|
||||
case 0:
|
||||
DLOG(INFO) << "Received action STOP";
|
||||
LOG(INFO) << "Received action STOP";
|
||||
stop_ = true;
|
||||
applied_actions_++;
|
||||
break;
|
||||
case 1:
|
||||
DLOG(INFO) << "Received action RESTART";
|
||||
LOG(INFO) << "Received action RESTART";
|
||||
stop_ = true;
|
||||
restart_ = true;
|
||||
applied_actions_++;
|
||||
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:
|
||||
DLOG(INFO) << "Unrecognized action.";
|
||||
LOG(INFO) << "Unrecognized action.";
|
||||
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()
|
||||
{
|
||||
|
||||
@ -35,16 +35,17 @@
|
||||
#ifndef GNSS_SDR_CONTROL_THREAD_H_
|
||||
#define GNSS_SDR_CONTROL_THREAD_H_
|
||||
|
||||
#include "gnss_satellite.h"
|
||||
#include "control_message_factory.h"
|
||||
#include "gnss_sdr_supl_client.h"
|
||||
#include "tcp_cmd_interface.h"
|
||||
#include "gnss_flowgraph.h"
|
||||
#include "configuration_interface.h"
|
||||
#include <boost/thread.hpp>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
class GNSSFlowgraph;
|
||||
class ConfigurationInterface;
|
||||
#include <armadillo>
|
||||
|
||||
|
||||
/*!
|
||||
@ -143,6 +144,12 @@ private:
|
||||
*/
|
||||
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
|
||||
*/
|
||||
|
||||
@ -1103,11 +1103,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
acq_channels_count_ = 0; //all channels are in stanby now
|
||||
break;
|
||||
case 11: //request coldstart mode
|
||||
LOG(INFO) << "TC request 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
|
||||
|
||||
LOG(INFO) << "TC request flowgraph coldstart";
|
||||
//start again the satellite acquisitions
|
||||
for (unsigned int i = 0; i < channels_count_; i++)
|
||||
{
|
||||
@ -1136,11 +1132,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
}
|
||||
break;
|
||||
case 12: //request hotstart mode
|
||||
LOG(INFO) << "TC request 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
|
||||
LOG(INFO) << "TC request flowgraph hotstart";
|
||||
for (unsigned int i = 0; i < channels_count_; i++)
|
||||
{
|
||||
unsigned int ch_index = (who + i + 1) % channels_count_;
|
||||
@ -1168,13 +1160,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
}
|
||||
break;
|
||||
case 13: //request warmstart mode
|
||||
LOG(INFO) << "TC request 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
|
||||
|
||||
LOG(INFO) << "TC request flowgraph warmstart";
|
||||
//start again the satellite acquisitions
|
||||
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)
|
||||
{
|
||||
if (running_)
|
||||
|
||||
@ -41,6 +41,11 @@
|
||||
#include "gnss_signal.h"
|
||||
#include "gnss_sdr_sample_counter.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/msg_queue.h>
|
||||
#include <list>
|
||||
@ -55,10 +60,6 @@
|
||||
#include "gnss_sdr_fpga_sample_counter.h"
|
||||
#endif
|
||||
|
||||
class GNSSBlockInterface;
|
||||
class ChannelInterface;
|
||||
class ConfigurationInterface;
|
||||
class GNSSBlockFactory;
|
||||
|
||||
/*! \brief This class represents a GNSS flow graph.
|
||||
*
|
||||
@ -129,6 +130,19 @@ public:
|
||||
*/
|
||||
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:
|
||||
void init(); // Populates the SV PRN list available for acquisition and tracking
|
||||
void set_signals_list();
|
||||
|
||||
@ -32,8 +32,22 @@
|
||||
#include "tcp_cmd_interface.h"
|
||||
#include "control_message_factory.h"
|
||||
#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 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 response;
|
||||
std::stringstream str_stream;
|
||||
//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 response;
|
||||
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS
|
||||
//todo: store it in a member variable
|
||||
if (commandLine.size() > 5)
|
||||
{
|
||||
//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());
|
||||
if (control_queue_ != nullptr)
|
||||
{
|
||||
@ -90,14 +162,37 @@ std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLin
|
||||
{
|
||||
response = "ERROR\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n";
|
||||
}
|
||||
return response;
|
||||
}
|
||||
|
||||
std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLine)
|
||||
{
|
||||
std::string response;
|
||||
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS
|
||||
//todo: store it in a member variable
|
||||
if (commandLine.size() > 5)
|
||||
{
|
||||
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());
|
||||
if (control_queue_ != nullptr)
|
||||
{
|
||||
@ -108,6 +203,12 @@ std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLi
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
@ -154,6 +255,10 @@ TcpCmdInterface::TcpCmdInterface()
|
||||
register_functions();
|
||||
keep_running_ = true;
|
||||
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);
|
||||
}
|
||||
}
|
||||
catch (const std::bad_function_call &ex)
|
||||
{
|
||||
response = "ERROR: command not found \n ";
|
||||
}
|
||||
catch (const std::exception &ex)
|
||||
{
|
||||
response = "ERROR: command execution error: " + std::string(ex.what()) + "\n";
|
||||
|
||||
@ -31,6 +31,7 @@
|
||||
#ifndef GNSS_SDR_TCPCMDINTERFACE_H_
|
||||
#define GNSS_SDR_TCPCMDINTERFACE_H_
|
||||
|
||||
#include "pvt_interface.h"
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
@ -42,7 +43,8 @@
|
||||
#include <cstdint>
|
||||
#include <gnuradio/message.h>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
|
||||
#include <armadillo>
|
||||
#include <time.h>
|
||||
|
||||
class TcpCmdInterface
|
||||
{
|
||||
@ -51,7 +53,16 @@ public:
|
||||
virtual ~TcpCmdInterface();
|
||||
void run_cmd_server(int tcp_port);
|
||||
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:
|
||||
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_;
|
||||
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_ */
|
||||
|
||||
@ -37,6 +37,7 @@ Gps_Almanac::Gps_Almanac()
|
||||
i_satellite_PRN = 0U;
|
||||
d_Delta_i = 0.0;
|
||||
d_Toa = 0.0;
|
||||
i_WNa = 0;
|
||||
d_M_0 = 0.0;
|
||||
d_e_eccentricity = 0.0;
|
||||
d_sqrt_A = 0.0;
|
||||
@ -44,6 +45,7 @@ Gps_Almanac::Gps_Almanac()
|
||||
d_OMEGA = 0.0;
|
||||
d_OMEGA_DOT = 0.0;
|
||||
i_SV_health = 0;
|
||||
i_AS_status = 0;
|
||||
d_A_f0 = 0.0;
|
||||
d_A_f1 = 0.0;
|
||||
}
|
||||
|
||||
@ -44,15 +44,17 @@ class Gps_Almanac
|
||||
{
|
||||
public:
|
||||
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]
|
||||
int32_t i_WNa; //!< Almanac week number
|
||||
double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles]
|
||||
double d_e_eccentricity; //!< Eccentricity [dimensionless]
|
||||
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_OMEGA; //!< Argument of Perigee [semi-cicles]
|
||||
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_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(d_Delta_i);
|
||||
ar& BOOST_SERIALIZATION_NVP(d_Toa);
|
||||
ar& BOOST_SERIALIZATION_NVP(i_WNa);
|
||||
ar& BOOST_SERIALIZATION_NVP(d_M_0);
|
||||
ar& BOOST_SERIALIZATION_NVP(d_e_eccentricity);
|
||||
ar& BOOST_SERIALIZATION_NVP(d_sqrt_A);
|
||||
@ -78,6 +81,7 @@ public:
|
||||
ar& BOOST_SERIALIZATION_NVP(d_OMEGA);
|
||||
ar& BOOST_SERIALIZATION_NVP(d_OMEGA_DOT);
|
||||
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_f1);
|
||||
}
|
||||
|
||||
@ -50,6 +50,7 @@ include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/core/system_parameters
|
||||
${CMAKE_SOURCE_DIR}/src/core/interfaces
|
||||
${CMAKE_SOURCE_DIR}/src/core/receiver
|
||||
${CMAKE_SOURCE_DIR}/src/core/monitor
|
||||
${CMAKE_SOURCE_DIR}/src/core/libs
|
||||
${CMAKE_SOURCE_DIR}/src/core/libs/supl
|
||||
${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp
|
||||
|
||||
@ -18,7 +18,6 @@
|
||||
|
||||
|
||||
set(SYSTEM_TESTING_LIB_SOURCES
|
||||
geofunctions.cc
|
||||
spirent_motion_csv_dump_reader.cc
|
||||
rtklib_solver_dump_reader.cc
|
||||
)
|
||||
|
||||
@ -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(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_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
|
||||
|
||||
@ -157,7 +157,7 @@ void NmeaPrinterTest::conf()
|
||||
TEST_F(NmeaPrinterTest, PrintLine)
|
||||
{
|
||||
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::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)
|
||||
{
|
||||
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::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc
|
||||
|
||||
@ -330,9 +330,10 @@ TEST(RTKLibSolverTest, test1)
|
||||
int nchannels = 8;
|
||||
std::string dump_filename = ".rtklib_solver_dump.dat";
|
||||
bool flag_dump_to_file = false;
|
||||
bool save_to_mat = false;
|
||||
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);
|
||||
|
||||
// load ephemeris
|
||||
|
||||
Loading…
Reference in New Issue
Block a user