From 7d045baa905ee8fed1964c53f68924d6cc8156b5 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 10:47:09 +0100 Subject: [PATCH 1/6] Fix name of saved file --- src/utils/reproducibility/ieee-access18/plot_dump.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/utils/reproducibility/ieee-access18/plot_dump.m b/src/utils/reproducibility/ieee-access18/plot_dump.m index 5865235ff..11aaba424 100644 --- a/src/utils/reproducibility/ieee-access18/plot_dump.m +++ b/src/utils/reproducibility/ieee-access18/plot_dump.m @@ -62,8 +62,8 @@ ylabel('Navigation data bits','fontname','Times','fontsize', fontsize) grid on -fileID = fopen('data/access18_pvt.dat', 'r'); -dinfo = dir('data/access18_pvt.dat'); +fileID = fopen('data/access18.dat', 'r'); +dinfo = dir('data/access18.dat'); filesize = dinfo.bytes; aux = 1; while ne(ftell(fileID), filesize) From 257099fee1a5569428c00ce30f016034f4a8c621 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 11:06:48 +0100 Subject: [PATCH 2/6] Fix warning --- .../observables/hybrid_observables_test.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index d3f232a50..9422e144a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -1836,9 +1836,9 @@ TEST_F(HybridObservablesTest, ValidationOfResults) if (FLAGS_duplicated_satellites_test) { //special test mode for duplicated satellites - std::vector prn_pairs; + std::vector prn_pairs; std::stringstream ss(FLAGS_duplicated_satellites_prns); - int i; + unsigned int i; while (ss >> i) { prn_pairs.push_back(i); From 39a062ca008370ba33a6780dd96aa179afba25d4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 31 Oct 2018 14:56:59 +0100 Subject: [PATCH 3/6] 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 4/6] 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 5/6] 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 6/6] 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 @@ +