From 29b0212c40dd7440252fdcbe2734774ba900b976 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 30 Oct 2018 21:09:34 +0100 Subject: [PATCH 01/26] Uniformize Acqusition dump basename to ./acquisition_XXX.mat --- .../adapters/galileo_e1_pcps_ambiguous_acquisition.cc | 2 +- .../adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc | 2 +- .../acquisition/adapters/galileo_e5a_pcps_acquisition.cc | 2 +- .../acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc | 2 +- .../acquisition/adapters/gps_l1_ca_pcps_acquisition.cc | 2 +- .../adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc | 2 +- .../acquisition/adapters/gps_l2_m_pcps_acquisition.cc | 2 +- .../acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc | 2 +- src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc | 2 +- .../acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc index 74c2db439..7efab9681 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc index deb7e77b5..041039147 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition_fpga.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc index 623159d17..2bd081e3e 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc index 710bd5237..30f2195b9 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc index e1b4857ad..30d7aa42d 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc index 38bc04716..dd69ed24f 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc @@ -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(); diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc index b10104bd7..32b426777 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc index fc988724f..db53d3815 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition_fpga.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc index 13ac34ec5..36d6e4f7a 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc @@ -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; diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc index d03663d79..124999dc9 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc @@ -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; From 39a062ca008370ba33a6780dd96aa179afba25d4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 14:56:59 +0100 Subject: [PATCH 02/26] Save PVT dump also in .mat. New parameter dump_mat can disable this feature --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 1 + .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 3 +- .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 1 + src/algorithms/PVT/libs/pvt_conf.cc | 1 + src/algorithms/PVT/libs/pvt_conf.h | 1 + src/algorithms/PVT/libs/rtklib_solver.cc | 367 ++++++++++++++++-- src/algorithms/PVT/libs/rtklib_solver.h | 4 +- .../pvt/nmea_printer_test.cc | 4 +- .../pvt/rtklib_solver_test.cc | 3 +- 9 files changed, 347 insertions(+), 38 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 65152bf8d..1fdaca521 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -64,6 +64,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); diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 34fa61800..970850059 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -249,6 +249,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, d_output_rate_ms = conf_.output_rate_ms; d_display_rate_ms = conf_.display_rate_ms; d_dump = conf_.dump; + d_dump_mat = conf_.dump_mat and d_dump; d_dump_filename = conf_.dump_filename; std::string dump_ls_pvt_filename = conf_.dump_filename; if (d_dump) @@ -448,7 +449,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, xml_base_path = xml_base_path + boost::filesystem::path::preferred_separator; } - d_ls_pvt = std::make_shared(static_cast(nchannels), dump_ls_pvt_filename, d_dump, rtk); + d_ls_pvt = std::make_shared(static_cast(nchannels), dump_ls_pvt_filename, d_dump, d_dump_mat, rtk); d_ls_pvt->set_averaging_depth(1); d_rx_time = 0.0; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 45ba07461..38db64a1b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -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; diff --git a/src/algorithms/PVT/libs/pvt_conf.cc b/src/algorithms/PVT/libs/pvt_conf.cc index c2f21dcf4..0f528f438 100644 --- a/src/algorithms/PVT/libs/pvt_conf.cc +++ b/src/algorithms/PVT/libs/pvt_conf.cc @@ -41,6 +41,7 @@ Pvt_Conf::Pvt_Conf() rinexnav_rate_ms = 0; dump = false; + dump_mat = true; flag_nmea_tty_port = false; diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 8f47d3765..1719863e1 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -49,6 +49,7 @@ public: std::map rtcm_msg_rate_ms; bool dump; + bool dump_mat; std::string dump_filename; bool flag_nmea_tty_port; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 9159407fd..12d316196 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -56,17 +56,19 @@ #include "GPS_L1_CA.h" #include "Galileo_E1.h" #include "GLONASS_L1_L2_CA.h" +#include #include 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; @@ -84,7 +86,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); } - catch (const std::ifstream::failure& e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); } @@ -92,6 +94,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(size) / static_cast(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(&TOW_at_current_symbol_ms[i]), sizeof(uint32_t)); + dump_file.read(reinterpret_cast(&week[i]), sizeof(uint32_t)); + dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_xx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_yy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_zz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_xy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_yz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&valid_sats[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&solution_status[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&solution_type[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&AR_ratio_factor[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&AR_ratio_threshold[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&gdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&hdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&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(matfp) != NULL) + { + size_t dims[2] = {1, static_cast(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() { @@ -101,11 +398,15 @@ rtklib_solver::~rtklib_solver() { d_dump_file.close(); } - catch (const std::exception& ex) + catch (const std::exception &ex) { LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what(); } } + if (d_flag_dump_mat_enabled) + { + save_matfile(); + } } @@ -133,7 +434,7 @@ double rtklib_solver::get_vdop() const } -bool rtklib_solver::get_PVT(const std::map& gnss_observables_map, bool flag_averaging) +bool rtklib_solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -559,73 +860,73 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ uint32_t tmp_uint32; // TOW tmp_uint32 = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms; - d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // WEEK tmp_uint32 = adjgpsweek(nav_data.eph[0].week); - d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // PVT GPS time tmp_double = gnss_observables_map.begin()->second.RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] tmp_double = rx_position_and_time(3); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) tmp_double = pvt_sol.rr[0]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.rr[1]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.rr[2]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.rr[3]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.rr[4]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.rr[5]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) tmp_double = pvt_sol.qr[0]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.qr[1]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.qr[2]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.qr[3]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.qr[4]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = pvt_sol.qr[5]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Latitude [deg] tmp_double = get_latitude(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Longitude [deg] tmp_double = get_longitude(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Height [m] tmp_double = get_height(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // NUMBER OF VALID SATS - d_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + d_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); // RTKLIB solution status - d_dump_file.write(reinterpret_cast(&pvt_sol.stat), sizeof(uint8_t)); + d_dump_file.write(reinterpret_cast(&pvt_sol.stat), sizeof(uint8_t)); // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) - d_dump_file.write(reinterpret_cast(&pvt_sol.type), sizeof(uint8_t)); + d_dump_file.write(reinterpret_cast(&pvt_sol.type), sizeof(uint8_t)); // AR ratio factor for validation - d_dump_file.write(reinterpret_cast(&pvt_sol.ratio), sizeof(float)); + d_dump_file.write(reinterpret_cast(&pvt_sol.ratio), sizeof(float)); // AR ratio threshold for validation - d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); + d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); // GDOP / PDOP/ HDOP/ VDOP - d_dump_file.write(reinterpret_cast(&dop_[0]), sizeof(double)); - d_dump_file.write(reinterpret_cast(&dop_[1]), sizeof(double)); - d_dump_file.write(reinterpret_cast(&dop_[2]), sizeof(double)); - d_dump_file.write(reinterpret_cast(&dop_[3]), sizeof(double)); + d_dump_file.write(reinterpret_cast(&dop_[0]), sizeof(double)); + d_dump_file.write(reinterpret_cast(&dop_[1]), sizeof(double)); + d_dump_file.write(reinterpret_cast(&dop_[2]), sizeof(double)); + d_dump_file.write(reinterpret_cast(&dop_[3]), sizeof(double)); } - catch (const std::ifstream::failure& e) + catch (const std::ifstream::failure &e) { LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 3bd054954..b584c4826 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -77,14 +77,16 @@ 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); + 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& gnss_observables_map, bool flag_averaging); diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc index 4e5c01fae..535cca464 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc @@ -157,7 +157,7 @@ void NmeaPrinterTest::conf() TEST_F(NmeaPrinterTest, PrintLine) { std::string filename("nmea_test.nmea"); - std::shared_ptr pvt_solution = std::make_shared(12, "filename", false, rtk); + std::shared_ptr pvt_solution = std::make_shared(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 pvt_solution = std::make_shared(12, "filename", false, rtk); + std::shared_ptr pvt_solution = std::make_shared(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 diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc index 22615cc2a..fe0c81ab6 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc @@ -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 d_ls_pvt(new rtklib_solver(nchannels, dump_filename, flag_dump_to_file, rtk)); + std::unique_ptr 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 From e5122198ae4c83d7bd1688c002271550a78cca5e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 16:17:11 +0100 Subject: [PATCH 03/26] Fix error reading eccentricity from the almanac --- src/core/libs/gnss_sdr_supl_client.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/libs/gnss_sdr_supl_client.cc b/src/core/libs/gnss_sdr_supl_client.cc index dae736999..b95221856 100644 --- a/src/core/libs/gnss_sdr_supl_client.cc +++ b/src/core/libs/gnss_sdr_supl_client.cc @@ -268,7 +268,7 @@ void gnss_sdr_supl_client::read_supl_data() gps_almanac_iterator->second.d_sqrt_A = static_cast(a->A_sqrt) * pow(2.0, -11); gps_almanac_iterator->second.d_OMEGA_DOT = static_cast(a->OMEGA_dot) * pow(2.0, -38); gps_almanac_iterator->second.d_Toa = static_cast(a->toa) * pow(2.0, 12); - gps_almanac_iterator->second.d_e_eccentricity = static_cast(a->toa) * pow(2.0, -21); + gps_almanac_iterator->second.d_e_eccentricity = static_cast(a->e) * pow(2.0, -21); gps_almanac_iterator->second.d_M_0 = static_cast(a->M0) * pow(2.0, -23); } } From 0e2eb0b7417d46c840bcbce406ed08c3aabb086b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 16:18:06 +0100 Subject: [PATCH 04/26] Adding missing parameters in GPS almanac class --- src/core/system_parameters/gps_almanac.cc | 2 ++ src/core/system_parameters/gps_almanac.h | 26 +++++++++++++---------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/core/system_parameters/gps_almanac.cc b/src/core/system_parameters/gps_almanac.cc index acf7068b0..c2776dcce 100644 --- a/src/core/system_parameters/gps_almanac.cc +++ b/src/core/system_parameters/gps_almanac.cc @@ -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; } diff --git a/src/core/system_parameters/gps_almanac.h b/src/core/system_parameters/gps_almanac.h index d3628059e..57288d3db 100644 --- a/src/core/system_parameters/gps_almanac.h +++ b/src/core/system_parameters/gps_almanac.h @@ -44,17 +44,19 @@ class Gps_Almanac { public: uint32_t i_satellite_PRN; //!< SV PRN NUMBER - double d_Delta_i; - double d_Toa; //!< Almanac data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] - 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 - double d_A_f0; //!< Coefficient 0 of code phase offset model [s] - double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s] + double d_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_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] /*! * Default constructor @@ -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); } From 2643af39348d996b29a333967ecbfe7fa8ba6f4c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 16:41:09 +0100 Subject: [PATCH 05/26] Add Week Number and AS_status to GPS almanac --- docs/xml-schemas/gps_almanac_map.xsd | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/xml-schemas/gps_almanac_map.xsd b/docs/xml-schemas/gps_almanac_map.xsd index 818dc49c3..5e96b4af1 100644 --- a/docs/xml-schemas/gps_almanac_map.xsd +++ b/docs/xml-schemas/gps_almanac_map.xsd @@ -17,6 +17,7 @@ + @@ -24,6 +25,7 @@ + From 14d24c394871d43dca0890eab2e408cd444c36c0 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 20:02:16 +0100 Subject: [PATCH 06/26] Fix build if matio was built on-the-fly --- src/algorithms/PVT/libs/CMakeLists.txt | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 31eaa70f7..6807e9602 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -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} ) From f5191c52e639f6f06d9e0fd7e76e8152f2af1e63 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 20:05:39 +0100 Subject: [PATCH 07/26] Improve flag descriptions --- src/tests/system-tests/libs/position_test_flags.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/system-tests/libs/position_test_flags.h b/src/tests/system-tests/libs/position_test_flags.h index a0e3d6197..cf33c344f 100644 --- a/src/tests/system-tests/libs/position_test_flags.h +++ b/src/tests/system-tests/libs/position_test_flags.h @@ -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 From 247c8cc6f3632a526658228e6e80a1c25706d14f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 1 Nov 2018 17:50:06 +0100 Subject: [PATCH 08/26] Enable RINEX printer for more combinations of signals --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 25 +- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 132 +- src/algorithms/PVT/libs/rinex_printer.cc | 1732 ++++++++++++++++- src/algorithms/PVT/libs/rinex_printer.h | 38 +- 4 files changed, 1845 insertions(+), 82 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 1fdaca521..74b2f4c5d 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -179,24 +179,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; @@ -215,11 +211,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; - //RTKLIB PVT solver options + + 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; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 970850059..d6f044e08 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1092,7 +1092,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) { - rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); + std::string signal("2S"); + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); b_rinex_header_written = true; // do not write header anymore } @@ -1101,7 +1102,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) { - rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); + std::string signal("L5"); + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); b_rinex_header_written = true; // do not write header anymore } @@ -1139,12 +1141,22 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); + std::string signal("1C 2S"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if (type_of_rx == 8) // GPS L1 + GPS L5 + { + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + { + std::string signal("1C L5"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, signal); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); b_rinex_header_written = true; // do not write header anymore } } - if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) @@ -1175,6 +1187,17 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item b_rinex_header_written = true; // do not write header anymore } } + if (type_of_rx == 13) // L5+E5a + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + { + std::string gal_signal("5X"); + std::string gps_signal("L5"); + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gps_signal, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } if (type_of_rx == 14) // Galileo E1B + Galileo E5a { if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) @@ -1265,7 +1288,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string glo_signal("2G"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); @@ -1281,7 +1304,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 30) // Galileo E1B + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { std::string glo_signal("2G"); std::string gal_signal("1B"); @@ -1292,7 +1315,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 31) // GPS L2C + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { std::string glo_signal("2G"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); @@ -1300,6 +1323,20 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item b_rinex_header_written = true; // do not write header anymore } } + + if (type_of_rx == 32) // L1+E1+L5+E5a + { + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && + (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) && + (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) + { + std::string gal_signal("1B 5X"); + std::string gps_signal("1C L5"); + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gps_signal, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } } @@ -1327,10 +1364,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); } + if (type_of_rx == 8) // L1+L5 + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); + } if ((type_of_rx == 9) or (type_of_rx == 10) or (type_of_rx == 11)) // GPS L1 C/A + Galileo { rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); } + if (type_of_rx == 13) // L5+E5a + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + } if ((type_of_rx == 14) or (type_of_rx == 15)) // Galileo E1B + Galileo E5a { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); @@ -1375,6 +1420,10 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_cnav_ephemeris_map, d_ls_pvt->glonass_gnav_ephemeris_map); } + if (type_of_rx == 32) // L1+E1+L5+E5a + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + } } galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); @@ -1475,6 +1524,27 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item b_rinex_header_updated = true; } } + if (type_of_rx == 8) // L1+L5 + { + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated and ((d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) or (d_ls_pvt->gps_utc_model.d_A0 != 0))) + { + if (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); + } + else + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); + } + b_rinex_header_updated = true; + } + } if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) @@ -1488,6 +1558,19 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item b_rinex_header_updated = true; } } + if (type_of_rx == 13) // L5+E5a + { + if ((gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + { + rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; // do not write header anymore + } + } if (type_of_rx == 14) // Galileo E1B + Galileo E5a { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) @@ -1611,7 +1694,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -1620,17 +1703,38 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 31) // GPS L2C + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); b_rinex_header_updated = true; // do not write header anymore } } + if (type_of_rx == 32) // L1+E1+L5+E5a + { + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated and ((d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) or (d_ls_pvt->gps_utc_model.d_A0 != 0)) and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + if (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model); + } + else + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model); + } + b_rinex_header_updated = true; // do not write header anymore + } + } } } } @@ -1693,7 +1797,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_ephemeris_iter; gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2086,7 +2190,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2139,12 +2243,12 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item i++; } - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() and (d_rtcm_MT1077_rate_ms != 0)) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0)) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() and (d_rtcm_MT1097_rate_ms != 0)) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 8aec5b3de..67fb12ea3 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -517,6 +517,7 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Glonass_Gnav_Utc_M out << line << std::endl; } + void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { if (glonass_gnav_almanac.i_satellite_freq_channel) @@ -1101,6 +1102,138 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_CNAV_Iono& ion } +void Rinex_Printer::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) +{ + std::string line; + + // -------- Line 1 + line = std::string(5, ' '); + line += stringVersion; + line += std::string(11, ' '); + line += std::string("N: GNSS NAV DATA"); + line += std::string(4, ' '); + line += std::string("M: MIXED"); + line += std::string(12, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("GNSS NAVIGATION MESSAGE FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See https://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line ionospheric info 1 + line.clear(); + line += std::string("GAL "); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai0_5, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai1_5, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai2_5, 10, 2), 12); + double zero = 0.0; + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(zero, 10, 2), 12); + line += std::string(7, ' '); + line += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line ionospheric info 2 + line.clear(); + line += std::string("GPSA"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha0, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha1, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha2, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha3, 10, 2), 12); + line += std::string(7, ' '); + line += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line ionospheric info 3 + line.clear(); + line += std::string("GPSB"); + line += std::string(1, ' '); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta0, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta1, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta2, 10, 2), 12); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta3, 10, 2), 12); + line += std::string(7, ' '); + line += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction + line.clear(); + line += std::string("GAUT"); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A0_6, 16, 2), 18); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A1_6, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.t0t_6), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.WNot_6), 5); + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line system time correction 2 + line.clear(); + line += std::string("GPUT"); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A0, 16, 2), 18); + line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A1, 15, 2), 16); + line += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.d_t_OT), 7); + line += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.i_WN_T + 1024), 5); // valid until 2019 + line += std::string(10, ' '); + line += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 6 leap seconds + // For leap second information, see http://www.endruntechnologies.com/leap.htm + line.clear(); + line += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.d_DeltaT_LS), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.d_DeltaT_LSF), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.i_WN_LSF), 6); + line += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.i_DN), 6); + line += std::string(36, ' '); + line += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- End of Header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& iono, const Gps_Utc_Model& utc_model) { std::string line; @@ -1933,6 +2066,135 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_CNAV_Utc_Mode } +void Rinex_Printer::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) +{ + std::vector data; + std::string line_aux; + + int64_t pos = out.tellp(); + out.seekp(0); + data.clear(); + + bool no_more_finds = false; + std::string line_str; + + while (!out.eof()) + { + std::getline(out, line_str); + + if (!no_more_finds) + { + line_aux.clear(); + if ((line_str.find("GAL", 0) != std::string::npos) && (line_str.find("IONOSPHERIC CORR", 59) != std::string::npos)) + { + line_aux += std::string("GAL "); + line_aux += std::string(1, ' '); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai0_5, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai1_5, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_iono.ai2_5, 10, 2), 12); + double zero = 0.0; + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(zero, 10, 2), 12); + line_aux += std::string(7, ' '); + line_aux += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GPSA", 0) != std::string::npos) && (line_str.find("IONOSPHERIC CORR", 59) != std::string::npos)) + { + line_aux += std::string("GPSA"); + line_aux += std::string(1, ' '); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha0, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha1, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha2, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_alpha3, 10, 2), 12); + line_aux += std::string(7, ' '); + line_aux += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GPSB", 0) != std::string::npos) && (line_str.find("IONOSPHERIC CORR", 59) != std::string::npos)) + { + line_aux += std::string("GPSB"); + line_aux += std::string(1, ' '); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta0, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta1, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta2, 10, 2), 12); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(iono.d_beta3, 10, 2), 12); + line_aux += std::string(7, ' '); + line_aux += Rinex_Printer::leftJustify("IONOSPHERIC CORR", 20); + data.push_back(line_aux); + } + + else if ((line_str.find("GAUT", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GAUT"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A0_6, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A1_6, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.t0t_6), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.WNot_6), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if ((line_str.find("GPGA", 0) != std::string::npos) && (line_str.find("TIME SYSTEM CORR", 59) != std::string::npos)) + { + line_aux += std::string("GPGA"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A_0G_10, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(galileo_utc_model.A_1G_10, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.t_0G_10), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(galileo_utc_model.WN_0G_10), 5); + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if (line_str.find("GPUT", 0) != std::string::npos) + { + line_aux += std::string("GPUT"); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A0, 16, 2), 18); + line_aux += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A1, 15, 2), 16); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.d_t_OT), 7); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.i_WN_T + 1024), 5); // valid until 2019 + line_aux += std::string(10, ' '); + line_aux += Rinex_Printer::leftJustify("TIME SYSTEM CORR", 20); + data.push_back(line_aux); + } + else if (line_str.find("LEAP SECONDS", 59) != std::string::npos) + { + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.d_DeltaT_LS), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.d_DeltaT_LSF), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.i_WN_LSF), 6); + line_aux += Rinex_Printer::rightJustify(boost::lexical_cast(utc_model.i_DN), 6); + line_aux += std::string(36, ' '); + line_aux += Rinex_Printer::leftJustify("LEAP SECONDS", 20); + data.push_back(line_aux); + } + else if (line_str.find("END OF HEADER", 59) != std::string::npos) + { + data.push_back(line_str); + no_more_finds = true; + } + else + { + data.push_back(line_str); + } + } + else + { + data.push_back(line_str); + } + } + out.close(); + out.open(navfilename, std::ios::out | std::ios::trunc); + out.seekp(0); + for (int32_t i = 0; i < static_cast(data.size()) - 1; i++) + { + out << data[i] << std::endl; + } + out.close(); + out.open(navfilename, std::ios::out | std::ios::app); + out.seekp(pos); + std::cout << "The RINEX Navigation file header has been updated with UTC and IONO info." << std::endl; +} + + void Rinex_Printer::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) { std::vector data; @@ -3235,6 +3497,15 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& gps_cnav_eph_map, const std::map& galileo_eph_map) +{ + version = 3; + stringVersion = "3.02"; + Rinex_Printer::log_rinex_nav(out, gps_cnav_eph_map); + Rinex_Printer::log_rinex_nav(out, galileo_eph_map); +} + + void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map& gps_eph_map, const std::map& glonass_gnav_eph_map) { Rinex_Printer::log_rinex_nav(out, gps_eph_map); @@ -4836,7 +5107,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph } -void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const double d_TOW_first_observation) +void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const double d_TOW_first_observation, const std::string gps_bands) { std::string line; @@ -4974,30 +5245,66 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris // -------- SYS / OBS TYPES // one line per available system line.clear(); + uint32_t number_of_observations_gps = 0; + std::string signal_("2S"); + std::size_t found_2S = gps_bands.find(signal_); + if (found_2S != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + signal_ = "L5"; + std::size_t found_L5 = gps_bands.find(signal_); + if (found_L5 != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } line += satelliteSystem["GPS"]; line += std::string(2, ' '); std::stringstream strm; - numberTypesObservations = 4; + numberTypesObservations = number_of_observations_gps; strm << numberTypesObservations; line += Rinex_Printer::rightJustify(strm.str(), 3); // per type of observation - // GPS L2 PSEUDORANGE - line += std::string(1, ' '); - line += observationType["PSEUDORANGE"]; - line += observationCode["GPS_L2_L2CM"]; - // GPS L2 PHASE - line += std::string(1, ' '); - line += observationType["CARRIER_PHASE"]; - line += observationCode["GPS_L2_L2CM"]; - // GPS DOPPLER L2 - line += std::string(1, ' '); - line += observationType["DOPPLER"]; - line += observationCode["GPS_L2_L2CM"]; - // GPS L2 SIGNAL STRENGTH - line += std::string(1, ' '); - line += observationType["SIGNAL_STRENGTH"]; - line += observationCode["GPS_L2_L2CM"]; + if (found_2S != std::string::npos) + { + // GPS L2 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS DOPPLER L2 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L2_L2CM"]; + } + + if (found_L5 != std::string::npos) + { + // GPS L5 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L5_Q"]; + // GPS DOPPLER L5 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L5_Q"]; + } line += std::string(60 - line.size(), ' '); line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); Rinex_Printer::lengthCheck(line); @@ -5045,7 +5352,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris } -void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph, const Gps_CNAV_Ephemeris& eph_cnav, const double d_TOW_first_observation) +void Rinex_Printer::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) { if (eph_cnav.d_i_0) { @@ -5186,46 +5493,90 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph // -------- SYS / OBS TYPES // one line per available system line.clear(); + uint32_t number_of_observations_gps = 0; + std::string signal_("1C"); + std::size_t found_1C = gps_bands.find(signal_); + if (found_1C != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + signal_ = "2S"; + std::size_t found_2S = gps_bands.find(signal_); + if (found_2S != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + signal_ = "L5"; + std::size_t found_L5 = gps_bands.find(signal_); + if (found_L5 != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } line += satelliteSystem["GPS"]; line += std::string(2, ' '); std::stringstream strm; - numberTypesObservations = 8; + numberTypesObservations = number_of_observations_gps; strm << numberTypesObservations; line += Rinex_Printer::rightJustify(strm.str(), 3); // per type of observation - // GPS L1 PSEUDORANGE - line += std::string(1, ' '); - line += observationType["PSEUDORANGE"]; - line += observationCode["GPS_L1_CA"]; - // GPS L1 PHASE - line += std::string(1, ' '); - line += observationType["CARRIER_PHASE"]; - line += observationCode["GPS_L1_CA"]; - // GPS DOPPLER L1 - line += std::string(1, ' '); - line += observationType["DOPPLER"]; - line += observationCode["GPS_L1_CA"]; - // GPS L1 CA SIGNAL STRENGTH - line += std::string(1, ' '); - line += observationType["SIGNAL_STRENGTH"]; - line += observationCode["GPS_L1_CA"]; - // GPS L2 PSEUDORANGE - line += std::string(1, ' '); - line += observationType["PSEUDORANGE"]; - line += observationCode["GPS_L2_L2CM"]; - // GPS L2 PHASE - line += std::string(1, ' '); - line += observationType["CARRIER_PHASE"]; - line += observationCode["GPS_L2_L2CM"]; - // GPS DOPPLER L2 - line += std::string(1, ' '); - line += observationType["DOPPLER"]; - line += observationCode["GPS_L2_L2CM"]; - // GPS L2 SIGNAL STRENGTH - line += std::string(1, ' '); - line += observationType["SIGNAL_STRENGTH"]; - line += observationCode["GPS_L2_L2CM"]; + if (found_1C != std::string::npos) + { + // GPS L1 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L1_CA"]; + // GPS DOPPLER L1 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 CA SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L1_CA"]; + } + if (found_2S != std::string::npos) + { + // GPS L2 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS DOPPLER L2 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L2_L2CM"]; + } + if (found_L5 != std::string::npos) + { + // GPS L5 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L5_Q"]; + // GPS DOPPLER L5 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L5_Q"]; + } line += std::string(60 - line.size(), ' '); line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); Rinex_Printer::lengthCheck(line); @@ -5273,6 +5624,661 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& eph } +void Rinex_Printer::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, const std::string galileo_bands) +{ + std::string line; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += "3.02"; + line += std::string(11, ' '); + line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); + line += satelliteSystem["Mixed"]; + line += std::string(19, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::leftJustify("G = GPS R = GLONASS E = GALILEO S = GEO M = MIXED", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 3 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("MIXED (GPS/GALILEO) OBSERVATION DATA FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See https://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER NAME + line.clear(); + line += Rinex_Printer::leftJustify("DEFAULT MARKER NAME", 60); // put a flag or a property, + line += Rinex_Printer::leftJustify("MARKER NAME", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER TYPE + //line.clear(); + //line += Rinex_Printer::leftJustify("NON_GEODETIC", 20); // put a flag or a property + //line += std::string(40, ' '); + //line += Rinex_Printer::leftJustify("MARKER TYPE", 20); + //Rinex_Printer::lengthCheck(line); + //out << line << std::endl; + + // -------- Line OBSERVER / AGENCY + line.clear(); + std::string username; + char c_username[20] = {0}; + int32_t nGet = getlogin_r(c_username, sizeof(c_username) - 1); + if (nGet == 0) + { + username = c_username; + } + else + { + username = "UNKNOWN USER"; + } + line += leftJustify(username, 20); + line += Rinex_Printer::leftJustify("CTTC", 40); // add flag and property + line += Rinex_Printer::leftJustify("OBSERVER / AGENCY", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line REC / TYPE VERS + line.clear(); + line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property + line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property + //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property + if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); + line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); + line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); + lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA TYPE + line.clear(); + line += Rinex_Printer::leftJustify("Antenna number", 20); // add flag and property + line += Rinex_Printer::leftJustify("Antenna type", 20); // add flag and property + line += std::string(20, ' '); + line += Rinex_Printer::leftJustify("ANT # / TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- APPROX POSITION (optional for moving platforms) + // put here real data! + double antena_x = 0.0; + double antena_y = 0.0; + double antena_z = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_x, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_y, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_z, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("APPROX POSITION XYZ", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA: DELTA H/E/N + // put here real data! + double antena_h = 0.0; + double antena_e = 0.0; + double antena_n = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_h, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_e, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_n, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("ANTENNA: DELTA H/E/N", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- SYS / OBS TYPES + // one line per available system + line.clear(); + uint32_t number_of_observations_gps = 0; + std::string signal_("1C"); + std::size_t found_1C = gps_bands.find(signal_); + if (found_1C != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + signal_ = "2S"; + std::size_t found_2S = gps_bands.find(signal_); + if (found_2S != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + signal_ = "L5"; + std::size_t found_L5 = gps_bands.find(signal_); + if (found_L5 != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + line += satelliteSystem["GPS"]; + line += std::string(2, ' '); + std::stringstream strm; + numberTypesObservations = number_of_observations_gps; + strm << numberTypesObservations; + line += Rinex_Printer::rightJustify(strm.str(), 3); + // per type of observation + if (found_1C != std::string::npos) + { + // GPS L1 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L1_CA"]; + // GPS DOPPLER L1 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 CA SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L1_CA"]; + } + if (found_2S != std::string::npos) + { + // GPS L2 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS DOPPLER L2 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L2_L2CM"]; + } + if (found_L5 != std::string::npos) + { + // GPS L5 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L5_Q"]; + // GPS DOPPLER L5 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L5_Q"]; + } + line += std::string(60 - line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + line.clear(); + uint32_t number_of_observations_gal = 0; + signal_ = "1B"; + std::size_t found_1B = galileo_bands.find(signal_); + if (found_1B != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + signal_ = "5X"; + std::size_t found_5X = galileo_bands.find(signal_); + if (found_5X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + signal_ = "7X"; + std::size_t found_7X = galileo_bands.find(signal_); + if (found_7X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + line += satelliteSystem["Galileo"]; + line += std::string(2, ' '); + line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_gal), 3); + if (found_1B != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E1_B"]; + } + if (found_5X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5a_IQ"]; + } + if (found_7X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5b_IQ"]; + } + line += std::string(60 - line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Signal Strength units + line.clear(); + line += Rinex_Printer::leftJustify("DBHZ", 20); + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("SIGNAL STRENGTH UNIT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- TIME OF FIRST OBS + line.clear(); + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(gps_eph, d_TOW_first_observation); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + std::string year(timestring, 0, 4); + std::string month(timestring, 4, 2); + std::string day(timestring, 6, 2); + std::string hour(timestring, 9, 2); + std::string minutes(timestring, 11, 2); + double gps_t = d_TOW_first_observation; + double seconds = fmod(gps_t, 60); + line += Rinex_Printer::rightJustify(year, 6); + line += Rinex_Printer::rightJustify(month, 6); + line += Rinex_Printer::rightJustify(day, 6); + line += Rinex_Printer::rightJustify(hour, 6); + line += Rinex_Printer::rightJustify(minutes, 6); + line += Rinex_Printer::rightJustify(asString(seconds, 7), 13); + line += Rinex_Printer::rightJustify(std::string("GPS"), 8); + line += std::string(9, ' '); + line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- end of header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + +void Rinex_Printer::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, const std::string galileo_bands) +{ + std::string line; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += "3.02"; + line += std::string(11, ' '); + line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); + line += satelliteSystem["Mixed"]; + line += std::string(19, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::leftJustify("G = GPS R = GLONASS E = GALILEO S = GEO M = MIXED", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 3 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("MIXED (GPS/GALILEO) OBSERVATION DATA FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See https://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER NAME + line.clear(); + line += Rinex_Printer::leftJustify("DEFAULT MARKER NAME", 60); // put a flag or a property, + line += Rinex_Printer::leftJustify("MARKER NAME", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER TYPE + //line.clear(); + //line += Rinex_Printer::leftJustify("NON_GEODETIC", 20); // put a flag or a property + //line += std::string(40, ' '); + //line += Rinex_Printer::leftJustify("MARKER TYPE", 20); + //Rinex_Printer::lengthCheck(line); + //out << line << std::endl; + + // -------- Line OBSERVER / AGENCY + line.clear(); + std::string username; + char c_username[20] = {0}; + int32_t nGet = getlogin_r(c_username, sizeof(c_username) - 1); + if (nGet == 0) + { + username = c_username; + } + else + { + username = "UNKNOWN USER"; + } + line += leftJustify(username, 20); + line += Rinex_Printer::leftJustify("CTTC", 40); // add flag and property + line += Rinex_Printer::leftJustify("OBSERVER / AGENCY", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line REC / TYPE VERS + line.clear(); + line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property + line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property + //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property + if (gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); + line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); + line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); + lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA TYPE + line.clear(); + line += Rinex_Printer::leftJustify("Antenna number", 20); // add flag and property + line += Rinex_Printer::leftJustify("Antenna type", 20); // add flag and property + line += std::string(20, ' '); + line += Rinex_Printer::leftJustify("ANT # / TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- APPROX POSITION (optional for moving platforms) + // put here real data! + double antena_x = 0.0; + double antena_y = 0.0; + double antena_z = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_x, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_y, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_z, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("APPROX POSITION XYZ", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA: DELTA H/E/N + // put here real data! + double antena_h = 0.0; + double antena_e = 0.0; + double antena_n = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_h, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_e, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_n, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("ANTENNA: DELTA H/E/N", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- SYS / OBS TYPES + // one line per available system + line.clear(); + uint32_t number_of_observations_gps = 0; + std::string signal_("2S"); + std::size_t found_2S = gps_bands.find(signal_); + if (found_2S != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + signal_ = "L5"; + std::size_t found_L5 = gps_bands.find(signal_); + if (found_L5 != std::string::npos) + { + number_of_observations_gps = number_of_observations_gps + 4; + } + line += satelliteSystem["GPS"]; + line += std::string(2, ' '); + std::stringstream strm; + numberTypesObservations = number_of_observations_gps; + strm << numberTypesObservations; + line += Rinex_Printer::rightJustify(strm.str(), 3); + // per type of observation + if (found_2S != std::string::npos) + { + // GPS L2 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS DOPPLER L2 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L2_L2CM"]; + // GPS L2 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L2_L2CM"]; + } + if (found_L5 != std::string::npos) + { + // GPS L5 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L5_Q"]; + // GPS DOPPLER L5 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L5_Q"]; + // GPS L5 SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L5_Q"]; + } + line += std::string(60 - line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + line.clear(); + uint32_t number_of_observations_gal = 0; + signal_ = "1B"; + std::size_t found_1B = galileo_bands.find(signal_); + if (found_1B != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + signal_ = "5X"; + std::size_t found_5X = galileo_bands.find(signal_); + if (found_5X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + signal_ = "7X"; + std::size_t found_7X = galileo_bands.find(signal_); + if (found_7X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + line += satelliteSystem["Galileo"]; + line += std::string(2, ' '); + line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_gal), 3); + if (found_1B != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E1_B"]; + } + if (found_5X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5a_IQ"]; + } + if (found_7X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5b_IQ"]; + } + line += std::string(60 - line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Signal Strength units + line.clear(); + line += Rinex_Printer::leftJustify("DBHZ", 20); + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("SIGNAL STRENGTH UNIT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- TIME OF FIRST OBS + line.clear(); + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph_cnav, d_TOW_first_observation); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + std::string year(timestring, 0, 4); + std::string month(timestring, 4, 2); + std::string day(timestring, 6, 2); + std::string hour(timestring, 9, 2); + std::string minutes(timestring, 11, 2); + double gps_t = d_TOW_first_observation; + double seconds = fmod(gps_t, 60); + line += Rinex_Printer::rightJustify(year, 6); + line += Rinex_Printer::rightJustify(month, 6); + line += Rinex_Printer::rightJustify(day, 6); + line += Rinex_Printer::rightJustify(hour, 6); + line += Rinex_Printer::rightJustify(minutes, 6); + line += Rinex_Printer::rightJustify(asString(seconds, 7), 13); + line += Rinex_Printer::rightJustify(std::string("GPS"), 8); + line += std::string(9, ' '); + line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- end of header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} + + void Rinex_Printer::rinex_obs_header(std::fstream& out, const Galileo_Ephemeris& eph, const double d_TOW_first_observation, const std::string bands) { std::string line; @@ -7482,6 +8488,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c //Get maps with GPS L1 and L2 observations std::map observablesL1; std::map observablesL2; + std::map observablesL5; std::map::const_iterator observables_iter; std::multimap total_mmap; @@ -7508,6 +8515,18 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c } total_mmap.insert(std::pair(observables_iter->second.PRN, observables_iter->second)); } + + if ((system_.compare("G") == 0) && (sig_.compare("L5") == 0)) + { + observablesL5.insert(std::pair(observables_iter->first, observables_iter->second)); + mmap_iter = total_mmap.find(observables_iter->second.PRN); + if (mmap_iter == total_mmap.end()) + { + Gnss_Synchro gs = Gnss_Synchro(); + total_mmap.insert(std::pair(observables_iter->second.PRN, gs)); + } + total_mmap.insert(std::pair(observables_iter->second.PRN, observables_iter->second)); + } } // Fill with zeros satellites with L1 obs but not L2 @@ -7555,6 +8574,18 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c } } + for (observables_iter = observablesL5.cbegin(); + observables_iter != observablesL5.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + it = available_prns.find(prn_); + if (it == available_prns.end()) + { + available_prns.insert(prn_); + } + } + int32_t numSatellitesObserved = available_prns.size(); line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); // Receiver clock offset (optional) @@ -8121,6 +9152,603 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep } +void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const Galileo_Ephemeris& galileo_eph, double gps_obs_time, const std::map& observables) +{ + if (galileo_eph.e_1) + { + } // avoid warning, not needed + std::string line; + + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph, gps_obs_time); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); + //double gps_t = eph.sv_clock_correction(obs_time); + double gps_t = gps_obs_time; + + std::string month(timestring, 4, 2); + std::string day(timestring, 6, 2); + std::string hour(timestring, 9, 2); + std::string minutes(timestring, 11, 2); + + std::string year(timestring, 0, 4); + line += std::string(1, '>'); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + line += month; + line += std::string(1, ' '); + line += day; + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + + line += std::string(1, ' '); + double seconds = fmod(gps_t, 60); + // Add extra 0 if seconds are < 10 + if (seconds < 10) + { + line += std::string(1, '0'); + } + line += Rinex_Printer::asString(seconds, 7); + line += std::string(2, ' '); + // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event + line += std::string(1, '0'); + + //Number of satellites observed in current epoch + + //Get maps with observations + std::map observablesG2S; + std::map observablesGL5; + std::map observablesE1B; + std::map observablesE5A; + std::map observablesE5B; + std::map::const_iterator observables_iter; + + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if ((system_.compare("E") == 0) && (sig_.compare("1B") == 0)) + { + observablesE1B.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("E") == 0) && (sig_.compare("5X") == 0)) + { + observablesE5A.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("E") == 0) && (sig_.compare("7X") == 0)) + { + observablesE5B.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("G") == 0) && (sig_.compare("2S") == 0)) + { + observablesG2S.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("G") == 0) && (sig_.compare("L5") == 0)) + { + observablesGL5.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + std::multimap total_gps_map; + std::multimap total_gal_map; + std::set available_gal_prns; + std::set available_gps_prns; + std::set::iterator it; + for (observables_iter = observablesE1B.cbegin(); + observables_iter != observablesE1B.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gal_prns.find(prn_); + if (it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for (observables_iter = observablesE5A.cbegin(); + observables_iter != observablesE5A.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gal_prns.find(prn_); + if (it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for (observables_iter = observablesE5B.cbegin(); + observables_iter != observablesE5B.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gal_prns.find(prn_); + if (it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for (observables_iter = observablesG2S.cbegin(); + observables_iter != observablesG2S.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gps_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gps_prns.find(prn_); + if (it == available_gps_prns.end()) + { + available_gps_prns.insert(prn_); + } + } + + for (observables_iter = observablesGL5.cbegin(); + observables_iter != observablesGL5.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gps_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gps_prns.find(prn_); + if (it == available_gps_prns.end()) + { + available_gps_prns.insert(prn_); + } + } + + int32_t numGalSatellitesObserved = available_gal_prns.size(); + int32_t numGpsSatellitesObserved = available_gps_prns.size(); + int32_t numSatellitesObserved = numGalSatellitesObserved + numGpsSatellitesObserved; + line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); + + // Receiver clock offset (optional) + //line += rightJustify(asString(clockOffset, 12), 15); + + line += std::string(80 - line.size(), ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + std::string s; + std::string lineObs; + + std::pair::iterator, std::multimap::iterator> ret; + for (it = available_gps_prns.begin(); + it != available_gps_prns.end(); + it++) + { + lineObs.clear(); + lineObs += satelliteSystem["GPS"]; + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_gps_map.equal_range(*it); + for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) + { + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int32_t lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + + // Signal Strength Indicator (SSI) + int32_t ssi = Rinex_Printer::signalStrength(iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // CARRIER PHASE + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); + } + + out << lineObs << std::endl; + } + + for (it = available_gal_prns.begin(); + it != available_gal_prns.end(); + it++) + { + lineObs.clear(); + lineObs += satelliteSystem["Galileo"]; + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_gal_map.equal_range(*it); + for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) + { + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int32_t lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + + // Signal Strength Indicator (SSI) + int32_t ssi = Rinex_Printer::signalStrength(iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo CARRIER PHASE + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); + } + + //if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } +} + + +void Rinex_Printer::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& observables) +{ + if (galileo_eph.e_1) + { + } // avoid warning, not needed + if (gps_cnav_eph.d_e_eccentricity) + { + } // avoid warning, not needed + std::string line; + + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(gps_eph, gps_obs_time); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); + //double gps_t = eph.sv_clock_correction(obs_time); + double gps_t = gps_obs_time; + + std::string month(timestring, 4, 2); + std::string day(timestring, 6, 2); + std::string hour(timestring, 9, 2); + std::string minutes(timestring, 11, 2); + + std::string year(timestring, 0, 4); + line += std::string(1, '>'); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + line += month; + line += std::string(1, ' '); + line += day; + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + + line += std::string(1, ' '); + double seconds = fmod(gps_t, 60); + // Add extra 0 if seconds are < 10 + if (seconds < 10) + { + line += std::string(1, '0'); + } + line += Rinex_Printer::asString(seconds, 7); + line += std::string(2, ' '); + // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event + line += std::string(1, '0'); + + //Number of satellites observed in current epoch + + //Get maps with observations + std::map observablesG2S; + std::map observablesGL5; + std::map observablesG1C; + std::map observablesE1B; + std::map observablesE5A; + std::map observablesE5B; + std::map::const_iterator observables_iter; + + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); + observables_iter++) + { + std::string system_(&observables_iter->second.System, 1); + std::string sig_(observables_iter->second.Signal); + if ((system_.compare("E") == 0) && (sig_.compare("1B") == 0)) + { + observablesE1B.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("E") == 0) && (sig_.compare("5X") == 0)) + { + observablesE5A.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("E") == 0) && (sig_.compare("7X") == 0)) + { + observablesE5B.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("G") == 0) && (sig_.compare("2S") == 0)) + { + observablesG2S.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("G") == 0) && (sig_.compare("L5") == 0)) + { + observablesGL5.insert(std::pair(observables_iter->first, observables_iter->second)); + } + if ((system_.compare("G") == 0) && (sig_.compare("1C") == 0)) + { + observablesG1C.insert(std::pair(observables_iter->first, observables_iter->second)); + } + } + + std::multimap total_gps_map; + std::multimap total_gal_map; + std::set available_gal_prns; + std::set available_gps_prns; + std::set::iterator it; + for (observables_iter = observablesE1B.cbegin(); + observables_iter != observablesE1B.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gal_prns.find(prn_); + if (it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for (observables_iter = observablesE5A.cbegin(); + observables_iter != observablesE5A.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gal_prns.find(prn_); + if (it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for (observables_iter = observablesE5B.cbegin(); + observables_iter != observablesE5B.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gal_prns.find(prn_); + if (it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for (observables_iter = observablesG1C.cbegin(); + observables_iter != observablesG1C.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gps_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gps_prns.find(prn_); + if (it == available_gps_prns.end()) + { + available_gps_prns.insert(prn_); + } + } + + for (observables_iter = observablesG2S.cbegin(); + observables_iter != observablesG2S.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gps_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gps_prns.find(prn_); + if (it == available_gps_prns.end()) + { + available_gps_prns.insert(prn_); + } + } + + for (observables_iter = observablesGL5.cbegin(); + observables_iter != observablesGL5.cend(); + observables_iter++) + { + uint32_t prn_ = observables_iter->second.PRN; + total_gps_map.insert(std::pair(prn_, observables_iter->second)); + it = available_gps_prns.find(prn_); + if (it == available_gps_prns.end()) + { + available_gps_prns.insert(prn_); + } + } + + int32_t numGalSatellitesObserved = available_gal_prns.size(); + int32_t numGpsSatellitesObserved = available_gps_prns.size(); + int32_t numSatellitesObserved = numGalSatellitesObserved + numGpsSatellitesObserved; + line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); + + // Receiver clock offset (optional) + //line += rightJustify(asString(clockOffset, 12), 15); + + line += std::string(80 - line.size(), ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + std::string s; + std::string lineObs; + + std::pair::iterator, std::multimap::iterator> ret; + for (it = available_gps_prns.begin(); + it != available_gps_prns.end(); + it++) + { + lineObs.clear(); + lineObs += satelliteSystem["GPS"]; + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_gps_map.equal_range(*it); + for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) + { + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int32_t lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + + // Signal Strength Indicator (SSI) + int32_t ssi = Rinex_Printer::signalStrength(iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // CARRIER PHASE + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); + } + + out << lineObs << std::endl; + } + + for (it = available_gal_prns.begin(); + it != available_gal_prns.end(); + it++) + { + lineObs.clear(); + lineObs += satelliteSystem["Galileo"]; + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_gal_map.equal_range(*it); + for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) + { + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int32_t lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + + // Signal Strength Indicator (SSI) + int32_t ssi = Rinex_Printer::signalStrength(iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo CARRIER PHASE + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + //else + // { + // lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + // } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); + } + + //if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } +} + + void Rinex_Printer::to_date_time(int32_t gps_week, int32_t gps_tow, int& year, int& month, int& day, int& hour, int& minute, int& second) { // represents GPS time (week, TOW) in the date time format of the Gregorian calendar. diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 9f4e21129..24b140e83 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -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& gps_eph_map, const std::map& 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& gps_cnav_eph_map, const std::map& 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& 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& 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& 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); From 1ca28873ab595036f3e587a4960df16f78ed07b4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 1 Nov 2018 19:10:50 +0100 Subject: [PATCH 09/26] Fix warnings --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 14 +++++++------- src/algorithms/PVT/libs/rinex_printer.cc | 13 ++++++++++++- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index d6f044e08..dd5468c6e 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1326,13 +1326,13 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (type_of_rx == 32) // L1+E1+L5+E5a { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && - (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) && + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and + (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { std::string gal_signal("1B 5X"); std::string gps_signal("1C L5"); - rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gps_signal, gal_signal); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gps_signal, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model); b_rinex_header_written = true; // do not write header anymore } @@ -1564,7 +1564,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model); @@ -1677,11 +1677,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -1690,7 +1690,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 30) // Galileo E1B + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 67fb12ea3..9a2ab524b 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -5628,6 +5628,14 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps { std::string line; version = 3; + if (eph_cnav.d_e_eccentricity == 0) + { + // avoid warning + } + if (galileo_eph.e_1 == 0) + { + // avoid warning + } // -------- Line 1 line = std::string(5, ' '); @@ -5968,7 +5976,10 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_CNAV_Ephemeris { std::string line; version = 3; - + if (galileo_eph.e_1 == 0) + { + // avoid warning + } // -------- Line 1 line = std::string(5, ' '); line += "3.02"; From 99e4dcbe2be53e6ae60e457aa06e3c6fa2ddb420 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 1 Nov 2018 19:24:06 +0100 Subject: [PATCH 10/26] Cosmetic fix --- src/algorithms/PVT/libs/rinex_printer.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 24b140e83..dde6724a6 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -134,8 +134,8 @@ public: void rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac); /*! - * \brief Generates the Mixed (GPS L2C C/A/GLONASS L1, L2) Navigation Data header - */ + * \brief Generates the Mixed (GPS L2C C/A/GLONASS L1, L2) Navigation Data header + */ void rinex_nav_header(std::fstream& out, const Gps_CNAV_Iono& gps_iono, const Gps_CNAV_Utc_Model& gps_utc_model, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac); /*! @@ -255,8 +255,8 @@ public: void log_rinex_nav(std::fstream& out, const std::map& gps_eph_map, const std::map& galileo_eph_map); /*! - * \brief Writes data from the Mixed (GPS/Galileo) navigation message into the RINEX file - */ + * \brief Writes data from the Mixed (GPS/Galileo) navigation message into the RINEX file + */ void log_rinex_nav(std::fstream& out, const std::map& gps_cnav_eph_map, const std::map& galileo_eph_map); /*! @@ -310,8 +310,8 @@ public: void log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& eph, const Galileo_Ephemeris& galileo_eph, double gps_obs_time, const std::map& observables); /*! - * \brief Writes Mixed GPS / Galileo observables into the RINEX file - */ + * \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& observables); /*! From 15df79837f7ecc8ee67a244dcff36cd8ed54741a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 1 Nov 2018 20:10:41 +0100 Subject: [PATCH 11/26] Fix header in RINEX nav file for L1+L5 --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index dd5468c6e..8ad22c461 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1535,12 +1535,12 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); } else { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); } b_rinex_header_updated = true; } From 7c238bfb44126d6866a3ff7fa26f89519b9ecc4c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 2 Nov 2018 12:15:28 +0100 Subject: [PATCH 12/26] Add RTCM messages for more signal combinations --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 904 +++++++++++------- 1 file changed, 561 insertions(+), 343 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 8ad22c461..e005ff45b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -868,9 +868,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item bool flag_write_RTCM_MSM_output = false; bool flag_write_RINEX_obs_output = false; bool flag_write_RINEX_nav_output = false; - uint32_t gps_channel = 0; - uint32_t gal_channel = 0; - uint32_t glo_channel = 0; gnss_observables_map.clear(); const Gnss_Synchro** in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer @@ -1067,7 +1064,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; std::map::const_iterator glonass_gnav_ephemeris_iter; - std::map::const_iterator gnss_observables_iter; // ####################### RINEX FILES ################# if (b_rinex_output_enabled) @@ -1339,7 +1335,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } } - if (b_rinex_header_written) // The header is already written, we can now log the navigation message data { if (flag_write_RINEX_nav_output) @@ -1738,6 +1733,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } } } + // ####################### RTCM MESSAGES ################# try { @@ -1747,19 +1743,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1019_output == true) { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (flag_write_RTCM_MSM_output == true) { - std::map::const_iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + std::map::const_iterator gps_eph_iter; + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1767,18 +1762,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1045_output == true) { - for (std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_ephemeris_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } } if (flag_write_RTCM_MSM_output == true) { - std::map::const_iterator gal_ephemeris_iter; - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + std::map::const_iterator gal_eph_iter; + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1786,20 +1781,41 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1019_output == true) { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (flag_write_RTCM_MSM_output == true) { - std::map::const_iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - std::map::const_iterator gps_cnav_ephemeris_iter; - gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + std::map::const_iterator gps_eph_iter; + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_eph_iter; + gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if (type_of_rx == 8) // L1+L5 + { + if (flag_write_RTCM_1019_output == true) + { + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gps_eph_iter; + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_eph_iter; + gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1807,23 +1823,25 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1019_output == true) { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (flag_write_RTCM_1045_output == true) { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } } if (flag_write_RTCM_MSM_output == true) { - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - uint32_t i = 0; + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gps_eph_iter; + std::map::const_iterator gal_eph_iter; + int gps_channel = 0; + int gal_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); @@ -1832,10 +1850,10 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("G") == 0) { // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - gps_channel = i; + gps_channel = 1; } } } @@ -1843,28 +1861,46 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (system.compare("E") == 0) { - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - gal_channel = i; + gal_channel = 1; } } } - i++; } - if (flag_write_RTCM_MSM_output == true) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if (type_of_rx == 13) // L5+E5a + { + std::map::const_iterator gal_eph_iter; + std::map::const_iterator gps_cnav_eph_iter; + if (flag_write_RTCM_1045_output == true) + { + for (gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); + } + } + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + if (flag_write_RTCM_MSM_output == true) + { + if (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1877,22 +1913,24 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } - - std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); - - if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + if (flag_write_RTCM_MSM_output == true) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } } b_rtcm_writing_started = true; } if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { + std::map::const_iterator gps_eph_iter; if (flag_write_RTCM_1019_output == true) { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + for (gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (flag_write_RTCM_1020_output == true) @@ -1904,10 +1942,12 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (flag_write_RTCM_MSM_output == true) { - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - uint32_t i = 0; - for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gps_eph_iter; + int gps_channel = 0; + int glo_channel = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); if (gps_channel == 0) @@ -1915,10 +1955,10 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("G") == 0) { // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - gps_channel = i; + gps_channel = 1; } } } @@ -1926,28 +1966,22 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (system.compare("R") == 0) { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - glo_channel = i; + glo_channel = 1; } } } - i++; } - if (flag_write_RTCM_MSM_output == true) + + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -1955,23 +1989,25 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1020_output == true) { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } if (flag_write_RTCM_1045_output == true) { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } } if (flag_write_RTCM_MSM_output == true) { - // gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - // galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - uint32_t i = 0; + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gal_eph_iter; + std::map::const_iterator glonass_gnav_eph_iter; + int gal_channel = 0; + int glo_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); @@ -1980,10 +2016,10 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("E") == 0) { // This is a channel with valid GPS signal - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - gal_channel = i; + gal_channel = 1; } } } @@ -1991,28 +2027,21 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (system.compare("R") == 0) { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) { - glo_channel = i; + glo_channel = 1; } } } - i++; } - if (flag_write_RTCM_MSM_output == true) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -2020,23 +2049,25 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1019_output == true) { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (flag_write_RTCM_1020_output == true) { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } if (flag_write_RTCM_MSM_output == true) { - // gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - // galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - uint32_t i = 0; + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gps_eph_iter; + std::map::const_iterator glonass_gnav_eph_iter; + int gps_channel = 0; + int glo_channel = 0; for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); @@ -2045,10 +2076,10 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("G") == 0) { // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - gps_channel = i; + gps_channel = 1; } } } @@ -2056,28 +2087,21 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (system.compare("R") == 0) { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - glo_channel = i; + glo_channel = 1; } } } - i++; } - if (flag_write_RTCM_MSM_output == true) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -2085,23 +2109,25 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1020_output == true) { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } if (flag_write_RTCM_1045_output == true) { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } } if (flag_write_RTCM_MSM_output == true) { - // gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - // galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - uint32_t i = 0; + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gal_eph_iter; + std::map::const_iterator glonass_gnav_eph_iter; + int gal_channel = 0; + int glo_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); @@ -2110,10 +2136,10 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("E") == 0) { // This is a channel with valid GPS signal - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { - gal_channel = i; + gal_channel = 1; } } } @@ -2121,28 +2147,81 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (system.compare("R") == 0) { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) { - glo_channel = i; + glo_channel = 1; } } } - i++; } - if (flag_write_RTCM_MSM_output == true) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if (type_of_rx == 32) // L1+E1+L5+E5a + { + if (flag_write_RTCM_1019_output == true) + { + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); + } + } + if (flag_write_RTCM_1045_output == true) + { + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gal_eph_iter; + std::map::const_iterator gps_eph_iter; + int gal_channel = 0; + int gps_channel = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gal_channel == 0) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + if (system.compare("E") == 0) + { + // This is a channel with valid GPS signal + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + gal_channel = 1; + } + } + } + if (gps_channel == 0) + { + if (system.compare("G") == 0) + { + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = 1; + } + } } } - if (flag_write_RTCM_MSM_output == true) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -2152,47 +2231,83 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (type_of_rx == 1) // GPS L1 C/A { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); + } } - - std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + if (d_rtcm_MSM_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } } b_rtcm_writing_started = true; } if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo { - for (std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_ephemeris_iter++) + if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); + } } - - std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); - - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + if (d_rtcm_MSM_rate_ms != 0) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } } b_rtcm_writing_started = true; } if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); + } } - - std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - std::map::const_iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if (d_rtcm_MSM_rate_ms != 0) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + + if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + b_rtcm_writing_started = true; + } + if (type_of_rx == 8) // L1+L5 + { + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); + } + } + if (d_rtcm_MSM_rate_ms != 0) + { + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + + if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } } b_rtcm_writing_started = true; } @@ -2200,72 +2315,100 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (d_rtcm_MT1045_rate_ms != 0) { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } } - - uint32_t i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + if (d_rtcm_MSM_rate_ms != 0) { - std::string system(&gnss_observables_iter->second.System, 1); - if (gps_channel == 0) + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gal_eph_iter; + std::map::const_iterator gps_eph_iter; + int gps_channel = 0; + int gal_channel = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { - if (system.compare("G") == 0) + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + if (system.compare("G") == 0) { - gps_channel = i; + // This is a channel with valid GPS signal + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = 1; + } + } + } + if (gal_channel == 0) + { + if (system.compare("E") == 0) + { + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + gal_channel = 1; + } } } } - if (gal_channel == 0) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end() and (d_rtcm_MT1077_rate_ms != 0)) { - if (system.compare("E") == 0) - { - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - gal_channel = i; - } - } + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end() and (d_rtcm_MT1097_rate_ms != 0)) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - i++; - } - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() and (d_rtcm_MT1077_rate_ms != 0)) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() and (d_rtcm_MT1097_rate_ms != 0)) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } } - - std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); - - if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + if (d_rtcm_MSM_rate_ms != 0) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + b_rtcm_writing_started = true; + } + if (type_of_rx == 13) // L5+E5a + { + std::map::const_iterator gal_eph_iter; + if (d_rtcm_MT1045_rate_ms != 0) + { + for (gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); + } + } + if (d_rtcm_MSM_rate_ms != 0) + { + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend() and (d_rtcm_MT1097_rate_ms != 0)) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } } b_rtcm_writing_started = true; } @@ -2273,235 +2416,310 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } - - // gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - // galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - uint32_t i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + if (d_rtcm_MSM_rate_ms != 0) { - std::string system(&gnss_observables_iter->second.System, 1); - if (gps_channel == 0) + std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gps_eph_iter; + int gps_channel = 0; + int glo_channel = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { - if (system.compare("G") == 0) + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + if (system.compare("G") == 0) { - gps_channel = i; + // This is a channel with valid GPS signal + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + gps_channel = 1; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + glo_channel = 1; + } } } } - if (glo_channel == 0) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - if (system.compare("R") == 0) - { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - glo_channel = i; - } - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - i++; } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; } if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B { if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } } - - uint32_t i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + if (d_rtcm_MSM_rate_ms != 0) { - std::string system(&gnss_observables_iter->second.System, 1); - if (gal_channel == 0) + int gal_channel = 0; + int glo_channel = 0; + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gal_eph_iter; + std::map::const_iterator glonass_gnav_eph_iter; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { - if (system.compare("E") == 0) + std::string system(&gnss_observables_iter->second.System, 1); + if (gal_channel == 0) { - // This is a channel with valid GPS signal - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + if (system.compare("E") == 0) { - gal_channel = i; + // This is a channel with valid GPS signal + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + gal_channel = 1; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = 1; + } } } } - if (glo_channel == 0) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - if (system.compare("R") == 0) - { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - glo_channel = i; - } - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - i++; - } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } if (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 C/A { if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } - - // gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - // galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - uint32_t i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + if (d_rtcm_MSM_rate_ms != 0) { - std::string system(&gnss_observables_iter->second.System, 1); - if (gps_channel == 0) + std::map::const_iterator gps_eph_iter; + std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gnss_observables_iter; + int gps_channel = 0; + int glo_channel = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { - if (system.compare("G") == 0) + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + if (system.compare("G") == 0) { - gps_channel = i; + // This is a channel with valid GPS signal + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + gps_channel = 1; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + glo_channel = 1; + } } } } - if (glo_channel == 0) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - if (system.compare("R") == 0) - { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - glo_channel = i; - } - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - i++; - } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } } - b_rtcm_writing_started = true; } if (type_of_rx == 30) // GLONASS L2 C/A + Galileo E1B { if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + for (std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_eph_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } } - - uint32_t i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + if (d_rtcm_MSM_rate_ms != 0) { - std::string system(&gnss_observables_iter->second.System, 1); - if (gal_channel == 0) + int gal_channel = 0; + int glo_channel = 0; + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gal_eph_iter; + std::map::const_iterator glonass_gnav_eph_iter; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { - if (system.compare("E") == 0) + std::string system(&gnss_observables_iter->second.System, 1); + if (gal_channel == 0) { - // This is a channel with valid GPS signal - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + if (system.compare("E") == 0) { - gal_channel = i; + // This is a channel with valid GPS signal + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + gal_channel = 1; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = 1; + } } } } - if (glo_channel == 0) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - if (system.compare("R") == 0) + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + b_rtcm_writing_started = true; + } + if (type_of_rx == 32) // L1+E1+L5+E5a + { + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); + } + } + if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); + } + } + if (d_rtcm_MSM_rate_ms != 0) + { + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator gal_eph_iter; + std::map::const_iterator gps_eph_iter; + int gps_channel = 0; + int gal_channel = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (system.compare("G") == 0) { - glo_channel = i; + // This is a channel with valid GPS signal + gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = 1; + } + } + } + if (gal_channel == 0) + { + if (system.compare("E") == 0) + { + gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + gal_channel = 1; + } } } } - i++; - } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } } + b_rtcm_writing_started = true; } } } - catch (const boost::exception& ex) { std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << std::endl; From 01394474084e31507d4a863a26e764eb950ff605 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 2 Nov 2018 14:12:31 +0100 Subject: [PATCH 13/26] Deactivate RTCM printer if not used --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 123 ++++++++++-------- .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 1 + 2 files changed, 67 insertions(+), 57 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index e005ff45b..aa3c921c5 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -342,59 +342,69 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, // initialize rtcm_printer std::string rtcm_dump_filename; rtcm_dump_filename = d_dump_filename; - d_rtcm_printer = std::make_shared(rtcm_dump_filename, conf_.rtcm_output_file_enabled, conf_.flag_rtcm_server, conf_.flag_rtcm_tty_port, conf_.rtcm_tcp_port, conf_.rtcm_station_id, conf_.rtcm_dump_devname, true, conf_.rtcm_output_file_path); - std::map rtcm_msg_rate_ms = conf_.rtcm_msg_rate_ms; - if (rtcm_msg_rate_ms.find(1019) != rtcm_msg_rate_ms.end()) + if (conf_.flag_rtcm_server or conf_.flag_rtcm_tty_port or conf_.rtcm_output_file_enabled) { - d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms[1019]; + d_rtcm_printer = std::make_shared(rtcm_dump_filename, conf_.rtcm_output_file_enabled, conf_.flag_rtcm_server, conf_.flag_rtcm_tty_port, conf_.rtcm_tcp_port, conf_.rtcm_station_id, conf_.rtcm_dump_devname, true, conf_.rtcm_output_file_path); + std::map rtcm_msg_rate_ms = conf_.rtcm_msg_rate_ms; + if (rtcm_msg_rate_ms.find(1019) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms[1019]; + } + else + { + d_rtcm_MT1019_rate_ms = bc::lcm(5000, d_output_rate_ms); // default value if not set + } + if (rtcm_msg_rate_ms.find(1020) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1020_rate_ms = rtcm_msg_rate_ms[1020]; + } + else + { + d_rtcm_MT1020_rate_ms = bc::lcm(5000, d_output_rate_ms); // default value if not set + } + if (rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; + } + else + { + d_rtcm_MT1045_rate_ms = bc::lcm(5000, d_output_rate_ms); // default value if not set + } + if (rtcm_msg_rate_ms.find(1077) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077 + { + d_rtcm_MT1077_rate_ms = rtcm_msg_rate_ms[1077]; + } + else + { + d_rtcm_MT1077_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set + } + if (rtcm_msg_rate_ms.find(1087) != rtcm_msg_rate_ms.end()) // whatever between 1081 and 1087 + { + d_rtcm_MT1087_rate_ms = rtcm_msg_rate_ms[1087]; + } + else + { + d_rtcm_MT1087_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set + } + if (rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 + { + d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097]; + d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms[1097]; + } + else + { + d_rtcm_MT1097_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set + d_rtcm_MSM_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set + } + b_rtcm_writing_started = false; + b_rtcm_enabled = true; } else { - d_rtcm_MT1019_rate_ms = bc::lcm(5000, d_output_rate_ms); // default value if not set + b_rtcm_enabled = false; + b_rtcm_writing_started = false; + d_rtcm_printer = nullptr; } - if (rtcm_msg_rate_ms.find(1020) != rtcm_msg_rate_ms.end()) - { - d_rtcm_MT1020_rate_ms = rtcm_msg_rate_ms[1020]; - } - else - { - d_rtcm_MT1020_rate_ms = bc::lcm(5000, d_output_rate_ms); // default value if not set - } - if (rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) - { - d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; - } - else - { - d_rtcm_MT1045_rate_ms = bc::lcm(5000, d_output_rate_ms); // default value if not set - } - if (rtcm_msg_rate_ms.find(1077) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077 - { - d_rtcm_MT1077_rate_ms = rtcm_msg_rate_ms[1077]; - } - else - { - d_rtcm_MT1077_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set - } - if (rtcm_msg_rate_ms.find(1087) != rtcm_msg_rate_ms.end()) // whatever between 1081 and 1087 - { - d_rtcm_MT1087_rate_ms = rtcm_msg_rate_ms[1087]; - } - else - { - d_rtcm_MT1087_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set - } - if (rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 - { - d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097]; - d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms[1097]; - } - else - { - d_rtcm_MT1097_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set - d_rtcm_MSM_rate_ms = bc::lcm(1000, d_output_rate_ms); // default value if not set - } - b_rtcm_writing_started = false; // initialize RINEX printer b_rinex_header_written = false; @@ -954,7 +964,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s; // } - if (d_ls_pvt->get_PVT(gnss_observables_map, false)) { //Optional debug code: export observables snapshot for rtklib unit testing @@ -1001,7 +1010,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { flag_write_RINEX_obs_output = true; } - if (current_RX_time_ms % static_cast(d_rinexnav_rate_ms) == 0) { flag_write_RINEX_nav_output = true; @@ -1059,15 +1067,16 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item * 29 | GPS L1 C/A + GLONASS L2 C/A * 30 | Galileo E1B + GLONASS L2 C/A * 31 | GPS L2C + GLONASS L2 C/A + * 32 | GPS L1 C/A + Galileo E1B + GPS L5 + Galileo E5a */ - std::map::const_iterator galileo_ephemeris_iter; - std::map::const_iterator gps_ephemeris_iter; - std::map::const_iterator gps_cnav_ephemeris_iter; - std::map::const_iterator glonass_gnav_ephemeris_iter; // ####################### RINEX FILES ################# if (b_rinex_output_enabled) { + std::map::const_iterator galileo_ephemeris_iter; + std::map::const_iterator gps_ephemeris_iter; + std::map::const_iterator gps_cnav_ephemeris_iter; + std::map::const_iterator glonass_gnav_ephemeris_iter; if (!b_rinex_header_written) // & we have utc data in nav message! { galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); @@ -1737,7 +1746,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // ####################### RTCM MESSAGES ################# try { - if (b_rtcm_writing_started) + if (b_rtcm_writing_started and b_rtcm_enabled) { if (type_of_rx == 1) // GPS L1 C/A { @@ -2219,7 +2228,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2227,7 +2236,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } } - if (!b_rtcm_writing_started) // the first time + if (!b_rtcm_writing_started and b_rtcm_enabled) // the first time { if (type_of_rx == 1) // GPS L1 C/A { diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 38db64a1b..427936704 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -81,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) From 6b854c35130173a6372622f2849c630e6a176112 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 3 Nov 2018 10:50:19 +0100 Subject: [PATCH 14/26] Fix segmentation fault under certain output combinations --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 115 +++++++++++------- 1 file changed, 71 insertions(+), 44 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index aa3c921c5..1a8bdae3c 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -401,6 +401,13 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, } else { + d_rtcm_MT1019_rate_ms = 0; + d_rtcm_MT1045_rate_ms = 0; + d_rtcm_MT1020_rate_ms = 0; + d_rtcm_MT1077_rate_ms = 0; + d_rtcm_MT1087_rate_ms = 0; + d_rtcm_MT1097_rate_ms = 0; + d_rtcm_MSM_rate_ms = 0; b_rtcm_enabled = false; b_rtcm_writing_started = false; d_rtcm_printer = nullptr; @@ -895,46 +902,49 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); } - try + if (b_rtcm_enabled) { - if (d_ls_pvt->gps_ephemeris_map.empty() == false) + try { - if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + if (d_ls_pvt->gps_ephemeris_map.empty() == false) { - d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->galileo_ephemeris_map.empty() == false) + { + if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->gps_cnav_ephemeris_map.empty() == false) + { + if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->glonass_gnav_ephemeris_map.empty() == false) + { + if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } } } - if (d_ls_pvt->galileo_ephemeris_map.empty() == false) + catch (const boost::exception& ex) { - if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } + std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << std::endl; + LOG(ERROR) << "RTCM boost exception: " << boost::diagnostic_information(ex); } - if (d_ls_pvt->gps_cnav_ephemeris_map.empty() == false) + catch (const std::exception& ex) { - if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } + std::cout << "RTCM std exception: " << ex.what() << std::endl; + LOG(ERROR) << "RTCM std exception: " << ex.what(); } - if (d_ls_pvt->glonass_gnav_ephemeris_map.empty() == false) - { - if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } - } - } - catch (const boost::exception& ex) - { - std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << std::endl; - LOG(ERROR) << "RTCM boost exception: " << boost::diagnostic_information(ex); - } - catch (const std::exception& ex) - { - std::cout << "RTCM std exception: " << ex.what() << std::endl; - LOG(ERROR) << "RTCM std exception: " << ex.what(); } } } @@ -976,17 +986,26 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { flag_display_pvt = true; } - if (current_RX_time_ms % d_rtcm_MT1019_rate_ms == 0 and d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - flag_write_RTCM_1019_output = true; + if (current_RX_time_ms % d_rtcm_MT1019_rate_ms == 0) + { + flag_write_RTCM_1019_output = true; + } } - if (current_RX_time_ms % d_rtcm_MT1020_rate_ms == 0 and d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 + if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - flag_write_RTCM_1020_output = true; + if (current_RX_time_ms % d_rtcm_MT1020_rate_ms == 0) + { + flag_write_RTCM_1020_output = true; + } } - if (current_RX_time_ms % d_rtcm_MT1045_rate_ms == 0 and d_rtcm_MT1045_rate_ms != 0) + if (d_rtcm_MT1045_rate_ms != 0) { - flag_write_RTCM_1045_output = true; + if (current_RX_time_ms % d_rtcm_MT1045_rate_ms == 0) + { + flag_write_RTCM_1045_output = true; + } } // TODO: RTCM 1077, 1087 and 1097 are not used, so, disable the output rates // if (current_RX_time_ms % d_rtcm_MT1077_rate_ms==0 and d_rtcm_MT1077_rate_ms != 0) @@ -1001,18 +1020,26 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // { // last_RTCM_1097_output_time = current_RX_time; // } - - if (current_RX_time_ms % d_rtcm_MSM_rate_ms == 0 and d_rtcm_MSM_rate_ms != 0) + if (d_rtcm_MSM_rate_ms != 0) { - flag_write_RTCM_MSM_output = true; + if (current_RX_time_ms % d_rtcm_MSM_rate_ms == 0) + { + flag_write_RTCM_MSM_output = true; + } } - if (current_RX_time_ms % static_cast(d_rinexobs_rate_ms) == 0) + if (d_rinexobs_rate_ms != 0) { - flag_write_RINEX_obs_output = true; + if (current_RX_time_ms % static_cast(d_rinexobs_rate_ms) == 0) + { + flag_write_RINEX_obs_output = true; + } } - if (current_RX_time_ms % static_cast(d_rinexnav_rate_ms) == 0) + if (d_rinexnav_rate_ms != 0) { - flag_write_RINEX_nav_output = true; + if (current_RX_time_ms % static_cast(d_rinexnav_rate_ms) == 0) + { + flag_write_RINEX_nav_output = true; + } } if (first_fix == true) From 766cc308bd79b83056cc691aa00c86a28bce3c7a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 4 Nov 2018 19:49:26 +0100 Subject: [PATCH 15/26] Fix crash after the first fix in some signal combinations --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 172 +++++++++--------- 1 file changed, 81 insertions(+), 91 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 1a8bdae3c..35630f31b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -908,28 +908,28 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (d_ls_pvt->gps_ephemeris_map.empty() == false) { - if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.cend()) { d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time } } if (d_ls_pvt->galileo_ephemeris_map.empty() == false) { - if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) + if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time } } if (d_ls_pvt->gps_cnav_ephemeris_map.empty() == false) { - if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) + if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.cend()) { d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time } } if (d_ls_pvt->glonass_gnav_ephemeris_map.empty() == false) { - if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time } @@ -967,7 +967,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { // receiver clock correction is disabled to be coherent with the RINEX and RTCM standard // std::cout << TEXT_RED << "(internal) accumulated RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]" << TEXT_RESET << std::endl; - // for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) + // for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.cend(); ++it) // { // todo: check if it has effect to correct the receiver time for the internal pvt solution // take into account that the RINEX obs with the RX time (integer ms) CAN NOT be corrected to keep the coherence in obs time @@ -1466,7 +1466,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (type_of_rx == 1) // GPS L1 C/A only { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1479,7 +1479,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 2) // GPS L2C only { - if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1492,7 +1492,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 3) // GPS L5 { - if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1505,7 +1505,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 4) // Galileo E1B only { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B"); } @@ -1518,7 +1518,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 5) // Galileo E5a only { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X"); } @@ -1531,7 +1531,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 6) // Galileo E5b only { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X"); } @@ -1544,7 +1544,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1557,7 +1557,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 8) // L1+L5 { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1578,7 +1578,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1591,7 +1591,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 13) // L5+E5a { - if ((gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + if ((gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1604,7 +1604,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 14) // Galileo E1B + Galileo E5a { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X"); } @@ -1617,7 +1617,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 15) // Galileo E1B + Galileo E5b { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X"); } @@ -1630,7 +1630,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 23) // GLONASS L1 C/A only { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C"); } @@ -1643,7 +1643,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 24) // GLONASS L2 C/A only { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "2C"); } @@ -1656,7 +1656,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 25) // GLONASS L1 C/A + GLONASS L2 C/A { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C 2C"); } @@ -1669,7 +1669,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1682,7 +1682,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1695,7 +1695,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 28) // GPS L2C + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1708,7 +1708,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 29) // GPS L1 C/A + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1721,7 +1721,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 30) // Galileo E1B + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1734,7 +1734,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 31) // GPS L2C + GLONASS L2 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1747,7 +1747,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 32) // L1+E1+L5+E5a { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); } @@ -1786,8 +1786,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (flag_write_RTCM_MSM_output == true) { - std::map::const_iterator gps_eph_iter; - gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -1805,8 +1804,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (flag_write_RTCM_MSM_output == true) { - std::map::const_iterator gal_eph_iter; - gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -1824,10 +1822,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (flag_write_RTCM_MSM_output == true) { - std::map::const_iterator gps_eph_iter; - gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - std::map::const_iterator gps_cnav_eph_iter; - gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -1845,10 +1841,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (flag_write_RTCM_MSM_output == true) { - std::map::const_iterator gps_eph_iter; - gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - std::map::const_iterator gps_cnav_eph_iter; - gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -1859,14 +1853,14 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { if (flag_write_RTCM_1019_output == true) { - for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_eph_iter++) + for (std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_eph_iter++) { d_rtcm_printer->Print_Rtcm_MT1019(gps_eph_iter->second); } } if (flag_write_RTCM_1045_output == true) { - for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_eph_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) { d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } @@ -1874,8 +1868,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (flag_write_RTCM_MSM_output == true) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gps_eph_iter; - std::map::const_iterator gal_eph_iter; + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); int gps_channel = 0; int gal_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -1979,8 +1973,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (flag_write_RTCM_MSM_output == true) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator glonass_gnav_eph_iter; - std::map::const_iterator gps_eph_iter; + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); int gps_channel = 0; int glo_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -2040,8 +2034,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (flag_write_RTCM_MSM_output == true) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gal_eph_iter; - std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); int gal_channel = 0; int glo_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -2064,18 +2058,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("R") == 0) { glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { glo_channel = 1; } } } } - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2100,11 +2094,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (flag_write_RTCM_MSM_output == true) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gps_eph_iter; - std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); int gps_channel = 0; int glo_channel = 0; - for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); if (gps_channel == 0) @@ -2160,8 +2154,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (flag_write_RTCM_MSM_output == true) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gal_eph_iter; - std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); int gal_channel = 0; int glo_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -2184,18 +2178,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("R") == 0) { glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { glo_channel = 1; } } } } - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2220,8 +2214,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (flag_write_RTCM_MSM_output == true) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gal_eph_iter; - std::map::const_iterator gps_eph_iter; + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); int gal_channel = 0; int gps_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -2244,7 +2238,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("G") == 0) { gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { gps_channel = 1; } @@ -2255,7 +2249,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2297,7 +2291,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (d_rtcm_MSM_rate_ms != 0) { std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -2318,7 +2311,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -2339,7 +2331,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - if ((gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, gps_cnav_eph_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -2358,7 +2349,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (d_rtcm_MT1045_rate_ms != 0) { - for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_eph_iter++) + for (std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_eph_iter++) { d_rtcm_printer->Print_Rtcm_MT1045(gal_eph_iter->second); } @@ -2366,8 +2357,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (d_rtcm_MSM_rate_ms != 0) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gal_eph_iter; - std::map::const_iterator gps_eph_iter; + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); int gps_channel = 0; int gal_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -2379,7 +2370,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { // This is a channel with valid GPS signal gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { gps_channel = 1; } @@ -2390,18 +2381,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("E") == 0) { gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { gal_channel = 1; } } } } - if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end() and (d_rtcm_MT1077_rate_ms != 0)) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_eph_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end() and (d_rtcm_MT1097_rate_ms != 0)) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2420,7 +2411,6 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (d_rtcm_MSM_rate_ms != 0) { std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); - if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); @@ -2466,9 +2456,9 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (d_rtcm_MSM_rate_ms != 0) { - std::map::const_iterator glonass_gnav_eph_iter; std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gps_eph_iter; + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); int gps_channel = 0; int glo_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -2530,8 +2520,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item int gal_channel = 0; int glo_channel = 0; std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gal_eph_iter; - std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); @@ -2552,18 +2542,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("R") == 0) { glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { glo_channel = 1; } } } } - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2588,8 +2578,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (d_rtcm_MSM_rate_ms != 0) { - std::map::const_iterator gps_eph_iter; - std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); std::map::const_iterator gnss_observables_iter; int gps_channel = 0; int glo_channel = 0; @@ -2653,8 +2643,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item int gal_channel = 0; int glo_channel = 0; std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gal_eph_iter; - std::map::const_iterator glonass_gnav_eph_iter; + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); @@ -2675,18 +2665,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("R") == 0) { glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { glo_channel = 1; } } } } - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_eph_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + if (glonass_gnav_eph_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_eph_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -2712,8 +2702,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (d_rtcm_MSM_rate_ms != 0) { std::map::const_iterator gnss_observables_iter; - std::map::const_iterator gal_eph_iter; - std::map::const_iterator gps_eph_iter; + std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + std::map::const_iterator gps_eph_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); int gps_channel = 0; int gal_channel = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) @@ -2725,7 +2715,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { // This is a channel with valid GPS signal gps_eph_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) + if (gps_eph_iter != d_ls_pvt->gps_ephemeris_map.cend()) { gps_channel = 1; } @@ -2736,7 +2726,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("E") == 0) { gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (gal_eph_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { gal_channel = 1; } From 40efd08b56de3ed6074c771f0eba91c946c94893 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 4 Nov 2018 19:52:20 +0100 Subject: [PATCH 16/26] Fix typo --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 35630f31b..fe2cce6e1 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -2521,7 +2521,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item int glo_channel = 0; std::map::const_iterator gnss_observables_iter; std::map::const_iterator gal_eph_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); - std::map::const_iterator glonass_gnav_eph_iter = = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + std::map::const_iterator glonass_gnav_eph_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); From 70800a7c6c941618928a178d39c06348abdbadbf Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 4 Nov 2018 20:14:25 +0100 Subject: [PATCH 17/26] Improve const correctness --- src/algorithms/PVT/libs/rinex_printer.cc | 88 ++++++++++++------------ 1 file changed, 44 insertions(+), 44 deletions(-) diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 9a2ab524b..88c001e2b 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -3296,8 +3296,8 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map::const_iterator glonass_gnav_ephemeris_iter; - for (glonass_gnav_ephemeris_iter = eph_map.begin(); - glonass_gnav_ephemeris_iter != eph_map.end(); + for (glonass_gnav_ephemeris_iter = eph_map.cbegin(); + glonass_gnav_ephemeris_iter != eph_map.cend(); glonass_gnav_ephemeris_iter++) { // -------- SV / EPOCH / SV CLK @@ -7114,15 +7114,15 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri //Number of satellites observed in current epoch int32_t numSatellitesObserved = 0; std::map::const_iterator observables_iter; - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { numSatellitesObserved++; } line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { line += satelliteSystem["GLONASS"]; @@ -7135,8 +7135,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri Rinex_Printer::lengthCheck(line); out << line << std::endl; - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { std::string lineObs; @@ -7218,8 +7218,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri //Number of satellites observed in current epoch int32_t numSatellitesObserved = 0; std::map::const_iterator observables_iter; - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { numSatellitesObserved++; @@ -7233,8 +7233,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri Rinex_Printer::lengthCheck(line); out << line << std::endl; - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { std::string lineObs; @@ -7390,8 +7390,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep std::map observablesR2C; std::map::const_iterator observables_iter; - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { std::string system_(&observables_iter->second.System, 1); @@ -7413,8 +7413,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep std::multimap total_glo_map; std::set available_glo_prns; std::set::iterator it; - for (observables_iter = observablesR1C.begin(); - observables_iter != observablesR1C.end(); + for (observables_iter = observablesR1C.cbegin(); + observables_iter != observablesR1C.cend(); observables_iter++) { uint32_t prn_ = observables_iter->second.PRN; @@ -7426,8 +7426,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep } } - for (observables_iter = observablesR2C.begin(); - observables_iter != observablesR2C.end(); + for (observables_iter = observablesR2C.cbegin(); + observables_iter != observablesR2C.cend(); observables_iter++) { uint32_t prn_ = observables_iter->second.PRN; @@ -7446,8 +7446,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep if (version == 2) { // Add list of GPS satellites - for (observables_iter = observablesG1C.begin(); - observables_iter != observablesG1C.end(); + for (observables_iter = observablesG1C.cbegin(); + observables_iter != observablesG1C.cend(); observables_iter++) { line += satelliteSystem["GPS"]; @@ -7455,8 +7455,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep line += boost::lexical_cast(static_cast(observables_iter->second.PRN)); } // Add list of GLONASS L1 satellites - for (observables_iter = observablesR1C.begin(); - observables_iter != observablesR1C.end(); + for (observables_iter = observablesR1C.cbegin(); + observables_iter != observablesR1C.cend(); observables_iter++) { line += satelliteSystem["GLONASS"]; @@ -7464,8 +7464,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep line += boost::lexical_cast(static_cast(observables_iter->second.PRN)); } // Add list of GLONASS L2 satellites - for (observables_iter = observablesR2C.begin(); - observables_iter != observablesR2C.end(); + for (observables_iter = observablesR2C.cbegin(); + observables_iter != observablesR2C.cend(); observables_iter++) { line += satelliteSystem["GLONASS"]; @@ -7480,8 +7480,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep // -------- OBSERVATION record std::string s; std::string lineObs; - for (observables_iter = observablesG1C.begin(); - observables_iter != observablesG1C.end(); + for (observables_iter = observablesG1C.cbegin(); + observables_iter != observablesG1C.cend(); observables_iter++) { lineObs.clear(); @@ -7664,8 +7664,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g std::map observablesR2C; std::map::const_iterator observables_iter; - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { std::string system_(&observables_iter->second.System, 1); @@ -7687,8 +7687,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g std::multimap total_glo_map; std::set available_glo_prns; std::set::iterator it; - for (observables_iter = observablesR1C.begin(); - observables_iter != observablesR1C.end(); + for (observables_iter = observablesR1C.cbegin(); + observables_iter != observablesR1C.cend(); observables_iter++) { uint32_t prn_ = observables_iter->second.PRN; @@ -7700,8 +7700,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g } } - for (observables_iter = observablesR2C.begin(); - observables_iter != observablesR2C.end(); + for (observables_iter = observablesR2C.cbegin(); + observables_iter != observablesR2C.cend(); observables_iter++) { uint32_t prn_ = observables_iter->second.PRN; @@ -7725,8 +7725,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g // -------- OBSERVATION record std::string s; std::string lineObs; - for (observables_iter = observablesG2S.begin(); - observables_iter != observablesG2S.end(); + for (observables_iter = observablesG2S.cbegin(); + observables_iter != observablesG2S.cend(); observables_iter++) { lineObs.clear(); @@ -7904,8 +7904,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga std::map observablesR2C; std::map::const_iterator observables_iter; - for (observables_iter = observables.begin(); - observables_iter != observables.end(); + for (observables_iter = observables.cbegin(); + observables_iter != observables.cend(); observables_iter++) { std::string system_(&observables_iter->second.System, 1); @@ -7927,8 +7927,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga std::multimap total_glo_map; std::set available_glo_prns; std::set::iterator it; - for (observables_iter = observablesR1C.begin(); - observables_iter != observablesR1C.end(); + for (observables_iter = observablesR1C.cbegin(); + observables_iter != observablesR1C.cend(); observables_iter++) { uint32_t prn_ = observables_iter->second.PRN; @@ -7939,8 +7939,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga available_glo_prns.insert(prn_); } } - for (observables_iter = observablesR2C.begin(); - observables_iter != observablesR2C.end(); + for (observables_iter = observablesR2C.cbegin(); + observables_iter != observablesR2C.cend(); observables_iter++) { uint32_t prn_ = observables_iter->second.PRN; @@ -7966,8 +7966,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga std::string s; std::string lineObs; - for (observables_iter = observablesE1B.begin(); - observables_iter != observablesE1B.end(); + for (observables_iter = observablesE1B.cbegin(); + observables_iter != observablesE1B.cend(); observables_iter++) { lineObs.clear(); @@ -8747,8 +8747,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep std::set::iterator it; if (found_1B != std::string::npos) { - for (observables_iter = observablesE1B.begin(); - observables_iter != observablesE1B.end(); + for (observables_iter = observablesE1B.cbegin(); + observables_iter != observablesE1B.cend(); observables_iter++) { uint32_t prn_ = observables_iter->second.PRN; From 8d3074a9dc31c0ca4e4fa087e0fa67562b92d29b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 4 Nov 2018 22:50:46 +0100 Subject: [PATCH 18/26] Replace boost::lexical_cast by std::to_string --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 6 +++--- .../tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 5401fae93..09f1cf8bf 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -50,7 +50,7 @@ #include "GPS_L5.h" #include "gps_l5_signal.h" #include "gnss_sdr_create_directory.h" -#include +#include #include #include #include @@ -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(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(d_channel)); + dump_filename_.append(std::to_string(d_channel)); // add extension dump_filename_.append(".dat"); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc index 71d8cfc5a..f73ed6c90 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc @@ -48,7 +48,7 @@ #include "GPS_L5.h" #include "gps_l5_signal.h" #include "gnss_sdr_create_directory.h" -#include +#include #include #include #include @@ -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(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(d_channel)); + dump_filename_.append(std::to_string(d_channel)); // add extension dump_filename_.append(".dat"); From 99989d472cf572411679c6f9fdddda36713f3074 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 5 Nov 2018 15:39:56 +0100 Subject: [PATCH 19/26] Fix NMEA GPGSV message for GPS L1 --- src/algorithms/PVT/libs/nmea_printer.cc | 88 ++---------------------- src/algorithms/PVT/libs/rtklib_solver.cc | 50 +++++++++++++- src/algorithms/PVT/libs/rtklib_solver.h | 1 + 3 files changed, 57 insertions(+), 82 deletions(-) diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index 1337526b9..26b18e3d0 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -34,6 +34,7 @@ */ #include "nmea_printer.h" +#include "rtklib_solution.h" #include #include // for create_directories, exists #include // for path, operator<< @@ -560,88 +561,13 @@ 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(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(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(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(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(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[200]; + outnmea_gsv(buff, &d_PVT_data->pvt_sol, *d_PVT_data->pvt_ssat); + sentence_str << buff; + return sentence_str.str(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 12d316196..1ef53bc58 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -53,6 +53,7 @@ #include "rtklib_solver.h" #include "rtklib_conversions.h" +#include "rtklib_solution.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" #include "GLONASS_L1_L2_CA.h" @@ -809,8 +810,55 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ index_aux++; } } - if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_); + for (gnss_observables_iter = gnss_observables_map.cbegin(); + gnss_observables_iter != gnss_observables_map.cend(); + ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system + { + switch (gnss_observables_iter->second.System) + { + case 'E': + break; + + case 'G': + { + // GPS L1 + std::string sig_(gnss_observables_iter->second.Signal); + if (sig_.compare("1C") == 0) + { + unsigned int snr = static_cast(std::round(gnss_observables_iter->second.CN0_dB_hz / 0.25)); + rtk_.ssat[gnss_observables_iter->second.PRN - 1].snr[0] = snr; //newobs.SNR[0]; + pvt_ssat[gnss_observables_iter->second.PRN - 1] = &rtk_.ssat[gnss_observables_iter->second.PRN - 1]; + } + // GPS L2 + if (sig_.compare("2S") == 0) + { + } + // GPS L5 + if (sig_.compare("L5") == 0) + { + } + break; + } + case 'R': + { + std::string sig_(gnss_observables_iter->second.Signal); + // GLONASS GNAV L1 + if (sig_.compare("1G") == 0) + { + } + // GLONASS GNAV L2 + if (sig_.compare("2G") == 0) + { + } + break; + } + default: + break; + } + } + + 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] diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index b584c4826..1e9684685 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -86,6 +86,7 @@ private: public: sol_t pvt_sol; + ssat_t* pvt_ssat[MAXSAT]; rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, rtk_t& rtk); ~rtklib_solver(); From 4f7b43255acc9c608a66f4f9ec035c6f593f05e4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 5 Nov 2018 17:36:26 +0100 Subject: [PATCH 20/26] Add work on NMEA messages --- src/algorithms/PVT/libs/rtklib_solver.cc | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1ef53bc58..9e0e375e2 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -818,8 +818,16 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ switch (gnss_observables_iter->second.System) { case 'E': - break; - + { + std::string sig_(gnss_observables_iter->second.Signal); + if (sig_.compare("1B") == 0) + { + unsigned int snr = static_cast(std::round(gnss_observables_iter->second.CN0_dB_hz / 0.25)); + rtk_.ssat[gnss_observables_iter->second.PRN - 1].snr[0] = snr; + pvt_ssat[gnss_observables_iter->second.PRN - 1] = &rtk_.ssat[gnss_observables_iter->second.PRN - 1]; + } + break; + } case 'G': { // GPS L1 @@ -827,7 +835,7 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ if (sig_.compare("1C") == 0) { unsigned int snr = static_cast(std::round(gnss_observables_iter->second.CN0_dB_hz / 0.25)); - rtk_.ssat[gnss_observables_iter->second.PRN - 1].snr[0] = snr; //newobs.SNR[0]; + rtk_.ssat[gnss_observables_iter->second.PRN - 1].snr[0] = snr; pvt_ssat[gnss_observables_iter->second.PRN - 1] = &rtk_.ssat[gnss_observables_iter->second.PRN - 1]; } // GPS L2 From 01a1954b155b5d9a72d0c2e05ddfb5f03fda67a1 Mon Sep 17 00:00:00 2001 From: Javier Date: Mon, 5 Nov 2018 17:53:53 +0100 Subject: [PATCH 21/26] Adding the Hotstart telecommand implementation to predict visible satellites for GPS and Galileo constellations --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 22 +++ src/algorithms/PVT/adapters/rtklib_pvt.h | 7 + .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 23 ++- .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 19 ++- src/algorithms/channel/libs/channel_fsm.cc | 4 +- src/algorithms/libs/CMakeLists.txt | 1 + .../libs/geofunctions.cc | 0 .../libs/geofunctions.h | 0 .../libs/rtklib/rtklib_conversions.cc | 58 ++++++- .../libs/rtklib/rtklib_conversions.h | 6 + src/core/interfaces/pvt_interface.h | 9 ++ src/core/receiver/control_thread.cc | 147 ++++++++++++++++++ src/core/receiver/control_thread.h | 13 +- src/core/receiver/gnss_flowgraph.cc | 64 ++++++-- src/core/receiver/gnss_flowgraph.h | 22 ++- src/core/receiver/tcp_cmd_interface.cc | 87 +++++++++-- src/core/receiver/tcp_cmd_interface.h | 17 +- src/main/CMakeLists.txt | 1 + src/tests/system-tests/libs/CMakeLists.txt | 1 - 19 files changed, 458 insertions(+), 43 deletions(-) rename src/{tests/system-tests => algorithms}/libs/geofunctions.cc (100%) rename src/{tests/system-tests => algorithms}/libs/geofunctions.h (100%) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 74b2f4c5d..c5532dea2 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -48,6 +48,12 @@ namespace bc = boost::integer; using google::LogMessage; + +void RtklibPvt::clear_ephemeris() +{ + pvt_->clear_ephemeris(); +} + RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, std::string role, unsigned int in_streams, @@ -515,6 +521,22 @@ RtklibPvt::~RtklibPvt() rtkfree(&rtk); } +std::map RtklibPvt::get_gps_ephemeris() +{ + return pvt_->get_gps_ephemeris_map(); +} +std::map RtklibPvt::get_galileo_ephemeris() +{ + return pvt_->get_galileo_ephemeris_map(); +} +std::map RtklibPvt::get_gps_almanac() +{ + return pvt_->get_gps_almanac_map(); +} +std::map RtklibPvt::get_galileo_almanac() +{ + return pvt_->get_galileo_almanac_map(); +} void RtklibPvt::connect(gr::top_block_sptr top_block) { diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index c79519581..75e9c6c6d 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -63,6 +63,13 @@ public: return "RTKLIB_PVT"; } + void clear_ephemeris(); + std::map get_gps_ephemeris(); + std::map get_galileo_ephemeris(); + std::map get_gps_almanac(); + std::map 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; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index fe2cce6e1..69cc62264 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -234,11 +234,32 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) } -std::map rtklib_pvt_cc::get_GPS_L1_ephemeris_map() +std::map rtklib_pvt_cc::get_gps_ephemeris_map() { return d_ls_pvt->gps_ephemeris_map; } +std::map rtklib_pvt_cc::get_gps_almanac_map() +{ + return d_ls_pvt->gps_almanac_map; +} + +std::map rtklib_pvt_cc::get_galileo_ephemeris_map() +{ + return d_ls_pvt->galileo_ephemeris_map; +} +std::map rtklib_pvt_cc::get_galileo_almanac_map() +{ + return d_ls_pvt->galileo_almanac_map; +} + +void rtklib_pvt_cc::clear_ephemeris() +{ + d_ls_pvt->gps_ephemeris_map.clear(); + d_ls_pvt->gps_almanac_map.clear(); + d_ls_pvt->galileo_ephemeris_map.clear(); + d_ls_pvt->galileo_almanac_map.clear(); +} rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, const Pvt_Conf& conf_, diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 427936704..f063431d1 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -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" @@ -142,11 +142,22 @@ 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 get_GPS_L1_ephemeris_map(); + std::map get_gps_ephemeris_map(); + + std::map get_gps_almanac_map(); + + std::map get_galileo_ephemeris_map(); + + std::map get_galileo_almanac_map(); + + /*! + * \brief Clear all ephemeris information and the almanacs for GPS and Galileo + * + */ + void clear_ephemeris(); ~rtklib_pvt_cc(); //!< Default destructor diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 5b212193a..14a4e6b12 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -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() diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index 18dd400b0..ce4560e6f 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -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 diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/algorithms/libs/geofunctions.cc similarity index 100% rename from src/tests/system-tests/libs/geofunctions.cc rename to src/algorithms/libs/geofunctions.cc diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/algorithms/libs/geofunctions.h similarity index 100% rename from src/tests/system-tests/libs/geofunctions.h rename to src/algorithms/libs/geofunctions.h diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 7953509a2..22b8af3b9 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -116,7 +116,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glona eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO; rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; @@ -174,7 +174,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; rtklib_sat.sat = gps_eph.i_satellite_PRN; rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; rtklib_sat.M0 = gps_eph.d_M_0; @@ -231,7 +231,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; @@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) return rtklib_sat; } + +alm_t alm_to_rtklib(const Gps_Almanac& gps_alm) +{ + alm_t rtklib_alm; + + rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + rtklib_alm.sat = gps_alm.i_satellite_PRN; + rtklib_alm.svh = gps_alm.i_SV_health; + rtklib_alm.svconf = gps_alm.i_AS_status; + rtklib_alm.week = gps_alm.i_WNa; + rtklib_alm.toa = gpst2time(gps_alm.i_WNa, gps_alm.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; +} diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index 11f4208d1..dca5ec7be 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -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 diff --git a/src/core/interfaces/pvt_interface.h b/src/core/interfaces/pvt_interface.h index 69b23f541..c9a2db162 100644 --- a/src/core/interfaces/pvt_interface.h +++ b/src/core/interfaces/pvt_interface.h @@ -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,11 @@ class PvtInterface : public GNSSBlockInterface { public: virtual void reset() = 0; + virtual void clear_ephemeris() = 0; + virtual std::map get_gps_ephemeris() = 0; + virtual std::map get_galileo_ephemeris() = 0; + virtual std::map get_gps_almanac() = 0; + virtual std::map get_galileo_almanac() = 0; }; #endif /* GNSS_SDR_PVT_INTERFACE_H_ */ diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index a73d2b446..c95973797 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -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 #include #include @@ -693,6 +698,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,6 +713,8 @@ void ControlThread::process_control_messages() void ControlThread::apply_action(unsigned int what) { + std::shared_ptr pvt_ptr; + std::vector> visible_satellites; switch (what) { case 0: @@ -717,12 +728,148 @@ void ControlThread::apply_action(unsigned int what) restart_ = true; applied_actions_++; break; + case 12: + DLOG(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: + DLOG(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."; break; } } +std::vector> 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 to associate an elevation to the each satellite + std::vector> available_satellites; + + std::shared_ptr pvt_ptr = flowgraph_->get_pvt(); + + std::map gps_eph_map = pvt_ptr->get_gps_ephemeris(); + for (std::map::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); + std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; + //push sat + if (El > 0) + { + available_satellites.push_back(std::pair(floor(El), + (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); + } + } + + std::map gal_eph_map = pvt_ptr->get_galileo_ephemeris(); + for (std::map::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); + std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; + //push sat + if (El > 0) + { + available_satellites.push_back(std::pair(floor(El), + (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN)))); + } + } + + std::map gps_alm_map = pvt_ptr->get_gps_almanac(); + for (std::map::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); + std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; + //push sat + if (El > 0) + { + available_satellites.push_back(std::pair(floor(El), + (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); + } + } + + std::map gal_alm_map = pvt_ptr->get_galileo_almanac(); + for (std::map::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) + { + available_satellites.push_back(std::pair(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 &a, const std::pair &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() { diff --git a/src/core/receiver/control_thread.h b/src/core/receiver/control_thread.h index ffc2ffa39..c62006eee 100644 --- a/src/core/receiver/control_thread.h +++ b/src/core/receiver/control_thread.h @@ -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 #include #include #include - -class GNSSFlowgraph; -class ConfigurationInterface; +#include /*! @@ -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> get_visible_sats(time_t rx_utc_time, arma::vec LLH); /* * Read initial GNSS assistance from SUPL server or local XML files */ diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index dbeabf004..527696d00 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1137,10 +1137,6 @@ 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 for (unsigned int i = 0; i < channels_count_; i++) { unsigned int ch_index = (who + i + 1) % channels_count_; @@ -1169,12 +1165,6 @@ 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 - //start again the satellite acquisitions for (unsigned int i = 0; i < channels_count_; i++) { @@ -1209,6 +1199,60 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } +void GNSSFlowgraph::priorize_satellites(std::vector> visible_satellites) +{ + size_t old_size; + Gnss_Signal gs; + for (std::vector>::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 configuration) { if (running_) diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index 1f1d07ada..d5e25543a 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -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 #include #include @@ -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 get_pvt() + { + return std::dynamic_pointer_cast(pvt_); + } + + /*! + * \brief Priorize visible satellites in the specified vector + */ + void priorize_satellites(std::vector> visible_satellites); + private: void init(); // Populates the SV PRN list available for acquisition and tracking void set_signals_list(); diff --git a/src/core/receiver/tcp_cmd_interface.cc b/src/core/receiver/tcp_cmd_interface.cc index 175d96859..b857ee204 100644 --- a/src/core/receiver/tcp_cmd_interface.cc +++ b/src/core/receiver/tcp_cmd_interface.cc @@ -34,6 +34,15 @@ #include +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 &commandLine) { std::string response; @@ -78,17 +87,40 @@ std::string TcpCmdInterface::status(const std::vector &commandLine) std::string TcpCmdInterface::hotstart(const std::vector &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 - std::unique_ptr cmf(new ControlMessageFactory()); - if (control_queue_ != nullptr) + if (commandLine.size() > 5) { - control_queue_->handle(cmf->GetQueueMessage(300, 12)); //send the standby message (who=300,what=12) - response = "OK\n"; + //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 cmf(new ControlMessageFactory()); + if (control_queue_ != nullptr) + { + control_queue_->handle(cmf->GetQueueMessage(300, 12)); //send the standby message (who=300,what=12) + response = "OK\n"; + } + else + { + response = "ERROR\n"; + } + } } else { - response = "ERROR\n"; + response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n"; } return response; } @@ -96,17 +128,40 @@ std::string TcpCmdInterface::hotstart(const std::vector &commandLin std::string TcpCmdInterface::warmstart(const std::vector &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 - std::unique_ptr cmf(new ControlMessageFactory()); - if (control_queue_ != nullptr) + if (commandLine.size() > 5) { - control_queue_->handle(cmf->GetQueueMessage(300, 13)); //send the standby message (who=300,what=13) - response = "OK\n"; + 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 cmf(new ControlMessageFactory()); + if (control_queue_ != nullptr) + { + control_queue_->handle(cmf->GetQueueMessage(300, 13)); //send the standby message (who=300,what=13) + response = "OK\n"; + } + else + { + response = "ERROR\n"; + } + } } else { - response = "ERROR\n"; + response = "ERROR: time parameter not found, please use warmstart %d/%m/%Y %H:%M:%S Lat Long H\n"; } return response; } @@ -154,6 +209,10 @@ TcpCmdInterface::TcpCmdInterface() register_functions(); keep_running_ = true; control_queue_ = nullptr; + rx_latitude_ = 0; + rx_longitude_ = 0; + rx_altitude_ = 0; + receiver_utc_time_ = 0; } diff --git a/src/core/receiver/tcp_cmd_interface.h b/src/core/receiver/tcp_cmd_interface.h index 6071e32bd..7da156fe0 100644 --- a/src/core/receiver/tcp_cmd_interface.h +++ b/src/core/receiver/tcp_cmd_interface.h @@ -42,7 +42,8 @@ #include #include #include - +#include +#include "time.h" class TcpCmdInterface { @@ -51,6 +52,14 @@ 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(); private: @@ -68,6 +77,12 @@ private: gr::msg_queue::sptr control_queue_; bool keep_running_; + + time_t receiver_utc_time_; + + double rx_latitude_; + double rx_longitude_; + double rx_altitude_; }; #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */ diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 7b41ccfbd..701f8ca95 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -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 diff --git a/src/tests/system-tests/libs/CMakeLists.txt b/src/tests/system-tests/libs/CMakeLists.txt index 4742e80fc..89b6970c3 100644 --- a/src/tests/system-tests/libs/CMakeLists.txt +++ b/src/tests/system-tests/libs/CMakeLists.txt @@ -18,7 +18,6 @@ set(SYSTEM_TESTING_LIB_SOURCES - geofunctions.cc spirent_motion_csv_dump_reader.cc rtklib_solver_dump_reader.cc ) From 18db62dfd391991423af074ccbd2c311e8a5bfa4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 5 Nov 2018 19:18:05 +0100 Subject: [PATCH 22/26] Fix building if Armadillo was not installed --- src/algorithms/libs/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index ce4560e6f..8ee0be55c 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -102,6 +102,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} @@ -129,6 +130,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} @@ -137,7 +139,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}) From e3446e47caa84723b91eaf30560a4d305098d4fa Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 5 Nov 2018 19:42:59 +0100 Subject: [PATCH 23/26] Add missing header to the list --- src/algorithms/libs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index 8ee0be55c..de4efc084 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -64,6 +64,7 @@ set(GNSS_SPLIBS_HEADERS conjugate_ic.h gnss_sdr_create_directory.h gnss_circular_deque.h + geofunctions.h ) From ea3db59fd720b4493902538580e63f9e95131b15 Mon Sep 17 00:00:00 2001 From: Javier Date: Tue, 6 Nov 2018 14:39:57 +0100 Subject: [PATCH 24/26] Adding Tele Command status for reporting the Position, Speed and Course over ground and Time --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 15 +++++ src/algorithms/PVT/adapters/rtklib_pvt.h | 7 +++ .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 28 ++++++++++ .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 13 ++++- src/algorithms/PVT/libs/pvt_solution.cc | 22 ++++++++ src/algorithms/PVT/libs/pvt_solution.h | 14 ++++- src/algorithms/PVT/libs/rtklib_solver.cc | 16 ++++++ src/core/interfaces/pvt_interface.h | 7 +++ src/core/receiver/control_thread.cc | 33 ++++++++--- src/core/receiver/gnss_flowgraph.cc | 10 +--- src/core/receiver/tcp_cmd_interface.cc | 56 ++++++++++++++++++- src/core/receiver/tcp_cmd_interface.h | 6 +- 12 files changed, 204 insertions(+), 23 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index c5532dea2..126813e26 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -49,6 +49,21 @@ 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(); diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index 75e9c6c6d..93f238733 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -86,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; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 69cc62264..98202bb68 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -47,6 +47,7 @@ #include #include #include +#include #if OLD_BOOST #include namespace bc = boost::math; @@ -893,9 +894,36 @@ bool rtklib_pvt_cc::load_gnss_synchro_map_xml(const std::string file_name) } +bool rtklib_pvt_cc::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) +{ + gr::thread::scoped_lock lock(d_setlock); + if (d_ls_pvt->is_valid_position()) + { + *latitude_deg = d_ls_pvt->get_latitude(); + *longitude_deg = d_ls_pvt->get_longitude(); + *height_m = d_ls_pvt->get_height(); + *ground_speed_kmh = d_ls_pvt->get_speed_over_ground() * 3600.0 / 1000.0; + *course_over_ground_deg = d_ls_pvt->get_course_over_ground(); + *UTC_time = boost::posix_time::to_time_t(d_ls_pvt->get_position_UTC_time()); + + return true; + } + else + { + return false; + } +} + int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items __attribute__((unused))) { + gr::thread::scoped_lock l(d_setlock); + for (int32_t epoch = 0; epoch < noutput_items; epoch++) { bool flag_display_pvt = false; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index f063431d1..bf53bfd3d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.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 { @@ -159,6 +159,17 @@ public: */ 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 int work(int noutput_items, gr_vector_const_void_star& input_items, diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index e71030085..0ecbd15eb 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -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 { diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 958bf4668..585703d68 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -49,9 +49,11 @@ class Pvt_Solution private: double d_rx_dt_s; // RX time offset [s] - double d_latitude_d; // RX position Latitude WGS84 [deg] - double d_longitude_d; // RX position Longitude WGS84 [deg] - double d_height_m; // RX position height WGS84 [m] + double d_latitude_d; // RX position Latitude WGS84 [deg] + double d_longitude_d; // RX position Longitude WGS84 [deg] + double d_height_m; // RX position height WGS84 [m] + double d_speed_over_ground_m_s; // RX speed over ground [m/s] + double d_course_over_ground_d; // RX course over ground [deg] double d_avg_latitude_d; // Averaged latitude in degrees double d_avg_longitude_d; // Averaged longitude in degrees @@ -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] diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 12d316196..a4e1251e9 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -827,6 +827,22 @@ bool rtklib_solver::get_PVT(const std::map &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] diff --git a/src/core/interfaces/pvt_interface.h b/src/core/interfaces/pvt_interface.h index c9a2db162..db37d9afd 100644 --- a/src/core/interfaces/pvt_interface.h +++ b/src/core/interfaces/pvt_interface.h @@ -61,6 +61,13 @@ public: virtual std::map get_galileo_ephemeris() = 0; virtual std::map get_gps_almanac() = 0; virtual std::map 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_ */ diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index c95973797..b8bf6e345 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -161,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); @@ -718,25 +719,33 @@ void ControlThread::apply_action(unsigned int what) 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: - DLOG(INFO) << "Receiver action HOTSTART"; + 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: - DLOG(INFO) << "Receiver action WARMSTART"; + 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(); @@ -750,7 +759,7 @@ void ControlThread::apply_action(unsigned int what) //start again the satellite acquisitions (done in chained applyaction to flowgraph) break; default: - DLOG(INFO) << "Unrecognized action."; + LOG(INFO) << "Unrecognized action."; break; } } @@ -775,6 +784,13 @@ std::vector> ControlThread::get_visible_sats(time std::vector> available_satellites; std::shared_ptr 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 gps_eph_map = pvt_ptr->get_gps_ephemeris(); for (std::map::iterator it = gps_eph_map.begin(); it != gps_eph_map.end(); ++it) @@ -789,10 +805,10 @@ std::vector> ControlThread::get_visible_sats(time 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 GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; //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(floor(El), (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); } @@ -811,10 +827,10 @@ std::vector> ControlThread::get_visible_sats(time 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 Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl; //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(floor(El), (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN)))); } @@ -832,10 +848,10 @@ std::vector> ControlThread::get_visible_sats(time 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 GPS 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(floor(El), (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN)))); } @@ -857,6 +873,7 @@ std::vector> ControlThread::get_visible_sats(time //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(floor(El), (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN)))); } diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 527696d00..ab15b1084 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -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,7 +1132,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } break; case 12: //request hotstart mode - LOG(INFO) << "TC request hotstart"; + LOG(INFO) << "TC request flowgraph hotstart"; for (unsigned int i = 0; i < channels_count_; i++) { unsigned int ch_index = (who + i + 1) % channels_count_; @@ -1164,7 +1160,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } break; case 13: //request warmstart mode - LOG(INFO) << "TC request warmstart"; + LOG(INFO) << "TC request flowgraph warmstart"; //start again the satellite acquisitions for (unsigned int i = 0; i < channels_count_; i++) { diff --git a/src/core/receiver/tcp_cmd_interface.cc b/src/core/receiver/tcp_cmd_interface.cc index b857ee204..5913034b5 100644 --- a/src/core/receiver/tcp_cmd_interface.cc +++ b/src/core/receiver/tcp_cmd_interface.cc @@ -32,8 +32,13 @@ #include "tcp_cmd_interface.h" #include "control_message_factory.h" #include +#include +void TcpCmdInterface::set_pvt(std::shared_ptr PVT_sptr) +{ + PVT_sptr_ = PVT_sptr; +} time_t TcpCmdInterface::get_utc_time() { return receiver_utc_time_; @@ -78,10 +83,51 @@ std::string TcpCmdInterface::standby(const std::vector &commandLine std::string TcpCmdInterface::status(const std::vector &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 &commandLine) @@ -277,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"; diff --git a/src/core/receiver/tcp_cmd_interface.h b/src/core/receiver/tcp_cmd_interface.h index 7da156fe0..fa039116a 100644 --- a/src/core/receiver/tcp_cmd_interface.h +++ b/src/core/receiver/tcp_cmd_interface.h @@ -31,6 +31,7 @@ #ifndef GNSS_SDR_TCPCMDINTERFACE_H_ #define GNSS_SDR_TCPCMDINTERFACE_H_ +#include "pvt_interface.h" #include #include #include @@ -43,7 +44,7 @@ #include #include #include -#include "time.h" +#include class TcpCmdInterface { @@ -61,6 +62,7 @@ public: */ arma::vec get_LLH(); + void set_pvt(std::shared_ptr PVT_sptr); private: std::unordered_map &)>> @@ -83,6 +85,8 @@ private: double rx_latitude_; double rx_longitude_; double rx_altitude_; + + std::shared_ptr PVT_sptr_; }; #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */ From 2bdbaaf9a5eedc937de139a87d8a7c4164d8e926 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 7 Nov 2018 16:03:45 +0100 Subject: [PATCH 25/26] Fix generation of GPGSV and GAGSV messages (NMEA) --- src/algorithms/PVT/libs/nmea_printer.cc | 2 +- src/algorithms/PVT/libs/rtklib_solver.cc | 67 ++++-------------------- src/algorithms/PVT/libs/rtklib_solver.h | 2 +- 3 files changed, 12 insertions(+), 59 deletions(-) diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index 26b18e3d0..c06653680 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -565,7 +565,7 @@ std::string Nmea_Printer::get_GPGSV() // Notice that NMEA 2.1 only supports 12 channels std::stringstream sentence_str; unsigned char buff[200]; - outnmea_gsv(buff, &d_PVT_data->pvt_sol, *d_PVT_data->pvt_ssat); + outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat); sentence_str << buff; return sentence_str.str(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 9024cd01a..b418dbdba 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -75,7 +75,11 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag rtk_ = rtk; for (unsigned int i = 0; i < 4; i++) dop_[i] = 0.0; pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0}; - + ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{{0, 0}}, {{0, 0}}}, {{}, {}}}; + for (unsigned int i = 0; i < MAXSAT; i++) + { + pvt_ssat[i] = ssat0; + } // ############# ENABLE DATA FILE LOG ################# if (d_flag_dump_enabled == true) { @@ -795,7 +799,11 @@ bool rtklib_solver::get_PVT(const std::map &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 azel; @@ -811,61 +819,6 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ } } - for (gnss_observables_iter = gnss_observables_map.cbegin(); - gnss_observables_iter != gnss_observables_map.cend(); - ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system - { - switch (gnss_observables_iter->second.System) - { - case 'E': - { - std::string sig_(gnss_observables_iter->second.Signal); - if (sig_.compare("1B") == 0) - { - unsigned int snr = static_cast(std::round(gnss_observables_iter->second.CN0_dB_hz / 0.25)); - rtk_.ssat[gnss_observables_iter->second.PRN - 1].snr[0] = snr; - pvt_ssat[gnss_observables_iter->second.PRN - 1] = &rtk_.ssat[gnss_observables_iter->second.PRN - 1]; - } - break; - } - case 'G': - { - // GPS L1 - std::string sig_(gnss_observables_iter->second.Signal); - if (sig_.compare("1C") == 0) - { - unsigned int snr = static_cast(std::round(gnss_observables_iter->second.CN0_dB_hz / 0.25)); - rtk_.ssat[gnss_observables_iter->second.PRN - 1].snr[0] = snr; - pvt_ssat[gnss_observables_iter->second.PRN - 1] = &rtk_.ssat[gnss_observables_iter->second.PRN - 1]; - } - // GPS L2 - if (sig_.compare("2S") == 0) - { - } - // GPS L5 - if (sig_.compare("L5") == 0) - { - } - break; - } - case 'R': - { - std::string sig_(gnss_observables_iter->second.Signal); - // GLONASS GNAV L1 - if (sig_.compare("1G") == 0) - { - } - // GLONASS GNAV L2 - if (sig_.compare("2G") == 0) - { - } - break; - } - default: - break; - } - } - if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_); this->set_valid_position(true); arma::vec rx_position_and_time(4); diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 1e9684685..293c0e06d 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -86,7 +86,7 @@ private: public: sol_t pvt_sol; - ssat_t* pvt_ssat[MAXSAT]; + 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(); From e9ec521f241f79a2cddec29c44c379e6bc9d2f8b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 7 Nov 2018 16:53:40 +0100 Subject: [PATCH 26/26] Fix GPGGA, GPGSA, GAGSA and GPRMC messages (NMEA) --- src/algorithms/PVT/libs/nmea_printer.cc | 283 +----------------------- 1 file changed, 10 insertions(+), 273 deletions(-) diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index c06653680..7253b9f79 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -377,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(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(); } @@ -478,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(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(); } @@ -564,7 +403,7 @@ std::string Nmea_Printer::get_GPGSV() // $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[200]; + 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(); @@ -573,112 +412,10 @@ std::string Nmea_Printer::get_GPGSV() 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(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 }