From e282b075f4a00460675b99b54dfae5d8c51ca08a Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 30 Aug 2018 14:34:05 +0200 Subject: [PATCH 01/35] Improving position test for dynamic scenarios --- .../libs/rtklib_solver_dump_reader.cc | 36 ++-- .../libs/spirent_motion_csv_dump_reader.cc | 7 +- src/tests/system-tests/position_test.cc | 188 ++++++++++++++++++ 3 files changed, 210 insertions(+), 21 deletions(-) diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc index 2c2fc87f0..286a5fc54 100644 --- a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc @@ -55,11 +55,11 @@ bool rtklib_solver_dump_reader::read_binary_obs() // std::cout << "qr" << qr[n] << std::endl; // } d_dump_file.read(reinterpret_cast(&latitude), sizeof(latitude)); - std::cout << "latitude: " << latitude << std::endl; + //std::cout << "latitude: " << latitude << std::endl; d_dump_file.read(reinterpret_cast(&longitude), sizeof(longitude)); - std::cout << "longitude: " << longitude << std::endl; + //std::cout << "longitude: " << longitude << std::endl; d_dump_file.read(reinterpret_cast(&height), sizeof(height)); - std::cout << "height: " << height << std::endl; + //std::cout << "height: " << height << std::endl; d_dump_file.read(reinterpret_cast(&ns), sizeof(ns)); // std::cout << "ns: " << (int)ns << std::endl; d_dump_file.read(reinterpret_cast(&status), sizeof(status)); @@ -103,22 +103,20 @@ bool rtklib_solver_dump_reader::restart() int64_t rtklib_solver_dump_reader::num_epochs() { - // std::ifstream::pos_type size; - // int number_of_double_vars = 1; - // int number_of_float_vars = 17; - // int epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + - // sizeof(float) * number_of_float_vars + sizeof(unsigned int); - // std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); - // if (tmpfile.is_open()) - // { - // size = tmpfile.tellg(); - // int64_t nepoch = size / epoch_size_bytes; - // return nepoch; - // } - // else - // { - return 0; - // } + std::ifstream::pos_type size; + int epoch_size_bytes = sizeof(TOW_at_current_symbol_ms) + sizeof(week) + sizeof(RX_time) + sizeof(clk_offset_s) + sizeof(rr) + sizeof(qr) + sizeof(latitude) + sizeof(longitude) + sizeof(height) + sizeof(ns) + sizeof(status) + sizeof(type) + sizeof(AR_ratio) + sizeof(AR_thres) + sizeof(dop); + + std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + if (tmpfile.is_open()) + { + size = tmpfile.tellg(); + int64_t nepoch = size / epoch_size_bytes; + return nepoch; + } + else + { + return 0; + } } diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc index f4b3d741c..edbf12ad0 100644 --- a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc @@ -54,6 +54,7 @@ bool spirent_motion_csv_dump_reader::read_csv_obs() vec.push_back(0.0); } } + parse_vector(vec); } } catch (const std::ifstream::failure &e) @@ -137,11 +138,13 @@ int64_t spirent_motion_csv_dump_reader::num_epochs() { int64_t nepoch = 0; std::string line; - std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + std::ifstream tmpfile(d_dump_filename.c_str()); if (tmpfile.is_open()) { while (std::getline(tmpfile, line)) - ++nepoch; + { + ++nepoch; + } return nepoch - header_lines; } else diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 21e8bfd81..5adc6636f 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -462,6 +462,16 @@ void StaticPositionSystemTest::check_results() std::vector pos_n; std::vector pos_u; + arma::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3) + arma::mat V_eb_e; //ECEF velocity (x,y,z) estimation in the Earth frame (Nx3) + arma::mat LLH; //Geodetic coordinates (latitude, longitude, height) estimation in WGS84 datum + arma::vec receiver_time_s; + + arma::mat ref_R_eb_e; //ECEF position (x,y,z) reference in the Earth frame (Nx3) + arma::mat ref_V_eb_e; //ECEF velocity (x,y,z) reference in the Earth frame (Nx3) + arma::mat ref_LLH; //Geodetic coordinates (latitude, longitude, height) reference in WGS84 datum + arma::vec ref_time_s; + std::istringstream iss2(FLAGS_static_position); std::string str_aux; std::getline(iss2, str_aux, ','); @@ -538,6 +548,12 @@ void StaticPositionSystemTest::check_results() //use complete binary dump from pvt solver rtklib_solver_dump_reader pvt_reader; pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename); + int64_t n_epochs = pvt_reader.num_epochs(); + R_eb_e = arma::zeros(n_epochs, 3); + V_eb_e = arma::zeros(n_epochs, 3); + LLH = arma::zeros(n_epochs, 3); + receiver_time_s = arma::zeros(n_epochs, 1); + int64_t current_epoch = 0; while (pvt_reader.read_binary_obs()) { double north, east, up; @@ -548,7 +564,27 @@ void StaticPositionSystemTest::check_results() pos_n.push_back(north); pos_u.push_back(up); // getchar(); + + // receiver_time_s(current_epoch) = static_cast(pvt_reader.TOW_at_current_symbol_ms) / 1000.0; + receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s; + R_eb_e(current_epoch, 0) = pvt_reader.rr[0]; + R_eb_e(current_epoch, 1) = pvt_reader.rr[1]; + R_eb_e(current_epoch, 2) = pvt_reader.rr[2]; + V_eb_e(current_epoch, 0) = pvt_reader.rr[3]; + V_eb_e(current_epoch, 1) = pvt_reader.rr[4]; + V_eb_e(current_epoch, 2) = pvt_reader.rr[5]; + LLH(current_epoch, 0) = pvt_reader.latitude; + LLH(current_epoch, 1) = pvt_reader.longitude; + LLH(current_epoch, 2) = pvt_reader.height; + + //debug check + // std::cout << "t1: " << pvt_reader.RX_time << std::endl; + // std::cout << "t2: " << pvt_reader.TOW_at_current_symbol_ms << std::endl; + // std::cout << "offset: " << pvt_reader.clk_offset_s << std::endl; + // getchar(); + current_epoch++; } + ASSERT_FALSE(current_epoch == 0) << "PVT dump is empty"; } // compute results @@ -618,6 +654,158 @@ void StaticPositionSystemTest::check_results() else { //dynamic position + spirent_motion_csv_dump_reader ref_reader; + ref_reader.open_obs_file(FLAGS_ref_motion_filename); + int64_t n_epochs = ref_reader.num_epochs(); + ref_R_eb_e = arma::zeros(n_epochs, 3); + ref_V_eb_e = arma::zeros(n_epochs, 3); + ref_LLH = arma::zeros(n_epochs, 3); + ref_time_s = arma::zeros(n_epochs, 1); + int64_t current_epoch = 0; + while (ref_reader.read_csv_obs()) + { + ref_time_s(current_epoch) = ref_reader.TOW_ms / 1000.0; + ref_R_eb_e(current_epoch, 0) = ref_reader.Pos_X; + ref_R_eb_e(current_epoch, 1) = ref_reader.Pos_Y; + ref_R_eb_e(current_epoch, 2) = ref_reader.Pos_Z; + ref_V_eb_e(current_epoch, 0) = ref_reader.Vel_X; + ref_V_eb_e(current_epoch, 1) = ref_reader.Vel_Y; + ref_V_eb_e(current_epoch, 2) = ref_reader.Vel_Z; + ref_LLH(current_epoch, 0) = ref_reader.Lat; + ref_LLH(current_epoch, 1) = ref_reader.Long; + ref_LLH(current_epoch, 2) = ref_reader.Height; + current_epoch++; + } + + //interpolation of reference data to receiver epochs timestamps + arma::mat ref_interp_R_eb_e = arma::zeros(R_eb_e.n_rows, 3); + arma::mat ref_interp_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); + arma::mat ref_interp_LLH = arma::zeros(LLH.n_rows, 3); + arma::vec tmp_vector; + for (int n = 0; n < 3; n++) + { + arma::interp1(ref_time_s, ref_R_eb_e.col(n), receiver_time_s, tmp_vector); + ref_interp_R_eb_e.col(n) = tmp_vector; + arma::interp1(ref_time_s, ref_V_eb_e.col(n), receiver_time_s, tmp_vector); + ref_interp_V_eb_e.col(n) = tmp_vector; + arma::interp1(ref_time_s, ref_LLH.col(n), receiver_time_s, tmp_vector); + ref_interp_LLH.col(n) = tmp_vector; + } + + //compute error vectors + + arma::mat error_R_eb_e = arma::zeros(R_eb_e.n_rows, 3); + arma::mat error_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); + arma::mat error_LLH = arma::zeros(LLH.n_rows, 3); + error_R_eb_e = R_eb_e - ref_interp_R_eb_e; + error_V_eb_e = V_eb_e - ref_interp_V_eb_e; + error_LLH = LLH - ref_interp_LLH; + arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_rows, 1); + arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_rows, 1); + for (uint64_t n = 0; n < R_eb_e.n_rows; n++) + { + error_module_R_eb_e(n) = arma::norm(error_R_eb_e.row(n)); + error_module_V_eb_e(n) = arma::norm(error_V_eb_e.row(n)); + } + //Error statistics + arma::vec tmp_vec; + //RMSE, Mean, Variance and peaks + tmp_vec = arma::square(error_module_R_eb_e); + double rmse_R_eb_e = sqrt(arma::mean(tmp_vec)); + double error_mean_R_eb_e = arma::mean(error_module_R_eb_e); + double error_var_R_eb_e = arma::var(error_module_R_eb_e); + double max_error_R_eb_e = arma::max(error_module_R_eb_e); + double min_error_R_eb_e = arma::min(error_module_R_eb_e); + + tmp_vec = arma::square(error_module_V_eb_e); + double rmse_V_eb_e = sqrt(arma::mean(tmp_vec)); + double error_mean_V_eb_e = arma::mean(error_module_V_eb_e); + double error_var_V_eb_e = arma::var(error_module_V_eb_e); + double max_error_V_eb_e = arma::max(error_module_V_eb_e); + double min_error_V_eb_e = arma::min(error_module_V_eb_e); + + //report + std::cout << "----- Position and Velocity 3D ECEF error statistics -----" << std::endl; + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << "---- 3D ECEF Position RMSE = " + << rmse_R_eb_e << ", mean = " << error_mean_R_eb_e + << ", stdev = " << sqrt(error_var_R_eb_e) + << " (max,min) = " << max_error_R_eb_e + << "," << min_error_R_eb_e + << " [m]" << std::endl; + std::cout << "---- 3D ECEF Velocity RMSE = " + << rmse_V_eb_e << ", mean = " << error_mean_V_eb_e + << ", stdev = " << sqrt(error_var_V_eb_e) + << " (max,min) = " << max_error_V_eb_e + << "," << min_error_V_eb_e + << " [m/s]" << std::endl; + std::cout.precision(ss); + + //plots + Gnuplot g1("points"); + if (FLAGS_show_plots) + { + g1.showonscreen(); // window output + } + else + { + g1.disablescreen(); + } + g1.set_title("3D ECEF error coordinates"); + g1.set_grid(); + //conversion between arma::vec and std:vector + std::vector X(error_R_eb_e.colptr(0), error_R_eb_e.colptr(0) + error_R_eb_e.n_rows); + std::vector Y(error_R_eb_e.colptr(1), error_R_eb_e.colptr(1) + error_R_eb_e.n_rows); + std::vector Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows); + + g1.cmd("set key box opaque"); + g1.plot_xyz(X, Y, Z, "ECEF_3d_error"); + g1.set_legend(); + g1.savetops("ECEF_3d_error"); + + + arma::vec time_vector_from_start_s = receiver_time_s - receiver_time_s(0); + Gnuplot g3("linespoints"); + if (FLAGS_show_plots) + { + g3.showonscreen(); // window output + } + else + { + g3.disablescreen(); + } + g3.set_title("3D Position estimation error module [m]"); + g3.set_grid(); + g3.set_xlabel("Receiver epoch time from first valid PVT [s]"); + g3.set_ylabel("3D Position error [m]"); + //conversion between arma::vec and std:vector + std::vector error_vec(error_module_R_eb_e.colptr(0), error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows); + g3.cmd("set key box opaque"); + g3.plot_xy(time_vector_from_start_s, error_vec, + "Position_3d_error"); + g3.set_legend(); + g3.savetops("Position_3d_error"); + + Gnuplot g4("linespoints"); + if (FLAGS_show_plots) + { + g4.showonscreen(); // window output + } + else + { + g4.disablescreen(); + } + g4.set_title("3D Velocity estimation error module [m/s]"); + g4.set_grid(); + g4.set_xlabel("Receiver epoch time from first valid PVT [s]"); + g4.set_ylabel("3D Velocity error [m/s]"); + //conversion between arma::vec and std:vector + std::vector error_vec2(error_module_V_eb_e.colptr(0), error_module_V_eb_e.colptr(0) + error_module_V_eb_e.n_rows); + g4.cmd("set key box opaque"); + g4.plot_xy(time_vector_from_start_s, error_vec2, + "Velocity_3d_error"); + g4.set_legend(); + g4.savetops("Velocity_3d_error"); } } From 774e50545e5d8b32b6e09317a77347e1c46c7751 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Aug 2018 19:24:28 +0200 Subject: [PATCH 02/35] Fix building if Armadillo was not installed in the system Code cleaning Keep consistency with names of include guards Fix some defects detected by Coverity Scan --- .../galileo_telemetry_decoder_cc.cc | 27 +-- .../galileo_telemetry_decoder_cc.h | 5 +- src/tests/system-tests/libs/CMakeLists.txt | 5 +- src/tests/system-tests/libs/geofunctions.cc | 135 +++++++------ src/tests/system-tests/libs/geofunctions.h | 182 +++++++++--------- .../libs/rtklib_solver_dump_reader.cc | 27 --- .../libs/spirent_motion_csv_dump_reader.cc | 7 +- .../libs/spirent_motion_csv_dump_reader.h | 8 +- 8 files changed, 191 insertions(+), 205 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc index aa53529fe..41eb414ed 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc @@ -87,7 +87,7 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc( switch (d_frame_type) { - case 1: //INAV + case 1: // INAV { d_PRN_code_period_ms = static_cast(GALILEO_E1_CODE_PERIOD_MS); d_samples_per_symbol = Galileo_E1_B_SAMPLES_PER_SYMBOL; @@ -98,12 +98,13 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc( d_required_symbols = static_cast(GALILEO_INAV_PAGE_SYMBOLS) + d_samples_per_preamble; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_secondary_code_samples = nullptr; d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; break; } - case 2: //FNAV + case 2: // FNAV { d_PRN_code_period_ms = static_cast(GALILEO_E5a_CODE_PERIOD_MS); d_samples_per_symbol = GALILEO_FNAV_CODES_PER_SYMBOL; @@ -114,7 +115,6 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc( d_required_symbols = static_cast(GALILEO_FNAV_SYMBOLS_PER_PAGE) * d_samples_per_symbol + d_samples_per_preamble; // preamble bits to sampled symbols d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_secondary_code_samples = static_cast(volk_gnsssdr_malloc(Galileo_E5a_I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); d_frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; @@ -142,7 +142,7 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc( { switch (d_frame_type) { - case 1: //INAV + case 1: // INAV { if (GALILEO_INAV_PREAMBLE.at(i) == '1') { @@ -162,7 +162,7 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc( } break; } - case 2: //FNAV for E5a-I + case 2: // FNAV for E5a-I { // Galileo E5a data channel (E5a-I) still has a secondary code int m = 0; @@ -412,6 +412,7 @@ void galileo_telemetry_decoder_cc::decode_FNAV_word(double *page_symbols, int32_ } } + void galileo_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite) { d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); @@ -525,7 +526,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( // call the decoder switch (d_frame_type) { - case 1: //INAV + case 1: // INAV // NEW Galileo page part is received // 0. fetch the symbols into an array if (corr_value > 0) //normal PLL lock @@ -544,7 +545,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( } decode_INAV_word(d_page_part_symbols, d_frame_length_symbols); break; - case 2: //FNAV + case 2: // FNAV // NEW Galileo page part is received // 0. fetch the symbols into an array if (corr_value > 0) //normal PLL lock @@ -620,7 +621,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( { switch (d_frame_type) { - case 1: //INAV + case 1: // INAV { if (d_inav_nav.flag_TOW_set == true) { @@ -647,7 +648,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( } break; } - case 2: //FNAV + case 2: // FNAV { if (d_fnav_nav.flag_TOW_set == true) { @@ -692,7 +693,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( { switch (d_frame_type) { - case 1: //INAV + case 1: // INAV { if (d_inav_nav.flag_TOW_set == true) { @@ -700,7 +701,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( } break; } - case 2: //FNAV + case 2: // FNAV { if (d_fnav_nav.flag_TOW_set == true) { @@ -720,7 +721,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( switch (d_frame_type) { - case 1: //INAV + case 1: // INAV { if (d_inav_nav.flag_TOW_set) { @@ -734,7 +735,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( break; } - case 2: //FNAV + case 2: // FNAV { if (d_fnav_nav.flag_TOW_set) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h index f67418868..a009fe66a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h @@ -29,8 +29,8 @@ */ -#ifndef GNSS_SDR_galileo_telemetry_decoder_cc_H -#define GNSS_SDR_galileo_telemetry_decoder_cc_H +#ifndef GNSS_SDR_GALILEO_TELEMETRY_DECODER_CC_H +#define GNSS_SDR_GALILEO_TELEMETRY_DECODER_CC_H #include "Galileo_E1.h" @@ -56,7 +56,6 @@ galileo_telemetry_decoder_cc_sptr galileo_make_telemetry_decoder_cc(const Gnss_S /*! * \brief This class implements a block that decodes the INAV and FNAV data defined in Galileo ICD - * */ class galileo_telemetry_decoder_cc : public gr::block { diff --git a/src/tests/system-tests/libs/CMakeLists.txt b/src/tests/system-tests/libs/CMakeLists.txt index a6459b058..0fe1d4440 100644 --- a/src/tests/system-tests/libs/CMakeLists.txt +++ b/src/tests/system-tests/libs/CMakeLists.txt @@ -28,6 +28,7 @@ include_directories( ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} ${MATIO_INCLUDE_DIRS} + ${ARMADILLO_INCLUDE_DIRS} ) @@ -37,5 +38,7 @@ add_library(system_testing_lib ${SYSTEM_TESTING_LIB_SOURCES} ${SYSTEM_TESTING_LI source_group(Headers FILES ${SYSTEM_TESTING_LIB_HEADERS}) if(NOT MATIO_FOUND) - add_dependencies(system_testing_lib matio-${GNSSSDR_MATIO_LOCAL_VERSION}) + add_dependencies(system_testing_lib armadillo-${armadillo_RELEASE} matio-${GNSSSDR_MATIO_LOCAL_VERSION}) +else(NOT MATIO_FOUND) + add_dependencies(system_testing_lib armadillo-${armadillo_RELEASE}) endif(NOT MATIO_FOUND) \ No newline at end of file diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc index fbbe97266..bbcd09f60 100644 --- a/src/tests/system-tests/libs/geofunctions.cc +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -1,5 +1,5 @@ /*! - * \file geofunctions.h + * \file geofunctions.cc * \brief A set of coordinate transformations functions and helpers, * some of them migrated from MATLAB, for geographic information systems. * \author Javier Arribas, 2018. jarribas(at)cttc.es @@ -30,10 +30,10 @@ */ #include "geofunctions.h" -#define STRP_G_SI 9.80665 -#define STRP_PI 3.1415926535898 //!< Pi as defined in IS-GPS-200E +const double STRP_G_SI = 9.80665; +const double STRP_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E -arma::mat Skew_symmetric(arma::vec a) +arma::mat Skew_symmetric(const arma::vec &a) { arma::mat A = arma::zeros(3, 3); @@ -41,54 +41,55 @@ arma::mat Skew_symmetric(arma::vec a) << a(2) << 0.0 << -a(0) << arma::endr << -a(1) << a(0) << 0 << arma::endr; - // {{0, -a(2), a(1)}, - // {a(2), 0, -a(0)}, - // {-a(1), a(0), 0}}; + // {{0, -a(2), a(1)}, + // {a(2), 0, -a(0)}, + // {-a(1), a(0), 0}}; return A; } double WGS84_g0(double Lat_rad) { - double k = 0.001931853; //normal gravity constant - double e2 = 0.00669438002290; //the square of the first numerical eccentricity - double nge = 9.7803253359; //normal gravity value on the equator (m/sec^2) - double b = sin(Lat_rad); //Lat in degrees + const double k = 0.001931853; // normal gravity constant + const double e2 = 0.00669438002290; // the square of the first numerical eccentricity + const double nge = 9.7803253359; // normal gravity value on the equator (m/sec^2) + double b = sin(Lat_rad); // Lat in degrees b = b * b; double g0 = nge * (1 + k * b) / (sqrt(1 - e2 * b)); return g0; } + double WGS84_geocentric_radius(double Lat_geodetic_rad) { - //WGS84 earth model Geocentric radius (Eq. 2.88) + // WGS84 earth model Geocentric radius (Eq. 2.88) + const double WGS84_A = 6378137.0; // Semi-major axis of the Earth, a [m] + const double WGS84_IF = 298.257223563; // Inverse flattening of the Earth + const double WGS84_F = (1.0 / WGS84_IF); // The flattening of the Earth + // double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m] + double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); // Eccentricity of the Earth - double WGS84_A = 6378137.0; //Semi-major axis of the Earth, a [m] - double WGS84_IF = 298.257223563; //Inverse flattening of the Earth - double WGS84_F = (1 / WGS84_IF); //The flattening of the Earth - //double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m] - double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); //Eccentricity of the Earth - - //transverse radius of curvature + // transverse radius of curvature double R_E = WGS84_A / sqrt(1 - WGS84_E * WGS84_E * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.66) - //gocentric radius at the Earth surface + // geocentric radius at the Earth surface double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) + (1 - WGS84_E * WGS84_E) * (1 - WGS84_E * WGS84_E) * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.88) return r_eS; } + int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx) { double lambda; double phi; double h; - double dtr = STRP_PI / 180.0; - double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84 - double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84 + const double dtr = STRP_PI / 180.0; + const double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84 + const double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84 // Transform x into geodetic coordinates togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); @@ -147,9 +148,9 @@ int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma:: int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) { *h = 0; - double tolsq = 1.e-10; // tolerance to accept convergence - int maxit = 10; // max number of iterations - double rtd = 180.0 / STRP_PI; + const double tolsq = 1.e-10; // tolerance to accept convergence + const int maxit = 10; // max number of iterations + const double rtd = 180.0 / STRP_PI; // compute square of eccentricity double esq; @@ -165,7 +166,7 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub // first guess double P = sqrt(X * X + Y * Y); // P is distance from spin axis - //direct calculation of longitude + // direct calculation of longitude if (P > 1.0E-20) { *dlambda = atan2(Y, X) * rtd; @@ -241,27 +242,28 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub return 0; } -arma::mat Gravity_ECEF(arma::vec r_eb_e) + +arma::mat Gravity_ECEF(const arma::vec &r_eb_e) { - //Parameters - double R_0 = 6378137; //WGS84 Equatorial radius in meters - double mu = 3.986004418E14; //WGS84 Earth gravitational constant (m^3 s^-2) - double J_2 = 1.082627E-3; //WGS84 Earth's second gravitational constant - double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s) + // Parameters + const double R_0 = 6378137; // WGS84 Equatorial radius in meters + const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2) + const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant + const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s) // Calculate distance from center of the Earth double mag_r = sqrt(arma::as_scalar(r_eb_e.t() * r_eb_e)); // If the input position is 0,0,0, produce a dummy output arma::vec g = arma::zeros(3, 1); if (mag_r != 0) { - //Calculate gravitational acceleration using (2.142) + // Calculate gravitational acceleration using (2.142) double z_scale = 5 * pow((r_eb_e(2) / mag_r), 2); arma::vec tmp_vec = {(1 - z_scale) * r_eb_e(0), (1 - z_scale) * r_eb_e(1), (3 - z_scale) * r_eb_e(2)}; arma::vec gamma_ = (-mu / pow(mag_r, 3)) * (r_eb_e + 1.5 * J_2 * pow(R_0 / mag_r, 2) * tmp_vec); - //Add centripetal acceleration using (2.133) + // Add centripetal acceleration using (2.133) g(0) = gamma_(0) + pow(omega_ie, 2) * r_eb_e(0); g(1) = gamma_(1) + pow(omega_ie, 2) * r_eb_e(1); g(2) = gamma_(2); @@ -269,20 +271,23 @@ arma::mat Gravity_ECEF(arma::vec r_eb_e) return g; } -arma::vec LLH_to_deg(arma::vec LLH) + +arma::vec LLH_to_deg(arma::vec &LLH) { - double rtd = 180.0 / STRP_PI; + const double rtd = 180.0 / STRP_PI; LLH(0) = LLH(0) * rtd; LLH(1) = LLH(1) * rtd; return LLH; } + double degtorad(double angleInDegrees) { double angleInRadians = (STRP_PI / 180.0) * angleInDegrees; return angleInRadians; } + double radtodeg(double angleInRadians) { double angleInDegrees = (180.0 / STRP_PI) * angleInRadians; @@ -296,15 +301,17 @@ double mstoknotsh(double MetersPerSeconds) return knots; } + double mstokph(double MetersPerSeconds) { double kph = 3600.0 * MetersPerSeconds / 1e3; return kph; } -arma::vec CTM_to_Euler(arma::mat C) + +arma::vec CTM_to_Euler(arma::mat &C) { - //Calculate Euler angles using (2.23) + // Calculate Euler angles using (2.23) arma::vec eul = arma::zeros(3, 1); eul(0) = atan2(C(1, 2), C(2, 2)); // roll if (C(0, 2) < -1.0) C(0, 2) = -1.0; @@ -314,18 +321,19 @@ arma::vec CTM_to_Euler(arma::mat C) return eul; } -arma::mat Euler_to_CTM(arma::vec eul) + +arma::mat Euler_to_CTM(const arma::vec &eul) { - //Eq.2.15 - //Euler angles to Attitude matrix is equivalent to rotate the body - //in the three axes: + // Eq.2.15 + // Euler angles to Attitude matrix is equivalent to rotate the body + // in the three axes: // arma::mat Ax= {{1,0,0}, {0,cos(Att_phi),sin(Att_phi)} ,{0,-sin(Att_phi),cos(Att_phi)}}; // arma::mat Ay= {{cos(Att_theta), 0, -sin(Att_theta)}, {0,1,0} , {sin(Att_theta), 0, cos(Att_theta)}}; // arma::mat Az= {{cos(Att_psi), sin(Att_psi), 0}, {-sin(Att_psi), cos(Att_psi), 0},{0,0,1}}; // arma::mat C_b_n=Ax*Ay*Az; // Attitude expressed in the LOCAL FRAME (NED) // C_b_n=C_b_n.t(); - //Precalculate sines and cosines of the Euler angles + // Precalculate sines and cosines of the Euler angles double sin_phi = sin(eul(0)); double cos_phi = cos(eul(0)); double sin_theta = sin(eul(1)); @@ -334,7 +342,7 @@ arma::mat Euler_to_CTM(arma::vec eul) double cos_psi = cos(eul(2)); arma::mat C = arma::zeros(3, 3); - //Calculate coordinate transformation matrix using (2.22) + // Calculate coordinate transformation matrix using (2.22) C(0, 0) = cos_theta * cos_psi; C(0, 1) = cos_theta * sin_psi; C(0, 2) = -sin_theta; @@ -347,7 +355,8 @@ arma::mat Euler_to_CTM(arma::vec eul) return C; } -arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection) + +arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection) { const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563}; @@ -380,37 +389,37 @@ arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection) return LLH; } -void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n) + +void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::mat &C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n) { - //Compute the Latitude of the ECEF position - LLH = cart2geo(r_eb_e, 4); //ECEF -> WGS84 geographical + // Compute the Latitude of the ECEF position + LLH = cart2geo(r_eb_e, 4); // ECEF -> WGS84 geographical // Calculate ECEF to Geographical coordinate transformation matrix using (2.150) double cos_lat = cos(LLH(0)); double sin_lat = sin(LLH(0)); double cos_long = cos(LLH(1)); double sin_long = sin(LLH(1)); - //C++11 and arma >= 5.2 + // C++11 and arma >= 5.2 // arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat}, // {-sin_long, cos_long, 0}, // {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; //ECEF to Geo - - //C++98 arma <5.2 arma::mat C_e_n = arma::zeros(3, 3); C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr << -sin_long << cos_long << 0 << arma::endr - << -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; //ECEF to Geo + << -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; // ECEF to Geo // Transform velocity using (2.73) v_eb_n = C_e_n * v_eb_e; C_b_n = C_e_n * C_b_e; // Attitude conversion from ECEF to NED } -void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e) + +void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e) { // Parameters - double R_0 = 6378137; //WGS84 Equatorial radius in meters - double e = 0.0818191908425; //WGS84 eccentricity + double R_0 = 6378137; // WGS84 Equatorial radius in meters + double e = 0.0818191908425; // WGS84 eccentricity // Calculate transverse radius of curvature using (2.105) double R_E = R_0 / sqrt(1 - (e * sin(LLH(0))) * (e * sin(LLH(0)))); @@ -424,13 +433,11 @@ void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_ (R_E + LLH(2)) * cos_lat * sin_long, ((1 - e * e) * R_E + LLH(2)) * sin_lat}; - //Calculate ECEF to Geo coordinate transformation matrix using (2.150) - //C++11 and arma>=5.2 + // Calculate ECEF to Geo coordinate transformation matrix using (2.150) + // C++11 and arma>=5.2 // arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat}, // {-sin_long, cos_long, 0}, // {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; - //C++98 arma <5.2 - //Calculate ECEF to Geo coordinate transformation matrix using (2.150) arma::mat C_e_n = arma::zeros(3, 3); C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr << -sin_long << cos_long << 0 << arma::endr @@ -444,11 +451,11 @@ void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_ } -void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e) +void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e) { - //% Parameters - double R_0 = 6378137; //WGS84 Equatorial radius in meters - double e = 0.0818191908425; //WGS84 eccentricity + // Parameters + const double R_0 = 6378137; // WGS84 Equatorial radius in meters + const double e = 0.0818191908425; // WGS84 eccentricity // Calculate transverse radius of curvature using (2.105) double R_E = R_0 / sqrt(1 - pow(e * sin(L_b), 2)); diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/tests/system-tests/libs/geofunctions.h index 4cd0fb90d..c03655e17 100644 --- a/src/tests/system-tests/libs/geofunctions.h +++ b/src/tests/system-tests/libs/geofunctions.h @@ -29,72 +29,72 @@ * ------------------------------------------------------------------------- */ -#ifndef GEOFUNCTIONS_H -#define GEOFUNCTIONS_H +#ifndef GNSS_SDR_GEOFUNCTIONS_H +#define GNSS_SDR_GEOFUNCTIONS_H #include -// %Skew_symmetric - Calculates skew-symmetric matrix -arma::mat Skew_symmetric(arma::vec a); + +arma::mat Skew_symmetric(const arma::vec &a); //!< Calculates skew-symmetric matrix double WGS84_g0(double Lat_rad); double WGS84_geocentric_radius(double Lat_geodetic_rad); -/* Transformation of vector dx into topocentric coordinate - system with origin at x - Inputs: - x - vector origin coordinates (in ECEF system [X; Y; Z;]) - dx - vector ([dX; dY; dZ;]). - - Outputs: - D - vector length. Units like the input - Az - azimuth from north positive clockwise, degrees - El - elevation angle, degrees - - Based on a Matlab function by Kai Borre +/*! + * \brief Transformation of vector dx into topocentric coordinate + * system with origin at x + * Inputs: + * x - vector origin coordinates (in ECEF system [X; Y; Z;]) + * dx - vector ([dX; dY; dZ;]). + * + * Outputs: + * D - vector length. Units like the input + * Az - azimuth from north positive clockwise, degrees + * El - elevation angle, degrees + * + * Based on a Matlab function by Kai Borre */ int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx); -/* Subroutine to calculate geodetic coordinates latitude, longitude, - height given Cartesian coordinates X,Y,Z, and reference ellipsoid - values semi-major axis (a) and the inverse of flattening (finv). - - The output units of angular quantities will be in decimal degrees - (15.5 degrees not 15 deg 30 min). The output units of h will be the - same as the units of X,Y,Z,a. - - Inputs: - a - semi-major axis of the reference ellipsoid - finv - inverse of flattening of the reference ellipsoid - X,Y,Z - Cartesian coordinates - - Outputs: - dphi - latitude - dlambda - longitude - h - height above reference ellipsoid - - Based in a Matlab function by Kai Borre +/*! + * \brief Subroutine to calculate geodetic coordinates latitude, longitude, + * height given Cartesian coordinates X,Y,Z, and reference ellipsoid + * values semi-major axis (a) and the inverse of flattening (finv). + * + * The output units of angular quantities will be in decimal degrees + * (15.5 degrees not 15 deg 30 min). The output units of h will be the + * same as the units of X,Y,Z,a. + * + * Inputs: + * a - semi-major axis of the reference ellipsoid + * finv - inverse of flattening of the reference ellipsoid + * X,Y,Z - Cartesian coordinates + * + * Outputs: + * dphi - latitude + * dlambda - longitude + * h - height above reference ellipsoid + * + * Based in a Matlab function by Kai Borre */ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); +arma::mat Gravity_ECEF(const arma::vec &r_eb_e); //!< Calculates acceleration due to gravity resolved about ECEF-frame -//Gravitation_ECI - Calculates acceleration due to gravity resolved about -//ECEF-frame -arma::mat Gravity_ECEF(arma::vec r_eb_e); - -/* Conversion of Cartesian coordinates (X,Y,Z) to geographical - coordinates (latitude, longitude, h) on a selected reference ellipsoid. - - Choices of Reference Ellipsoid for Geographical Coordinates - 0. International Ellipsoid 1924 - 1. International Ellipsoid 1967 - 2. World Geodetic System 1972 - 3. Geodetic Reference System 1980 - 4. World Geodetic System 1984 +/*! + * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical + * coordinates (latitude, longitude, h) on a selected reference ellipsoid. + * + * Choices of Reference Ellipsoid for Geographical Coordinates + * 0. International Ellipsoid 1924 + * 1. International Ellipsoid 1967 + * 2. World Geodetic System 1972 + * 3. Geodetic Reference System 1980 + * 4. World Geodetic System 1984 */ -arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection); +arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection); -arma::vec LLH_to_deg(arma::vec LLH); +arma::vec LLH_to_deg(arma::vec &LLH); double degtorad(double angleInDegrees); @@ -104,53 +104,51 @@ double mstoknotsh(double MetersPerSeconds); double mstokph(double Kph); +arma::vec CTM_to_Euler(arma::mat &C); -arma::vec CTM_to_Euler(arma::mat C); +arma::mat Euler_to_CTM(const arma::vec &eul); -arma::mat Euler_to_CTM(arma::vec eul); - -void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n); +void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::mat &C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n); -// % -// % Inputs: -// % L_b latitude (rad) -// % lambda_b longitude (rad) -// % h_b height (m) -// % v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along -// % north, east, and down (m/s) -// % C_b_n body-to-NED coordinate transformation matrix -// % -// % Outputs: -// % r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved -// % along ECEF-frame axes (m) -// % v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along -// % ECEF-frame axes (m/s) -// % C_b_e body-to-ECEF-frame coordinate transformation matrix -// -// % Copyright 2012, Paul Groves -// % License: BSD; see license.txt for details - -void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e); +/*! + * \brief From Geographic to ECEF coordinates + * + * Inputs: + * LLH latitude (rad), longitude (rad), height (m) + * v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along + * north, east, and down (m/s) + * C_b_n body-to-NED coordinate transformation matrix + * + * Outputs: + * r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved + * along ECEF-frame axes (m) + * v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along + * ECEF-frame axes (m/s) + * C_b_e body-to-ECEF-frame coordinate transformation matrix + * + */ +void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e); -//pv_Geo_to_ECEF - Converts curvilinear to Cartesian position and velocity -//resolving axes from NED to ECEF -//This function created 11/4/2012 by Paul Groves -//% -//% Inputs: -//% L_b latitude (rad) -//% lambda_b longitude (rad) -//% h_b height (m) -//% v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along -//% north, east, and down (m/s) -//% -//% Outputs: -//% r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved -//% along ECEF-frame axes (m) -//% v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along -//% ECEF-frame axes (m/s) - -void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e); +/*! + * \brief Converts curvilinear to Cartesian position and velocity + * resolving axes from NED to ECEF + * This function created 11/4/2012 by Paul Groves + * + * Inputs: + * L_b latitude (rad) + * lambda_b longitude (rad) + * h_b height (m) + * v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along + * north, east, and down (m/s) + * + * Outputs: + * r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved + * along ECEF-frame axes (m) + * v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along + * ECEF-frame axes (m/s) + */ +void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e); #endif diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc index 286a5fc54..d85c0081b 100644 --- a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc @@ -29,7 +29,6 @@ */ #include "rtklib_solver_dump_reader.h" - #include bool rtklib_solver_dump_reader::read_binary_obs() @@ -37,46 +36,20 @@ bool rtklib_solver_dump_reader::read_binary_obs() try { d_dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms), sizeof(TOW_at_current_symbol_ms)); - // std::cout << "TOW_at_current_symbol_ms: " << TOW_at_current_symbol_ms << std::endl; d_dump_file.read(reinterpret_cast(&week), sizeof(week)); - // std::cout << "week: " << week << std::endl; d_dump_file.read(reinterpret_cast(&RX_time), sizeof(RX_time)); - // std::cout << "RX_time: " << RX_time << std::endl; d_dump_file.read(reinterpret_cast(&clk_offset_s), sizeof(clk_offset_s)); - // std::cout << "clk_offset_s: " << clk_offset_s << std::endl; d_dump_file.read(reinterpret_cast(&rr[0]), sizeof(rr)); - // for (int n = 0; n < 6; n++) - // { - // std::cout << "rr: " << rr[n] << std::endl; - // } d_dump_file.read(reinterpret_cast(&qr[0]), sizeof(qr)); - // for (int n = 0; n < 6; n++) - // { - // std::cout << "qr" << qr[n] << std::endl; - // } d_dump_file.read(reinterpret_cast(&latitude), sizeof(latitude)); - //std::cout << "latitude: " << latitude << std::endl; d_dump_file.read(reinterpret_cast(&longitude), sizeof(longitude)); - //std::cout << "longitude: " << longitude << std::endl; d_dump_file.read(reinterpret_cast(&height), sizeof(height)); - //std::cout << "height: " << height << std::endl; d_dump_file.read(reinterpret_cast(&ns), sizeof(ns)); - // std::cout << "ns: " << (int)ns << std::endl; d_dump_file.read(reinterpret_cast(&status), sizeof(status)); - // std::cout << "status: " << (int)status << std::endl; d_dump_file.read(reinterpret_cast(&type), sizeof(type)); - // std::cout << "type: " << (int)type << std::endl; d_dump_file.read(reinterpret_cast(&AR_ratio), sizeof(AR_ratio)); - // std::cout << "AR_ratio: " << AR_ratio << std::endl; d_dump_file.read(reinterpret_cast(&AR_thres), sizeof(AR_thres)); - // std::cout << "AR_thres: " << AR_thres << std::endl; d_dump_file.read(reinterpret_cast(&dop[0]), sizeof(dop)); - - // for (int n = 0; n < 4; n++) - // { - // std::cout << "dop" << dop[n] << std::endl; - // } - // getchar(); } catch (const std::ifstream::failure &e) { diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc index edbf12ad0..d93d8c572 100644 --- a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc @@ -64,6 +64,7 @@ bool spirent_motion_csv_dump_reader::read_csv_obs() return true; } + bool spirent_motion_csv_dump_reader::parse_vector(std::vector &vec) { try @@ -114,6 +115,8 @@ bool spirent_motion_csv_dump_reader::parse_vector(std::vector &vec) return false; } } + + bool spirent_motion_csv_dump_reader::restart() { if (d_dump_file.is_open()) @@ -136,7 +139,7 @@ bool spirent_motion_csv_dump_reader::restart() int64_t spirent_motion_csv_dump_reader::num_epochs() { - int64_t nepoch = 0; + int64_t nepoch = 0LL; std::string line; std::ifstream tmpfile(d_dump_filename.c_str()); if (tmpfile.is_open()) @@ -182,6 +185,7 @@ bool spirent_motion_csv_dump_reader::open_obs_file(std::string out_file) } } + void spirent_motion_csv_dump_reader::close_obs_file() { if (d_dump_file.is_open() == false) @@ -190,6 +194,7 @@ void spirent_motion_csv_dump_reader::close_obs_file() } } + spirent_motion_csv_dump_reader::spirent_motion_csv_dump_reader() { header_lines = 2; diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h index c7fe736cf..e6b6f43eb 100644 --- a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h @@ -28,8 +28,8 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_spirent_motion_csv_dump_READER_H -#define GNSS_SDR_spirent_motion_csv_dump_READER_H +#ifndef GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H +#define GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H #include #include @@ -49,7 +49,7 @@ public: void close_obs_file(); int header_lines; - //dump variables + // dump variables double TOW_ms; double Pos_X; double Pos_Y; @@ -95,4 +95,4 @@ private: bool parse_vector(std::vector &vec); }; -#endif //GNSS_SDR_spirent_motion_csv_dump_READER_H +#endif // GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H From c850adf5e74291a7372f057d63091fa53ad17625 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Aug 2018 19:40:17 +0200 Subject: [PATCH 03/35] Fix defects detected by coverity scan (uninitialized members) --- .../libs/spirent_motion_csv_dump_reader.cc | 69 +++++++++++++++---- 1 file changed, 54 insertions(+), 15 deletions(-) diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc index d93d8c572..ba4714109 100644 --- a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc @@ -31,6 +31,60 @@ #include "spirent_motion_csv_dump_reader.h" #include + +spirent_motion_csv_dump_reader::spirent_motion_csv_dump_reader() +{ + header_lines = 2; + TOW_ms = 0.0; + Pos_X = 0.0; + Pos_Y = 0.0; + Pos_Z = 0.0; + Vel_X = 0.0; + Vel_Y = 0.0; + Vel_Z = 0.0; + Acc_X = 0.0; + Acc_Y = 0.0; + Acc_Z = 0.0; + Jerk_X = 0.0; + Jerk_Y = 0.0; + Jerk_Z = 0.0; + Lat = 0.0; + Long = 0.0; + Height = 0.0; + Heading = 0.0; + Elevation = 0.0; + Bank = 0.0; + Ang_vel_X = 0.0; + Ang_vel_Y = 0.0; + Ang_vel_Z = 0.0; + Ang_acc_X = 0.0; + Ang_acc_Y = 0.0; + Ang_acc_Z = 0.0; + Ant1_Pos_X = 0.0; + Ant1_Pos_Y = 0.0; + Ant1_Pos_Z = 0.0; + Ant1_Vel_X = 0.0; + Ant1_Vel_Y = 0.0; + Ant1_Vel_Z = 0.0; + Ant1_Acc_X = 0.0; + Ant1_Acc_Y = 0.0; + Ant1_Acc_Z = 0.0; + Ant1_Lat = 0.0; + Ant1_Long = 0.0; + Ant1_Height = 0.0; + Ant1_DOP = 0.0; +} + + +spirent_motion_csv_dump_reader::~spirent_motion_csv_dump_reader() +{ + if (d_dump_file.is_open() == true) + { + d_dump_file.close(); + } +} + + bool spirent_motion_csv_dump_reader::read_csv_obs() { try @@ -193,18 +247,3 @@ void spirent_motion_csv_dump_reader::close_obs_file() d_dump_file.close(); } } - - -spirent_motion_csv_dump_reader::spirent_motion_csv_dump_reader() -{ - header_lines = 2; -} - - -spirent_motion_csv_dump_reader::~spirent_motion_csv_dump_reader() -{ - if (d_dump_file.is_open() == true) - { - d_dump_file.close(); - } -} From 1ea88104ac976eb609e6ef568f12c29e35d6ed21 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Aug 2018 19:58:37 +0200 Subject: [PATCH 04/35] Fix defect detected by coverity scan (avoid null pointer dereference) --- src/tests/system-tests/libs/geofunctions.cc | 29 ++++++--------------- 1 file changed, 8 insertions(+), 21 deletions(-) diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc index bbcd09f60..8098609c1 100644 --- a/src/tests/system-tests/libs/geofunctions.cc +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -70,10 +70,7 @@ double WGS84_geocentric_radius(double Lat_geodetic_rad) double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); // Eccentricity of the Earth // transverse radius of curvature - double R_E = WGS84_A / sqrt(1 - - WGS84_E * WGS84_E * - sin(Lat_geodetic_rad) * - sin(Lat_geodetic_rad)); // (Eq. 2.66) + double R_E = WGS84_A / sqrt(1 - WGS84_E * WGS84_E * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.66) // geocentric radius at the Earth surface double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) + @@ -99,19 +96,9 @@ int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma:: double cb = cos(phi * dtr); double sb = sin(phi * dtr); - arma::mat F = arma::zeros(3, 3); - - F(0, 0) = -sl; - F(0, 1) = -sb * cl; - F(0, 2) = cb * cl; - - F(1, 0) = cl; - F(1, 1) = -sb * sl; - F(1, 2) = cb * sl; - - F(2, 0) = 0; - F(2, 1) = cb; - F(2, 2) = sb; + arma::mat F = {{-sl, -sb * cl, cb * cl}, + {cl, -sb * sl, cb * sl}, + {0.0, cb, sb}}; arma::vec local_vector; @@ -126,8 +113,8 @@ int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma:: if (hor_dis < 1.0E-20) { - *Az = 0; - *El = 90; + *Az = 0.0; + *El = 90.0; } else { @@ -147,7 +134,7 @@ int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma:: int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) { - *h = 0; + *h = 0.0; const double tolsq = 1.e-10; // tolerance to accept convergence const int maxit = 10; // max number of iterations const double rtd = 180.0 / STRP_PI; @@ -199,7 +186,7 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub // approximate distance from origin to surface of ellipsoid if (r < 1.0E-20) { - *h = 0; + *h = 0.0; return 1; } From aa3429ebb2ae31c578fd6a02a1ed7df3e97dfe08 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Aug 2018 20:04:36 +0200 Subject: [PATCH 05/35] Fix defect detected by coverity scan (avoid null pointer dereference) --- src/tests/system-tests/libs/geofunctions.cc | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc index 8098609c1..8c1b985a0 100644 --- a/src/tests/system-tests/libs/geofunctions.cc +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -328,17 +328,10 @@ arma::mat Euler_to_CTM(const arma::vec &eul) double sin_psi = sin(eul(2)); double cos_psi = cos(eul(2)); - arma::mat C = arma::zeros(3, 3); // Calculate coordinate transformation matrix using (2.22) - C(0, 0) = cos_theta * cos_psi; - C(0, 1) = cos_theta * sin_psi; - C(0, 2) = -sin_theta; - C(1, 0) = -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi; - C(1, 1) = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi; - C(1, 2) = sin_phi * cos_theta; - C(2, 0) = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi; - C(2, 1) = -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi; - C(2, 2) = cos_phi * cos_theta; + arma::mat C = {{cos_theta * cos_psi, cos_theta * sin_psi, -sin_theta}, + {-cos_phi * sin_psi + sin_phi * sin_theta * cos_psi, cos_phi * cos_psi + sin_phi * sin_theta * sin_psi, sin_phi * cos_theta}, + {sin_phi * sin_psi + cos_phi * sin_theta * cos_psi, -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi, cos_phi * cos_theta}}; return C; } From 753dc7241f07980ac9e482e19b9ddf41fa21c134 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Aug 2018 20:22:15 +0200 Subject: [PATCH 06/35] Fix defect detected by coverity scan (avoid null pointer dereference) --- src/algorithms/PVT/libs/pvt_solution.cc | 29 +++++-------------------- 1 file changed, 6 insertions(+), 23 deletions(-) diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index d0f31cc39..e71030085 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -73,16 +73,9 @@ arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec omegatau = OMEGA_EARTH_DOT * traveltime; //--- Build a rotation matrix ---------------------------------------------- - arma::mat R3 = arma::zeros(3, 3); - R3(0, 0) = cos(omegatau); - R3(0, 1) = sin(omegatau); - R3(0, 2) = 0.0; - R3(1, 0) = -sin(omegatau); - R3(1, 1) = cos(omegatau); - R3(1, 2) = 0.0; - R3(2, 0) = 0.0; - R3(2, 1) = 0.0; - R3(2, 2) = 1; + arma::mat R3 = {{cos(omegatau), sin(omegatau), 0.0}, + {-sin(omegatau), cos(omegatau), 0.0}, + {0.0, 0.0, 1.0}}; //--- Do the rotation ------------------------------------------------------ arma::vec X_sat_rot; @@ -394,19 +387,9 @@ int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &x double cb = cos(phi * dtr); double sb = sin(phi * dtr); - arma::mat F = arma::zeros(3, 3); - - F(0, 0) = -sl; - F(0, 1) = -sb * cl; - F(0, 2) = cb * cl; - - F(1, 0) = cl; - F(1, 1) = -sb * sl; - F(1, 2) = cb * sl; - - F(2, 0) = 0; - F(2, 1) = cb; - F(2, 2) = sb; + arma::mat F = {{-sl, -sb * cl, cb * cl}, + {cl, -sb * sl, cb * sl}, + {0, 0, cb, sb}}; arma::vec local_vector; From 5b2b487a2fd5441be98f136fed70ff8079e3ab08 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Aug 2018 21:17:19 +0200 Subject: [PATCH 07/35] Fix warning (type qualifiers ignored on cast result type) --- src/algorithms/libs/galileo_e1_signal_processing.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index b265ffb43..5078fd479 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -108,7 +108,7 @@ void galileo_e1_sinboc_61_gen_int(int* _dest, int* _prn, uint32_t _length_out) void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], uint32_t _prn) { std::string _galileo_signal = _Signal; - const uint32_t _codeLength = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + const uint32_t _codeLength = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); int32_t primary_code_E1_chips[4092]; // _codeLength not accepted by Clang galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip for (uint32_t i = 0; i < _codeLength; i++) From 34382d7ac5e683c34e3883f8eb1cb2342ffe02f8 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 30 Aug 2018 21:45:58 +0200 Subject: [PATCH 08/35] Fix defect detected by coverity scan (uninitialized members in constructor) --- .../gnuradio_blocks/galileo_telemetry_decoder_cc.cc | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc index 41eb414ed..2648788c9 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc @@ -133,6 +133,17 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc( break; } default: + d_bits_per_preamble = 0; + d_samples_per_preamble = 0; + d_preamble_period_symbols = 0; + d_preamble_samples = nullptr; + d_secondary_code_samples = nullptr; + d_samples_per_symbol = 0U; + d_PRN_code_period_ms = 0U; + d_required_symbols = 0U; + d_frame_length_symbols = 0.0; + CodeLength = 0; + DataLength = 0; std::cout << "Galileo unified telemetry decoder error: Unknown frame type " << std::endl; } From 3fcb027138b3a08ba4114bbd7587fa607ab0fd61 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 10:03:35 +0200 Subject: [PATCH 09/35] Update Access18 experiment with new PVT stored data structure --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +- .../ieee-access18/L2-access18.conf | 2 +- .../reproducibility/ieee-access18/plot_dump.m | 44 ++++++++++++++----- 3 files changed, 35 insertions(+), 15 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1f87b8797..15a95cdf2 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -599,8 +599,8 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ tmp_double = pvt_sol.thres; d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); - //GDOP//PDOP//HDOP//VDOP - d_dump_file.write(reinterpret_cast(&dop_[0]), sizeof(dop_)); + // GDOP / PDOP/ HDOP/ VDOP + d_dump_file.write(reinterpret_cast(&dop_[0]), sizeof(double) * 4); } catch (const std::ifstream::failure& e) { diff --git a/src/utils/reproducibility/ieee-access18/L2-access18.conf b/src/utils/reproducibility/ieee-access18/L2-access18.conf index d51a040d2..e4d7b5e67 100644 --- a/src/utils/reproducibility/ieee-access18/L2-access18.conf +++ b/src/utils/reproducibility/ieee-access18/L2-access18.conf @@ -133,7 +133,7 @@ PVT.display_rate_ms=500 ;# KML, GeoJSON, NMEA and RTCM output configuration ;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump. -PVT.dump_filename=./data/PVT +PVT.dump_filename=./data/access18 ;#nmea_dump_filename: NMEA log path and filename PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea diff --git a/src/utils/reproducibility/ieee-access18/plot_dump.m b/src/utils/reproducibility/ieee-access18/plot_dump.m index 33416ad77..5865235ff 100644 --- a/src/utils/reproducibility/ieee-access18/plot_dump.m +++ b/src/utils/reproducibility/ieee-access18/plot_dump.m @@ -62,26 +62,46 @@ ylabel('Navigation data bits','fontname','Times','fontsize', fontsize) grid on -fileID = fopen('data/PVT_ls_pvt.dat', 'r'); -dinfo = dir('data/PVT_ls_pvt.dat'); +fileID = fopen('data/access18_pvt.dat', 'r'); +dinfo = dir('data/access18_pvt.dat'); filesize = dinfo.bytes; aux = 1; while ne(ftell(fileID), filesize) - navsol.RX_time(aux) = fread(fileID, 1, 'double'); - navsol.X(aux) = fread(fileID, 1, 'double'); - navsol.Y(aux) = fread(fileID, 1, 'double'); - navsol.Z(aux) = fread(fileID, 1, 'double'); - navsol.user_clock(aux) = fread(fileID, 1, 'double'); - navsol.lat(aux) = fread(fileID, 1, 'double'); - navsol.long(aux) = fread(fileID, 1, 'double'); - navsol.height(aux) = fread(fileID, 1, 'double'); + navsol.TOW_at_current_symbol_ms(aux) = fread(fileID, 1, 'uint32'); + navsol.week(aux) = fread(fileID, 1, 'uint32'); + navsol.RX_time(aux) = fread(fileID, 1, 'double'); + navsol.user_clock_offset(aux) = fread(fileID, 1, 'double'); + navsol.X(aux) = fread(fileID, 1, 'double'); + navsol.Y(aux) = fread(fileID, 1, 'double'); + navsol.Z(aux) = fread(fileID, 1, 'double'); + navsol.VX(aux) = fread(fileID, 1, 'double'); + navsol.VY(aux) = fread(fileID, 1, 'double'); + navsol.VZ(aux) = fread(fileID, 1, 'double'); + navsol.varXX(aux) = fread(fileID, 1, 'double'); + navsol.varYY(aux) = fread(fileID, 1, 'double'); + navsol.varZZ(aux) = fread(fileID, 1, 'double'); + navsol.varXY(aux) = fread(fileID, 1, 'double'); + navsol.varYZ(aux) = fread(fileID, 1, 'double'); + navsol.varZX(aux) = fread(fileID, 1, 'double'); + navsol.latitude(aux) = fread(fileID, 1, 'double'); + navsol.longitude(aux) = fread(fileID, 1, 'double'); + navsol.height(aux) = fread(fileID, 1, 'double'); + navsol.number_sats(aux) = fread(fileID, 1, 'uint8'); + navsol.solution_status(aux) = fread(fileID, 1, 'uint8'); + navsol.solution_type(aux) = fread(fileID, 1, 'uint8'); + navsol.AR_ratio_factor(aux) = fread(fileID, 1, 'float'); + navsol.AR_ratio_threshold(aux) = fread(fileID, 1, 'float'); + navsol.GDOP(aux) = fread(fileID, 1, 'double'); + navsol.PDOP(aux) = fread(fileID, 1, 'double'); + navsol.HDOP(aux) = fread(fileID, 1, 'double'); + navsol.VDOP(aux) = fread(fileID, 1, 'double'); aux = aux + 1; end fclose(fileID); -mean_Latitude = mean(navsol.lat); -mean_Longitude = mean(navsol.long); +mean_Latitude = mean(navsol.latitude); +mean_Longitude = mean(navsol.longitude); mean_h = mean(navsol.height); utmZone = findUtmZone(mean_Latitude, mean_Longitude); [ref_X_cart, ref_Y_cart, ref_Z_cart] = geo2cart(dms2mat(deg2dms(mean_Latitude)), dms2mat(deg2dms(mean_Longitude)), mean_h, 5); From e4303d0facea17e9db1eda92d49dda229e988de1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 13:52:35 +0200 Subject: [PATCH 10/35] Fix build with latest GNU Radio master (towards 3.8) --- src/algorithms/input_filter/adapters/CMakeLists.txt | 4 ++++ src/algorithms/input_filter/adapters/fir_filter.h | 4 ++++ .../input_filter/adapters/freq_xlating_fir_filter.h | 4 ++++ src/core/receiver/CMakeLists.txt | 4 ++-- src/tests/CMakeLists.txt | 4 ++++ ...galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc | 4 ++++ .../galileo_e1_pcps_ambiguous_acquisition_gsoc2013_test.cc | 4 ++++ .../galileo_e1_pcps_ambiguous_acquisition_gsoc_test.cc | 4 ++++ .../acquisition/galileo_e1_pcps_ambiguous_acquisition_test.cc | 4 ++++ ...ileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc | 4 ++++ ...o_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc | 4 ++++ ...alileo_e1_pcps_tong_ambiguous_acquisition_gsoc2013_test.cc | 4 ++++ .../galileo_e5a_pcps_acquisition_gsoc2014_gensource_test.cc | 4 ++++ .../glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc | 4 ++++ .../acquisition/glonass_l1_ca_pcps_acquisition_test.cc | 4 ++++ .../acquisition/glonass_l2_ca_pcps_acquisition_test.cc | 4 ++++ .../acquisition/gps_l1_ca_pcps_acquisition_gsoc2013_test.cc | 4 ++++ .../acquisition/gps_l1_ca_pcps_acquisition_test.cc | 4 ++++ .../acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc | 4 ++++ .../gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc | 4 ++++ .../gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc | 4 ++++ .../gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc | 4 ++++ .../acquisition/gps_l2_m_pcps_acquisition_test.cc | 4 ++++ .../signal-processing-blocks/filter/fir_filter_test.cc | 4 ++++ .../signal-processing-blocks/filter/notch_filter_lite_test.cc | 4 ++++ .../signal-processing-blocks/filter/notch_filter_test.cc | 4 ++++ .../filter/pulse_blanking_filter_test.cc | 4 ++++ .../resampler/direct_resampler_conditioner_cc_test.cc | 4 ++++ .../signal-processing-blocks/resampler/mmse_resampler_test.cc | 4 ++++ .../signal-processing-blocks/sources/gnss_sdr_valve_test.cc | 4 ++++ .../telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc | 4 ++++ .../tracking/galileo_e1_dll_pll_veml_tracking_test.cc | 4 ++++ .../tracking/galileo_e5a_tracking_test.cc | 4 ++++ .../tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc | 4 ++++ .../tracking/glonass_l1_ca_dll_pll_tracking_test.cc | 4 ++++ .../tracking/gps_l1_ca_dll_pll_tracking_test.cc | 4 ++++ .../tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc | 4 ++++ .../tracking/gps_l1_ca_kf_tracking_test.cc | 4 ++++ .../tracking/gps_l2_m_dll_pll_tracking_test.cc | 4 ++++ 39 files changed, 154 insertions(+), 2 deletions(-) diff --git a/src/algorithms/input_filter/adapters/CMakeLists.txt b/src/algorithms/input_filter/adapters/CMakeLists.txt index f12efd609..765789e51 100644 --- a/src/algorithms/input_filter/adapters/CMakeLists.txt +++ b/src/algorithms/input_filter/adapters/CMakeLists.txt @@ -37,6 +37,10 @@ include_directories( ${VOLK_INCLUDE_DIRS} ) +if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) + add_definitions( -DGR_GREATER_38=1 ) +endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) + file(GLOB INPUT_FILTER_ADAPTER_HEADERS "*.h") list(SORT INPUT_FILTER_ADAPTER_HEADERS) add_library(input_filter_adapters ${INPUT_FILTER_ADAPTER_SOURCES} ${INPUT_FILTER_ADAPTER_HEADERS}) diff --git a/src/algorithms/input_filter/adapters/fir_filter.h b/src/algorithms/input_filter/adapters/fir_filter.h index 05c494b5f..db1044f15 100644 --- a/src/algorithms/input_filter/adapters/fir_filter.h +++ b/src/algorithms/input_filter/adapters/fir_filter.h @@ -43,8 +43,12 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include #include +#endif #include #include #include diff --git a/src/algorithms/input_filter/adapters/freq_xlating_fir_filter.h b/src/algorithms/input_filter/adapters/freq_xlating_fir_filter.h index 804e09995..8f69efc97 100644 --- a/src/algorithms/input_filter/adapters/freq_xlating_fir_filter.h +++ b/src/algorithms/input_filter/adapters/freq_xlating_fir_filter.h @@ -36,9 +36,13 @@ #include "gnss_block_interface.h" #include "short_x2_to_cshort.h" #include "complex_float_to_complex_byte.h" +#ifdef GR_GREATER_38 +#include +#else #include #include #include +#endif #include #include #include diff --git a/src/core/receiver/CMakeLists.txt b/src/core/receiver/CMakeLists.txt index 46a13614e..c135cb0c0 100644 --- a/src/core/receiver/CMakeLists.txt +++ b/src/core/receiver/CMakeLists.txt @@ -98,9 +98,9 @@ if(ENABLE_AD9361) set(OPT_RECEIVER_INCLUDE_DIRS ${OPT_RECEIVER_INCLUDE_DIRS} ${IIO_INCLUDE_DIRS}) endif(ENABLE_AD9361) -if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.15" ) +if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) add_definitions( -DGR_GREATER_38=1 ) -endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.15" ) +endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) include_directories( diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 8ed59e572..ad11216d7 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -136,6 +136,10 @@ if(Boost_VERSION LESS 105000) add_definitions(-DOLD_BOOST=1) endif(Boost_VERSION LESS 105000) +if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) + add_definitions( -DGR_GREATER_38=1 ) +endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) +message(STATUS "++++++++++++++++++++++++++++ ${PC_GNURADIO_RUNTIME_VERSION}") if(OPENSSL_FOUND) add_definitions( -DUSE_OPENSSL_FALLBACK=1 ) endif(OPENSSL_FOUND) diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc index 6cf6b2928..edc2eb3ce 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_8ms_ambiguous_acquisition_gsoc2013_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_block_interface.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc2013_test.cc index 1e6c5c328..d5cbef8c0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc2013_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_block_interface.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc_test.cc index 8bf13b0a7..e0c6b4534 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_gsoc_test.cc @@ -45,7 +45,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_test.cc index 6d56fd5ad..10ed1e676 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_test.cc @@ -37,7 +37,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc index 25cbe9854..d2738a9a3 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_cccwsr_ambiguous_acquisition_gsoc2013_test.cc @@ -36,7 +36,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_block_interface.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc index a52ce784f..9acd50a0a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_quicksync_ambiguous_acquisition_gsoc2014_test.cc @@ -39,7 +39,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include "gnss_block_interface.h" #include "in_memory_configuration.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_tong_ambiguous_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_tong_ambiguous_acquisition_gsoc2013_test.cc index 531823eea..ce86126ea 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_tong_ambiguous_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_tong_ambiguous_acquisition_gsoc2013_test.cc @@ -36,7 +36,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e5a_pcps_acquisition_gsoc2014_gensource_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e5a_pcps_acquisition_gsoc2014_gensource_test.cc index 741bb0d7e..63cf64e3e 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e5a_pcps_acquisition_gsoc2014_gensource_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e5a_pcps_acquisition_gsoc2014_gensource_test.cc @@ -33,7 +33,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_block_interface.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc index 425c085bc..30758feac 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_gsoc2017_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc index 9c6574d6e..b250b7e64 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc @@ -37,7 +37,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l2_ca_pcps_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l2_ca_pcps_acquisition_test.cc index d59e7992e..cf2af8a56 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l2_ca_pcps_acquisition_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/glonass_l2_ca_pcps_acquisition_test.cc @@ -34,7 +34,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_gsoc2013_test.cc index e323a9ad5..7fe814a5c 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_gsoc2013_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc index d5bfafeac..6f028cd4a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc @@ -38,7 +38,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc index 7db3c6d52..4fba7a4b9 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc @@ -36,7 +36,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc index 48d1ed090..4ff687b66 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_opencl_acquisition_gsoc2013_test.cc @@ -36,7 +36,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_block_interface.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc index e48d9384a..2ffd1bd15 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc @@ -37,7 +37,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc index 68e23ac91..d069fd703 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc @@ -36,7 +36,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc index b83802008..d6cb2813c 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc @@ -37,7 +37,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc index 51181dd2f..142802256 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/fir_filter_test.cc @@ -34,7 +34,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc index 8c5e02dbb..9028403bf 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_lite_test.cc @@ -34,7 +34,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc index 9db3593a6..2010b646c 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/notch_filter_test.cc @@ -34,7 +34,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc b/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc index 0ca1840aa..4cff0f421 100644 --- a/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/filter/pulse_blanking_filter_test.cc @@ -34,7 +34,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/resampler/direct_resampler_conditioner_cc_test.cc b/src/tests/unit-tests/signal-processing-blocks/resampler/direct_resampler_conditioner_cc_test.cc index 8dc1ef42c..55c49c568 100644 --- a/src/tests/unit-tests/signal-processing-blocks/resampler/direct_resampler_conditioner_cc_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/resampler/direct_resampler_conditioner_cc_test.cc @@ -34,7 +34,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_sdr_valve.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/resampler/mmse_resampler_test.cc b/src/tests/unit-tests/signal-processing-blocks/resampler/mmse_resampler_test.cc index 1292f93dc..c00561eb3 100644 --- a/src/tests/unit-tests/signal-processing-blocks/resampler/mmse_resampler_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/resampler/mmse_resampler_test.cc @@ -32,7 +32,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_sdr_valve.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/sources/gnss_sdr_valve_test.cc b/src/tests/unit-tests/signal-processing-blocks/sources/gnss_sdr_valve_test.cc index 26a57621e..41c10d607 100644 --- a/src/tests/unit-tests/signal-processing-blocks/sources/gnss_sdr_valve_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/sources/gnss_sdr_valve_test.cc @@ -33,7 +33,11 @@ #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include "gnss_sdr_valve.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc index 5f35d6014..22a1118d9 100644 --- a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc @@ -38,7 +38,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc index 55b2e80f8..fb61cd4bd 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc index 2aad4e5bd..9c6d274f7 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc index 34ca29f51..352c837e1 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_c_aid_tracking_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc index 2afd0d444..6503e30db 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/glonass_l1_ca_dll_pll_tracking_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc index fa897a5cf..3484fc791 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc @@ -39,7 +39,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc index d62956e30..977f87f7a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc @@ -41,7 +41,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc index 65e820921..599e408c6 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc @@ -38,7 +38,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc index 5c9c035f9..3c40eb54c 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc @@ -35,7 +35,11 @@ #include #include #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include #include #include From 01b6cad75dbb27a2e3468fac42228baf49e33b07 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 13:58:45 +0200 Subject: [PATCH 11/35] Fix build with latest GNU Radio master (towards 3.8) --- src/algorithms/input_filter/adapters/pulse_blanking_filter.h | 4 ++++ src/algorithms/resampler/adapters/CMakeLists.txt | 4 ++-- src/tests/CMakeLists.txt | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/algorithms/input_filter/adapters/pulse_blanking_filter.h b/src/algorithms/input_filter/adapters/pulse_blanking_filter.h index 0d9d17cb4..87f8c4994 100644 --- a/src/algorithms/input_filter/adapters/pulse_blanking_filter.h +++ b/src/algorithms/input_filter/adapters/pulse_blanking_filter.h @@ -35,7 +35,11 @@ #include "gnss_block_interface.h" #include "pulse_blanking_cc.h" #include +#ifdef GR_GREATER_38 +#include +#else #include +#endif #include class ConfigurationInterface; diff --git a/src/algorithms/resampler/adapters/CMakeLists.txt b/src/algorithms/resampler/adapters/CMakeLists.txt index 886ad5487..7c3dc92a5 100644 --- a/src/algorithms/resampler/adapters/CMakeLists.txt +++ b/src/algorithms/resampler/adapters/CMakeLists.txt @@ -32,9 +32,9 @@ include_directories( ) -if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.15" ) +if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) add_definitions( -DGR_GREATER_38=1 ) -endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.15" ) +endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) file(GLOB RESAMPLER_ADAPTER_HEADERS "*.h") diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index ad11216d7..b9ee6a474 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -139,7 +139,7 @@ endif(Boost_VERSION LESS 105000) if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) add_definitions( -DGR_GREATER_38=1 ) endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.13.4" ) -message(STATUS "++++++++++++++++++++++++++++ ${PC_GNURADIO_RUNTIME_VERSION}") + if(OPENSSL_FOUND) add_definitions( -DUSE_OPENSSL_FALLBACK=1 ) endif(OPENSSL_FOUND) From d2e3afec1e0d74e44962bd0ad409d6bdaf07e6e7 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 15:08:07 +0200 Subject: [PATCH 12/35] Fix build with latest GNU Radio master (towards 3.8) --- .../sources/unpack_2bit_samples_test.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc b/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc index 9229f16be..89da655be 100644 --- a/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc @@ -33,9 +33,13 @@ #include #include +#ifdef GR_GREATER_38 +#include +#else #include #include #include +#endif #include #include "unpack_2bit_samples.h" From f328e708b00732fa851eb6e83c3b5e9386902884 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 15:30:22 +0200 Subject: [PATCH 13/35] Fix build with latest GNU Radio master (towards 3.8) --- .../signal-processing-blocks/sources/unpack_2bit_samples_test.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc b/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc index 89da655be..68891bff0 100644 --- a/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/sources/unpack_2bit_samples_test.cc @@ -35,6 +35,7 @@ #include #ifdef GR_GREATER_38 #include +#include #else #include #include From 0f6a5d16ac2f0fdfbde6873a78e70b9022e49d56 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 15:51:07 +0200 Subject: [PATCH 14/35] Fix build with latest GNU Radio master (towards 3.8) --- src/algorithms/input_filter/adapters/fir_filter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/input_filter/adapters/fir_filter.h b/src/algorithms/input_filter/adapters/fir_filter.h index db1044f15..207d8e174 100644 --- a/src/algorithms/input_filter/adapters/fir_filter.h +++ b/src/algorithms/input_filter/adapters/fir_filter.h @@ -44,7 +44,7 @@ #include #include #ifdef GR_GREATER_38 -#include +#include #else #include #include From 9395a660513589c2e09812706710ab7bcd3b5e3f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 20:35:56 +0200 Subject: [PATCH 15/35] Fix saving of DOP parameters --- src/algorithms/PVT/libs/rtklib_solver.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 15a95cdf2..d46126f4a 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -600,7 +600,10 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ 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) * 4); + 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) { From 2a9639789003b86725e776b780eda1f5331168ca Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 31 Aug 2018 21:25:24 +0200 Subject: [PATCH 16/35] Update to Google Test 1.8.1 --- CMakeLists.txt | 2 +- README.md | 6 +++--- src/tests/CMakeLists.txt | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d8d7cc94..56392b083 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -341,7 +341,7 @@ set(GNSSSDR_MATIO_MIN_VERSION "1.5.3") set(GNSSSDR_GFLAGS_LOCAL_VERSION "2.2.1") set(GNSSSDR_GLOG_LOCAL_VERSION "0.3.5") set(GNSSSDR_ARMADILLO_LOCAL_VERSION "unstable") -set(GNSSSDR_GTEST_LOCAL_VERSION "1.8.0") +set(GNSSSDR_GTEST_LOCAL_VERSION "1.8.1") set(GNSSSDR_GNSS_SIM_LOCAL_VERSION "master") set(GNSSSDR_GPSTK_LOCAL_VERSION "2.10") set(GNSSSDR_MATIO_LOCAL_VERSION "1.5.12") diff --git a/README.md b/README.md index 9fe634565..16d1702a0 100644 --- a/README.md +++ b/README.md @@ -229,9 +229,9 @@ $ sudo ldconfig #### Build the [Google C++ Testing Framework](https://github.com/google/googletest "Googletest Homepage"), also known as Google Test: ~~~~~~ -$ wget https://github.com/google/googletest/archive/release-1.8.0.zip -$ unzip release-1.8.0.zip -$ cd googletest-release-1.8.0 +$ wget https://github.com/google/googletest/archive/release-1.8.1.zip +$ unzip release-1.8.1.zip +$ cd googletest-release-1.8.1 $ cmake -DBUILD_GTEST=ON -DBUILD_GMOCK=OFF . $ make ~~~~~~ diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index b9ee6a474..8236d1a07 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -55,7 +55,7 @@ if(NOT ${GTEST_DIR_LOCAL}) GIT_TAG release-${GNSSSDR_GTEST_LOCAL_VERSION} SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gtest/gtest-${GNSSSDR_GTEST_LOCAL_VERSION} BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/../../gtest-${GNSSSDR_GTEST_LOCAL_VERSION} - CMAKE_ARGS ${GTEST_COMPILER} -DBUILD_GTEST=ON -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} + CMAKE_ARGS ${GTEST_COMPILER} -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} UPDATE_COMMAND "" PATCH_COMMAND "" INSTALL_COMMAND "" @@ -67,7 +67,7 @@ if(NOT ${GTEST_DIR_LOCAL}) GIT_TAG release-${GNSSSDR_GTEST_LOCAL_VERSION} SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gtest/gtest-${GNSSSDR_GTEST_LOCAL_VERSION} BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/../../gtest-${GNSSSDR_GTEST_LOCAL_VERSION} - CMAKE_ARGS ${GTEST_COMPILER} -DBUILD_GTEST=ON -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} + CMAKE_ARGS ${GTEST_COMPILER} -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} UPDATE_COMMAND "" PATCH_COMMAND "" BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/../../gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/googletest/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX} From 8c7fb525a760d615df94637e27fbbe4390950af4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 1 Sep 2018 00:31:02 +0200 Subject: [PATCH 17/35] Do not install Google Test (fix building in CentOS 7) --- README.md | 2 +- src/tests/CMakeLists.txt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 16d1702a0..9e2e71712 100644 --- a/README.md +++ b/README.md @@ -232,7 +232,7 @@ $ sudo ldconfig $ wget https://github.com/google/googletest/archive/release-1.8.1.zip $ unzip release-1.8.1.zip $ cd googletest-release-1.8.1 -$ cmake -DBUILD_GTEST=ON -DBUILD_GMOCK=OFF . +$ cmake -DINSTALL_GTEST=OFF -DBUILD_GMOCK=OFF . $ make ~~~~~~ diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 8236d1a07..fcde4ab50 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -55,7 +55,7 @@ if(NOT ${GTEST_DIR_LOCAL}) GIT_TAG release-${GNSSSDR_GTEST_LOCAL_VERSION} SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gtest/gtest-${GNSSSDR_GTEST_LOCAL_VERSION} BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/../../gtest-${GNSSSDR_GTEST_LOCAL_VERSION} - CMAKE_ARGS ${GTEST_COMPILER} -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} + CMAKE_ARGS ${GTEST_COMPILER} -DINSTALL_GTEST=OFF -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} UPDATE_COMMAND "" PATCH_COMMAND "" INSTALL_COMMAND "" @@ -67,7 +67,7 @@ if(NOT ${GTEST_DIR_LOCAL}) GIT_TAG release-${GNSSSDR_GTEST_LOCAL_VERSION} SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gtest/gtest-${GNSSSDR_GTEST_LOCAL_VERSION} BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/../../gtest-${GNSSSDR_GTEST_LOCAL_VERSION} - CMAKE_ARGS ${GTEST_COMPILER} -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} + CMAKE_ARGS ${GTEST_COMPILER} -DINSTALL_GTEST=OFF -DBUILD_GMOCK=OFF ${TOOLCHAIN_ARG} UPDATE_COMMAND "" PATCH_COMMAND "" BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/../../gtest-${GNSSSDR_GTEST_LOCAL_VERSION}/googletest/${CMAKE_FIND_LIBRARY_PREFIXES}gtest${CMAKE_STATIC_LIBRARY_SUFFIX} From 5ffa5aba8e6624c5021bd37dd5d5de29c79ec241 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 1 Sep 2018 16:53:27 +0200 Subject: [PATCH 18/35] Fix logging of PVT fixes --- src/algorithms/PVT/libs/rtklib_solver.cc | 34 +++++++++++++++++++----- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index d46126f4a..4d43c18b4 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -571,10 +571,32 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ 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) - d_dump_file.write(reinterpret_cast(&pvt_sol.rr[0]), sizeof(pvt_sol.rr)); + tmp_double = pvt_sol.rr[0]; + 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)); + tmp_double = pvt_sol.rr[2]; + 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)); + tmp_double = pvt_sol.rr[4]; + 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)); - // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) - d_dump_file.write(reinterpret_cast(&pvt_sol.qr[0]), sizeof(pvt_sol.qr)); + // 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)); + tmp_double = pvt_sol.qr[1]; + 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)); + tmp_double = pvt_sol.qr[3]; + 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)); + tmp_double = pvt_sol.qr[5]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Latitude [deg] tmp_double = get_latitude(); @@ -592,11 +614,9 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ 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)); - //AR ratio factor for validation - tmp_double = pvt_sol.ratio; + // AR ratio factor for validation d_dump_file.write(reinterpret_cast(&pvt_sol.ratio), sizeof(float)); - //AR ratio threshold for validation - tmp_double = pvt_sol.thres; + // AR ratio threshold for validation d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); // GDOP / PDOP/ HDOP/ VDOP From 6ef9c51a3d4eb1a0ad876f4541d1bb652eed69c0 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 2 Sep 2018 09:22:52 +0200 Subject: [PATCH 19/35] Fix computation of CEP and bias accuracy --- .../libs/rtklib_solver_dump_reader.cc | 46 ++++++++++++------- .../libs/rtklib_solver_dump_reader.h | 10 ++-- src/tests/system-tests/position_test.cc | 6 +-- 3 files changed, 37 insertions(+), 25 deletions(-) diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc index d85c0081b..b32f12bc5 100644 --- a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc @@ -35,21 +35,34 @@ bool rtklib_solver_dump_reader::read_binary_obs() { try { - d_dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms), sizeof(TOW_at_current_symbol_ms)); - d_dump_file.read(reinterpret_cast(&week), sizeof(week)); - d_dump_file.read(reinterpret_cast(&RX_time), sizeof(RX_time)); - d_dump_file.read(reinterpret_cast(&clk_offset_s), sizeof(clk_offset_s)); - d_dump_file.read(reinterpret_cast(&rr[0]), sizeof(rr)); - d_dump_file.read(reinterpret_cast(&qr[0]), sizeof(qr)); - d_dump_file.read(reinterpret_cast(&latitude), sizeof(latitude)); - d_dump_file.read(reinterpret_cast(&longitude), sizeof(longitude)); - d_dump_file.read(reinterpret_cast(&height), sizeof(height)); - d_dump_file.read(reinterpret_cast(&ns), sizeof(ns)); - d_dump_file.read(reinterpret_cast(&status), sizeof(status)); - d_dump_file.read(reinterpret_cast(&type), sizeof(type)); - d_dump_file.read(reinterpret_cast(&AR_ratio), sizeof(AR_ratio)); - d_dump_file.read(reinterpret_cast(&AR_thres), sizeof(AR_thres)); - d_dump_file.read(reinterpret_cast(&dop[0]), sizeof(dop)); + d_dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms), sizeof(uint32_t)); + d_dump_file.read(reinterpret_cast(&week), sizeof(uint32_t)); + d_dump_file.read(reinterpret_cast(&RX_time), sizeof(double)); + d_dump_file.read(reinterpret_cast(&clk_offset_s), sizeof(double)); + d_dump_file.read(reinterpret_cast(&rr[0]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&rr[1]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&rr[2]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&rr[3]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&rr[4]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&rr[5]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&qr[0]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&qr[1]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&qr[2]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&qr[3]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&qr[4]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&qr[5]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&latitude), sizeof(double)); + d_dump_file.read(reinterpret_cast(&longitude), sizeof(double)); + d_dump_file.read(reinterpret_cast(&height), sizeof(double)); + d_dump_file.read(reinterpret_cast(&ns), sizeof(uint8_t)); + d_dump_file.read(reinterpret_cast(&status), sizeof(uint8_t)); + d_dump_file.read(reinterpret_cast(&type), sizeof(uint8_t)); + d_dump_file.read(reinterpret_cast(&AR_ratio), sizeof(float)); + d_dump_file.read(reinterpret_cast(&AR_thres), sizeof(float)); + d_dump_file.read(reinterpret_cast(&dop[0]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&dop[1]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&dop[2]), sizeof(double)); + d_dump_file.read(reinterpret_cast(&dop[3]), sizeof(double)); } catch (const std::ifstream::failure &e) { @@ -77,8 +90,7 @@ bool rtklib_solver_dump_reader::restart() int64_t rtklib_solver_dump_reader::num_epochs() { std::ifstream::pos_type size; - int epoch_size_bytes = sizeof(TOW_at_current_symbol_ms) + sizeof(week) + sizeof(RX_time) + sizeof(clk_offset_s) + sizeof(rr) + sizeof(qr) + sizeof(latitude) + sizeof(longitude) + sizeof(height) + sizeof(ns) + sizeof(status) + sizeof(type) + sizeof(AR_ratio) + sizeof(AR_thres) + sizeof(dop); - + int epoch_size_bytes = 2 * sizeof(uint32_t) + 21 * sizeof(double) + 3 * sizeof(uint8_t) + 2 * sizeof(float); std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); if (tmpfile.is_open()) { diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.h b/src/tests/system-tests/libs/rtklib_solver_dump_reader.h index e89659b37..5006482dd 100644 --- a/src/tests/system-tests/libs/rtklib_solver_dump_reader.h +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.h @@ -45,7 +45,7 @@ public: int64_t num_epochs(); bool open_obs_file(std::string out_file); - //rtklib_solver dump variables + // rtklib_solver dump variables // TOW uint32_t TOW_at_current_symbol_ms; // WEEK @@ -57,7 +57,7 @@ public: // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) double rr[6]; // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) - float qr[6]; + double qr[6]; // GEO user position Latitude [deg] double latitude; @@ -72,12 +72,12 @@ public: uint8_t status; // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) uint8_t type; - //AR ratio factor for validation + // AR ratio factor for validation float AR_ratio; - //AR ratio threshold for validation + // AR ratio threshold for validation float AR_thres; - //GDOP//PDOP//HDOP//VDOP + // GDOP / PDOP / HDOP / VDOP double dop[4]; private: diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 5adc6636f..b95a2b42d 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -614,13 +614,13 @@ void StaticPositionSystemTest::check_results() stm << "---- ACCURACY ----" << std::endl; stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; - stm << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl; + stm << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, ref_n) + 0.56 * compute_stdev_accuracy(pos_e, ref_e) << " [m]" << std::endl; stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; - stm << "Bias 2D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl; - stm << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl; + stm << "Bias 2D = " << sqrt(std::pow(abs(mean__e - ref_e), 2.0) + std::pow(abs(mean__n - ref_n), 2.0)) << " [m]" << std::endl; + stm << "Bias 3D = " << sqrt(std::pow(abs(mean__e - ref_e), 2.0) + std::pow(abs(mean__n - ref_n), 2.0) + std::pow(abs(mean__u - ref_u), 2.0)) << " [m]" << std::endl; stm << std::endl; } From 025ea65a3ab3d5326c4e7c7d168a7e7f0f6e6258 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 4 Sep 2018 08:13:55 +0200 Subject: [PATCH 20/35] Find libgfortran8 --- cmake/Modules/FindGFORTRAN.cmake | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/cmake/Modules/FindGFORTRAN.cmake b/cmake/Modules/FindGFORTRAN.cmake index eec7a4265..baeaad1fd 100644 --- a/cmake/Modules/FindGFORTRAN.cmake +++ b/cmake/Modules/FindGFORTRAN.cmake @@ -135,6 +135,14 @@ /usr/lib/sparc64-linux-gnu /usr/lib/x86_64-linux-gnux32 /usr/lib/alpha-linux-gnu + /usr/lib/gcc/x86_64-linux-gnu/8 # libgfortran8 + /usr/lib/gcc/aarch64-linux-gnu/8 + /usr/lib/gcc/arm-linux-gnueabihf/8 + /usr/lib/gcc/i686-linux-gnu/8 + /usr/lib/gcc/powerpc64le-linux-gnu/8 + /usr/lib/gcc/s390x-linux-gnu/8 + /usr/lib/gcc/alpha-linux-gnu/8 ) + INCLUDE(FindPackageHandleStandardArgs) -FIND_PACKAGE_HANDLE_STANDARD_ARGS(GFORTRAN DEFAULT_MSG GFORTRAN) \ No newline at end of file +FIND_PACKAGE_HANDLE_STANDARD_ARGS(GFORTRAN DEFAULT_MSG GFORTRAN) From 853acbffdea5df94b633b794f689e295da716f41 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 4 Sep 2018 08:24:31 +0200 Subject: [PATCH 21/35] Update Google Test version in example --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 9e2e71712..32195a8d0 100644 --- a/README.md +++ b/README.md @@ -239,10 +239,10 @@ $ make Please **DO NOT install** Google Test (do *not* type ```sudo make install```). Every user needs to compile his tests using the same compiler flags used to compile the installed Google Test libraries; otherwise he may run into undefined behaviors (i.e. the tests can behave strangely and may even crash for no obvious reasons). The reason is that C++ has this thing called the One-Definition Rule: if two C++ source files contain different definitions of the same class/function/variable, and you link them together, you violate the rule. The linker may or may not catch the error (in many cases it is not required by the C++ standard to catch the violation). If it does not, you get strange run-time behaviors that are unexpected and hard to debug. If you compile Google Test and your test code using different compiler flags, they may see different definitions of the same class/function/variable (e.g. due to the use of ```#if``` in Google Test). Therefore, for your sanity, we recommend to avoid installing pre-compiled Google Test libraries. Instead, each project should compile Google Test itself such that it can be sure that the same flags are used for both Google Test and the tests. The building system of GNSS-SDR does the compilation and linking of googletest to its own tests; it is only required that you tell the system where the googletest folder that you downloaded resides. Just add to your ```$HOME/.bashrc``` file the following line: ~~~~~~ -export GTEST_DIR=/home/username/googletest-release-1.8.0/googletest +export GTEST_DIR=/home/username/googletest-release-1.8.1/googletest ~~~~~~ -changing `/home/username/googletest-release-1.8.0/googletest` by the actual directory where you built googletest. +changing `/home/username/googletest-release-1.8.1/googletest` by the actual directory where you built googletest. From 1c6cf9d60f1b5a91976274394e4145d348c21803 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 4 Sep 2018 15:51:45 +0200 Subject: [PATCH 22/35] Move include only used by the implementation to the .cc file --- src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc | 1 + src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc index ba4714109..15cd3335d 100644 --- a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc @@ -29,6 +29,7 @@ */ #include "spirent_motion_csv_dump_reader.h" +#include #include diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h index e6b6f43eb..b08beb6e0 100644 --- a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h @@ -31,7 +31,6 @@ #ifndef GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H #define GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H -#include #include #include #include From 49beb1241a390ba083a209ed259f8fddff88a45d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 4 Sep 2018 20:08:59 +0200 Subject: [PATCH 23/35] Fix header and testing in non-interactive environments --- .../tracking/adapters/gps_l1_ca_kf_tracking.h | 4 ++-- .../tracking/gps_l1_ca_kf_tracking_test.cc | 18 ++++++++++++++++-- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h index a687b8a9e..e17831ffb 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h @@ -1,6 +1,6 @@ /*! - * \file GPS_L1_CA_KF_Tracking.h - * \brief Interface of an adapter of a DLL + Kalman carrier + * \file gps_l1_ca_kf_tracking.h + * \brief Interface of an adapter of a DLL + Kalman carrier * tracking loop block for GPS L1 C/A signals * \author Javier Arribas, 2018. jarribas(at)cttc.es * \author Jordi Vila-Valls 2018. jvila(at)cttc.es diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc index 599e408c6..775c3540b 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc @@ -550,7 +550,14 @@ TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults) g1.plot_xy(timevec, late, "Late", decimate); g1.savetops("Correlators_outputs"); g1.savetopdf("Correlators_outputs", 18); - g1.showonscreen(); // window output + if (FLAGS_show_plots) + { + g1.showonscreen(); // window output + } + else + { + g1.disablescreen(); + } Gnuplot g2("points"); g2.set_title("Constellation diagram (satellite PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")"); @@ -561,7 +568,14 @@ TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults) g2.plot_xy(promptI, promptQ); g2.savetops("Constellation"); g2.savetopdf("Constellation", 18); - g2.showonscreen(); // window output + if (FLAGS_show_plots) + { + g2.showonscreen(); // window output + } + else + { + g2.disablescreen(); + } } catch (const GnuplotException& ge) { From 8cd4ac060bff6822dfe35fc15ba4c9ccef1c8708 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 4 Sep 2018 20:11:33 +0200 Subject: [PATCH 24/35] Test reordering --- src/tests/test_main.cc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 64920b9a1..b2c8c3342 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -138,11 +138,16 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test_fpga.cc" #endif +#include "unit-tests/signal-processing-blocks/telemetry_decoder/galileo_fnav_inav_decoder_test.cc" +#include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" +#include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc" + #include "unit-tests/signal-processing-blocks/pvt/rtcm_test.cc" #include "unit-tests/signal-processing-blocks/pvt/rtcm_printer_test.cc" #include "unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc" #include "unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc" + #if EXTRA_TESTS #include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc" #include "unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc" @@ -158,9 +163,6 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc" #endif -#include "unit-tests/signal-processing-blocks/telemetry_decoder/galileo_fnav_inav_decoder_test.cc" -#include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" -#include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc" // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; From a956fdee7dcac852459bb30c3e50cc8a2e047ca7 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 5 Sep 2018 21:02:48 +0200 Subject: [PATCH 25/35] Minor 3D error plot improvements --- src/tests/system-tests/position_test.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index b95a2b42d..d41f9ca24 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -781,8 +781,10 @@ void StaticPositionSystemTest::check_results() //conversion between arma::vec and std:vector std::vector error_vec(error_module_R_eb_e.colptr(0), error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows); g3.cmd("set key box opaque"); - g3.plot_xy(time_vector_from_start_s, error_vec, - "Position_3d_error"); + g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error"); + double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); + std::vector error_mean(mean3d, error_vec.size()); + g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); g3.set_legend(); g3.savetops("Position_3d_error"); From 7e9a094caacfb9509e89c2ac146ef10941398595 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 5 Sep 2018 21:24:36 +0200 Subject: [PATCH 26/35] Fix arma_vec usage --- src/tests/system-tests/position_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index d41f9ca24..e4f09e444 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -783,7 +783,7 @@ void StaticPositionSystemTest::check_results() g3.cmd("set key box opaque"); g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error"); double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); - std::vector error_mean(mean3d, error_vec.size()); + std::vector error_mean(mean3d, time_vector_from_start_s.n_elem); g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); g3.set_legend(); g3.savetops("Position_3d_error"); From a95cd87f797ddffbe4504618ee15daa5e4fca97f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 5 Sep 2018 21:41:15 +0200 Subject: [PATCH 27/35] Fix position test --- src/tests/system-tests/position_test.cc | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index e4f09e444..44ae584c4 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -783,7 +783,7 @@ void StaticPositionSystemTest::check_results() g3.cmd("set key box opaque"); g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error"); double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); - std::vector error_mean(mean3d, time_vector_from_start_s.n_elem); + std::vector error_mean(mean3d, error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows); g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); g3.set_legend(); g3.savetops("Position_3d_error"); @@ -804,8 +804,7 @@ void StaticPositionSystemTest::check_results() //conversion between arma::vec and std:vector std::vector error_vec2(error_module_V_eb_e.colptr(0), error_module_V_eb_e.colptr(0) + error_module_V_eb_e.n_rows); g4.cmd("set key box opaque"); - g4.plot_xy(time_vector_from_start_s, error_vec2, - "Velocity_3d_error"); + g4.plot_xy(time_vector_from_start_s, error_vec2, "Velocity 3D error"); g4.set_legend(); g4.savetops("Velocity_3d_error"); } From ec7bd4dae54a7ea0a2ef8ba3942c825f17263602 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 5 Sep 2018 21:50:46 +0200 Subject: [PATCH 28/35] Fix position test --- src/tests/system-tests/position_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 44ae584c4..e6ae9b882 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -783,7 +783,7 @@ void StaticPositionSystemTest::check_results() g3.cmd("set key box opaque"); g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error"); double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); - std::vector error_mean(mean3d, error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows); + std::vector error_mean(mean3d, error_module_R_eb_e.n_rows); g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); g3.set_legend(); g3.savetops("Position_3d_error"); From 116b327d36dfe140d0ec42058e535c511d34035e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 6 Sep 2018 07:38:41 +0200 Subject: [PATCH 29/35] Fix position test --- src/tests/system-tests/position_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index e6ae9b882..e8e00be17 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -783,7 +783,7 @@ void StaticPositionSystemTest::check_results() g3.cmd("set key box opaque"); g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error"); double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); - std::vector error_mean(mean3d, error_module_R_eb_e.n_rows); + std::vector error_mean(error_module_R_eb_e.n_rows, mean3d); g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); g3.set_legend(); g3.savetops("Position_3d_error"); From a8b1fdc37c4f7ac7300ea542f991f4501afd987b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 6 Sep 2018 08:17:35 +0200 Subject: [PATCH 30/35] Fix position test --- src/tests/system-tests/position_test.cc | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index e8e00be17..6a2b1ca1d 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -759,7 +759,7 @@ void StaticPositionSystemTest::check_results() std::vector Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows); g1.cmd("set key box opaque"); - g1.plot_xyz(X, Y, Z, "ECEF_3d_error"); + g1.plot_xyz(X, Y, Z, "ECEF 3D error"); g1.set_legend(); g1.savetops("ECEF_3d_error"); @@ -784,6 +784,7 @@ void StaticPositionSystemTest::check_results() g3.plot_xy(time_vector_from_start_s, error_vec, "Position 3D error"); double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); std::vector error_mean(error_module_R_eb_e.n_rows, mean3d); + g3.set_style("lines"); g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); g3.set_legend(); g3.savetops("Position_3d_error"); @@ -805,6 +806,10 @@ void StaticPositionSystemTest::check_results() std::vector error_vec2(error_module_V_eb_e.colptr(0), error_module_V_eb_e.colptr(0) + error_module_V_eb_e.n_rows); g4.cmd("set key box opaque"); g4.plot_xy(time_vector_from_start_s, error_vec2, "Velocity 3D error"); + double mean3dv = std::accumulate(error_vec2.begin(), error_vec2.end(), 0.0) / error_vec2.size(); + std::vector error_mean_v(error_module_V_eb_e.n_rows, mean3dv); + g4.set_style("lines"); + g4.plot_xy(time_vector_from_start_s, error_mean_v, "Mean"); g4.set_legend(); g4.savetops("Velocity_3d_error"); } From ab1c87bdc4b9aa87f6e71a7b338740116de6dbfe Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 6 Sep 2018 11:16:56 +0200 Subject: [PATCH 31/35] Add configuration name in output figures --- src/tests/system-tests/position_test.cc | 55 ++++++++++++++++++++----- 1 file changed, 44 insertions(+), 11 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 6a2b1ca1d..1bf4bc71d 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -761,9 +761,14 @@ void StaticPositionSystemTest::check_results() g1.cmd("set key box opaque"); g1.plot_xyz(X, Y, Z, "ECEF 3D error"); g1.set_legend(); - g1.savetops("ECEF_3d_error"); - - + if (FLAGS_config_file_ptest.empty()) + { + g1.savetops("ECEF_3d_error"); + } + else + { + g1.savetops("ECEF_3d_error_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + } arma::vec time_vector_from_start_s = receiver_time_s - receiver_time_s(0); Gnuplot g3("linespoints"); if (FLAGS_show_plots) @@ -787,7 +792,14 @@ void StaticPositionSystemTest::check_results() g3.set_style("lines"); g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); g3.set_legend(); - g3.savetops("Position_3d_error"); + if (FLAGS_config_file_ptest.empty()) + { + g3.savetops("Position_3d_error"); + } + else + { + g3.savetops("Position_3d_error_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + } Gnuplot g4("linespoints"); if (FLAGS_show_plots) @@ -811,7 +823,14 @@ void StaticPositionSystemTest::check_results() g4.set_style("lines"); g4.plot_xy(time_vector_from_start_s, error_mean_v, "Mean"); g4.set_legend(); - g4.savetops("Velocity_3d_error"); + if (FLAGS_config_file_ptest.empty()) + { + g4.savetops("Velocity_3d_error"); + } + else + { + g4.savetops("Velocity_3d_error_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + } } } @@ -881,9 +900,16 @@ void StaticPositionSystemTest::print_results(const std::vector& east, g1.cmd("set grid front"); g1.cmd("replot"); - - g1.savetops("Position_test_2D"); - g1.savetopdf("Position_test_2D", 18); + if (FLAGS_config_file_ptest.empty()) + { + g1.savetops("Position_test_2D"); + g1.savetopdf("Position_test_2D", 18); + } + else + { + g1.savetops("Position_test_2D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + g1.savetopdf("Position_test_2D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5), 18); + } Gnuplot g2("points"); if (FLAGS_show_plots) @@ -909,9 +935,16 @@ void StaticPositionSystemTest::print_results(const std::vector& east, std::to_string(ninty_sas) + "\n fx(v,u) = r*cos(v)*cos(u)\n fy(v,u) = r*cos(v)*sin(u)\n fz(v) = r*sin(v) \n splot fx(v,u),fy(v,u),fz(v) title \"90\%-SAS\" lt rgb \"gray\"\n"); g2.plot_xyz(east, north, up, "3D Position Fixes"); - - g2.savetops("Position_test_3D"); - g2.savetopdf("Position_test_3D"); + if (FLAGS_config_file_ptest.empty()) + { + g2.savetops("Position_test_3D"); + g2.savetopdf("Position_test_3D"); + } + else + { + g2.savetops("Position_test_3D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + g2.savetopdf("Position_test_3D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + } } catch (const GnuplotException& ge) { From 3adba38c34c8dde04ba6d959af18efc102e6a87d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 6 Sep 2018 11:48:40 +0200 Subject: [PATCH 32/35] Fix name of output files --- src/tests/system-tests/position_test.cc | 29 ++++++++++++++++++------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 1bf4bc71d..0f1f4ed80 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -67,6 +67,7 @@ public: int configure_receiver(); int run_receiver(); void check_results(); + std::string config_filename_no_extension; private: std::string generator_binary; @@ -608,7 +609,10 @@ void StaticPositionSystemTest::check_results() std::stringstream stm; std::ofstream position_test_file; - + if (!FLAGS_config_file_ptest.empty()) + { + stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl; + } if (FLAGS_config_file_ptest.empty()) { stm << "---- ACCURACY ----" << std::endl; @@ -726,6 +730,10 @@ void StaticPositionSystemTest::check_results() //report std::cout << "----- Position and Velocity 3D ECEF error statistics -----" << std::endl; + if (!FLAGS_config_file_ptest.empty()) + { + std::cout << "Configuration file: " << FLAGS_config_file_ptest << std::endl; + } std::streamsize ss = std::cout.precision(); std::cout << std::setprecision(10) << "---- 3D ECEF Position RMSE = " << rmse_R_eb_e << ", mean = " << error_mean_R_eb_e @@ -767,7 +775,7 @@ void StaticPositionSystemTest::check_results() } else { - g1.savetops("ECEF_3d_error_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + g1.savetops("ECEF_3d_error_" + config_filename_no_extension); } arma::vec time_vector_from_start_s = receiver_time_s - receiver_time_s(0); Gnuplot g3("linespoints"); @@ -798,7 +806,7 @@ void StaticPositionSystemTest::check_results() } else { - g3.savetops("Position_3d_error_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + g3.savetops("Position_3d_error_" + config_filename_no_extension); } Gnuplot g4("linespoints"); @@ -829,7 +837,7 @@ void StaticPositionSystemTest::check_results() } else { - g4.savetops("Velocity_3d_error_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + g4.savetops("Velocity_3d_error_" + config_filename_no_extension); } } } @@ -907,8 +915,8 @@ void StaticPositionSystemTest::print_results(const std::vector& east, } else { - g1.savetops("Position_test_2D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); - g1.savetopdf("Position_test_2D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5), 18); + g1.savetops("Position_test_2D_" + config_filename_no_extension); + g1.savetopdf("Position_test_2D_" + config_filename_no_extension, 18); } Gnuplot g2("points"); @@ -942,8 +950,8 @@ void StaticPositionSystemTest::print_results(const std::vector& east, } else { - g2.savetops("Position_test_3D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); - g2.savetopdf("Position_test_3D_" + FLAGS_config_file_ptest.erase(FLAGS_config_file_ptest.length() - 5)); + g2.savetops("Position_test_3D_" + config_filename_no_extension); + g2.savetopdf("Position_test_3D_" + config_filename_no_extension); } } catch (const GnuplotException& ge) @@ -966,6 +974,11 @@ TEST_F(StaticPositionSystemTest, Position_system_test) generate_signal(); } } + else + { + config_filename_no_extension = FLAGS_config_file_ptest.substr(FLAGS_config_file_ptest.find_last_of("/\\") + 1); + config_filename_no_extension = config_filename_no_extension.erase(config_filename_no_extension.length() - 5); + } // Configure receiver configure_receiver(); From db50c2a27533986f509143dc115f27cdb289da1b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 6 Sep 2018 12:05:31 +0200 Subject: [PATCH 33/35] Cosmetics --- src/tests/system-tests/position_test.cc | 34 ++++++++++++------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 0f1f4ed80..6d099075b 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -59,7 +59,7 @@ concurrent_queue global_gps_acq_assist_queue; concurrent_map global_gps_acq_assist_map; -class StaticPositionSystemTest : public ::testing::Test +class PositionSystemTest : public ::testing::Test { public: int configure_generator(); @@ -101,7 +101,7 @@ private: }; -void StaticPositionSystemTest::geodetic2Ecef(const double latitude, const double longitude, const double altitude, +void PositionSystemTest::geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x, double* y, double* z) { const double a = 6378137.0; // WGS84 @@ -126,7 +126,7 @@ void StaticPositionSystemTest::geodetic2Ecef(const double latitude, const double } -void StaticPositionSystemTest::geodetic2Enu(double latitude, double longitude, double altitude, +void PositionSystemTest::geodetic2Enu(double latitude, double longitude, double altitude, double* east, double* north, double* up) { double x, y, z; @@ -169,7 +169,7 @@ void StaticPositionSystemTest::geodetic2Enu(double latitude, double longitude, d } -double StaticPositionSystemTest::compute_stdev_precision(const std::vector& vec) +double PositionSystemTest::compute_stdev_precision(const std::vector& vec) { double sum__ = std::accumulate(vec.begin(), vec.end(), 0.0); double mean__ = sum__ / vec.size(); @@ -182,7 +182,7 @@ double StaticPositionSystemTest::compute_stdev_precision(const std::vector& vec, const double ref) +double PositionSystemTest::compute_stdev_accuracy(const std::vector& vec, const double ref) { const double mean__ = ref; double accum__ = 0.0; @@ -194,7 +194,7 @@ double StaticPositionSystemTest::compute_stdev_accuracy(const std::vector control_thread; if (FLAGS_config_file_ptest.empty()) @@ -449,15 +449,15 @@ int StaticPositionSystemTest::run_receiver() { std::string aux = std::string(buffer); EXPECT_EQ(aux.empty(), false); - StaticPositionSystemTest::generated_kml_file = aux.erase(aux.length() - 1, 1); + PositionSystemTest::generated_kml_file = aux.erase(aux.length() - 1, 1); } pclose(fp); - EXPECT_EQ(StaticPositionSystemTest::generated_kml_file.empty(), false); + EXPECT_EQ(PositionSystemTest::generated_kml_file.empty(), false); return 0; } -void StaticPositionSystemTest::check_results() +void PositionSystemTest::check_results() { std::vector pos_e; std::vector pos_n; @@ -488,7 +488,7 @@ void StaticPositionSystemTest::check_results() if (!FLAGS_use_pvt_solver_dump) { //fall back to read receiver KML output (position only) - std::fstream myfile(StaticPositionSystemTest::generated_kml_file, std::ios_base::in); + std::fstream myfile(PositionSystemTest::generated_kml_file, std::ios_base::in); ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened"; std::string line; // Skip header @@ -638,7 +638,7 @@ void StaticPositionSystemTest::check_results() stm << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; std::cout << stm.rdbuf(); - std::string output_filename = "position_test_output_" + StaticPositionSystemTest::generated_kml_file.erase(StaticPositionSystemTest::generated_kml_file.length() - 3, 3) + "txt"; + std::string output_filename = "position_test_output_" + PositionSystemTest::generated_kml_file.erase(PositionSystemTest::generated_kml_file.length() - 3, 3) + "txt"; position_test_file.open(output_filename.c_str()); if (position_test_file.is_open()) { @@ -732,7 +732,7 @@ void StaticPositionSystemTest::check_results() std::cout << "----- Position and Velocity 3D ECEF error statistics -----" << std::endl; if (!FLAGS_config_file_ptest.empty()) { - std::cout << "Configuration file: " << FLAGS_config_file_ptest << std::endl; + std::cout << "----- Configuration file: " << FLAGS_config_file_ptest << std::endl; } std::streamsize ss = std::cout.precision(); std::cout << std::setprecision(10) << "---- 3D ECEF Position RMSE = " @@ -843,7 +843,7 @@ void StaticPositionSystemTest::check_results() } -void StaticPositionSystemTest::print_results(const std::vector& east, +void PositionSystemTest::print_results(const std::vector& east, const std::vector& north, const std::vector& up) { @@ -961,7 +961,7 @@ void StaticPositionSystemTest::print_results(const std::vector& east, } } -TEST_F(StaticPositionSystemTest, Position_system_test) +TEST_F(PositionSystemTest, Position_system_test) { if (FLAGS_config_file_ptest.empty()) { From 5ad98885a517fb1986678407475b6f1cdb08fc4d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 6 Sep 2018 12:26:03 +0200 Subject: [PATCH 34/35] Cosmetics --- src/tests/system-tests/position_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 6d099075b..7d66adfc3 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -732,7 +732,7 @@ void PositionSystemTest::check_results() std::cout << "----- Position and Velocity 3D ECEF error statistics -----" << std::endl; if (!FLAGS_config_file_ptest.empty()) { - std::cout << "----- Configuration file: " << FLAGS_config_file_ptest << std::endl; + std::cout << "---- Configuration file: " << FLAGS_config_file_ptest << std::endl; } std::streamsize ss = std::cout.precision(); std::cout << std::setprecision(10) << "---- 3D ECEF Position RMSE = " From a5205c1f7118c0e8292d1bc56fcd582725f5e7e3 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 7 Sep 2018 14:36:11 +0200 Subject: [PATCH 35/35] Add option -DENABLE_UNIT_TESTING_MINIMAL for building on systems with limited memory --- CMakeLists.txt | 1 + src/tests/CMakeLists.txt | 6 +- src/tests/test_main.cc | 16 ++++ .../acquisition/acq_performance_test.cc | 6 ++ .../libs/CMakeLists.txt | 1 + .../libs/acquisition_msg_rx.cc | 70 ++++++++++++++++ .../libs/acquisition_msg_rx.h | 62 ++++++++++++++ .../observables/hybrid_observables_test.cc | 9 +++ .../tracking/tracking_pull-in_test.cc | 80 ++++--------------- 9 files changed, 185 insertions(+), 66 deletions(-) create mode 100644 src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.cc create mode 100644 src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 56392b083..314bbab62 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ endif(ENABLE_PACKAGING) # Testing option(ENABLE_UNIT_TESTING "Build unit tests" ON) +option(ENABLE_UNIT_TESTING_MINIMAL "Build a minimal set of unit tests" OFF) option(ENABLE_UNIT_TESTING_EXTRA "Download external files and build extra unit tests" OFF) option(ENABLE_SYSTEM_TESTING "Build system tests" OFF) option(ENABLE_SYSTEM_TESTING_EXTRA "Download external tools and build extra system tests" OFF) diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index fcde4ab50..4821afbd4 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -168,9 +168,13 @@ if(GNUPLOT_FOUND) endif(GNUPLOT_FOUND) if(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.15" ) - add_definitions( -DGR_GREATER_38=1 ) + add_definitions( -DGR_GREATER_38=1 ) endif(${PC_GNURADIO_RUNTIME_VERSION} VERSION_GREATER "3.7.15" ) +if(ENABLE_UNIT_TESTING_MINIMAL) + add_definitions(-DUNIT_TESTING_MINIMAL=1) +endif(ENABLE_UNIT_TESTING_MINIMAL) + ################################################################################ # Optional generator diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index b2c8c3342..9d3aaaf38 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -69,6 +69,21 @@ using google::LogMessage; DECLARE_string(log_dir); +#if UNIT_TESTING_MINIMAL + +#include "unit-tests/arithmetic/matio_test.cc" +#if EXTRA_TESTS +#include "unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc" +#include "unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc" +#if ENABLE_FPGA +#include "unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test_fpga.cc" +#endif +#include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc" +#endif + + +#else + #include "unit-tests/arithmetic/complex_carrier_test.cc" #include "unit-tests/arithmetic/conjugate_test.cc" #include "unit-tests/arithmetic/magnitude_squared_test.cc" @@ -163,6 +178,7 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc" #endif +#endif // UNIT_TESTING_MINIMAL // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc index f61727542..d0dcc4673 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc @@ -37,6 +37,10 @@ #include "glonass_l2_ca_pcps_acquisition.h" #include "gps_l2_m_pcps_acquisition.h" #include "gps_l5i_pcps_acquisition.h" +#include "in_memory_configuration.h" +#include "file_configuration.h" +#include "gnss_sdr_valve.h" +#include "acquisition_dump_reader.h" #include "display.h" #include "gnuplot_i.h" #include "signal_generator_flags.h" @@ -45,6 +49,8 @@ #include "true_observables_reader.h" #include #include +#include +#include #include diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt b/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt index b9a0eb93e..e19fb1cac 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt +++ b/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt @@ -19,6 +19,7 @@ set(SIGNAL_PROCESSING_TESTING_LIB_SOURCES acquisition_dump_reader.cc + acquisition_msg_rx.cc tracking_dump_reader.cc tlm_dump_reader.cc observables_dump_reader.cc diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.cc b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.cc new file mode 100644 index 000000000..5f7d18b77 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.cc @@ -0,0 +1,70 @@ +/*! + * \file acquisition_msg_rx.cc + * \brief This is a helper class to catch the asynchronous messages + * emitted by an acquisition block. + * \author Carles Fernandez-Prades, 2018. cfernandez(at)cttc.cat + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "acquisition_msg_rx.h" +#include +#include +#include +#include + + +Acquisition_msg_rx_sptr Acquisition_msg_rx_make() +{ + return Acquisition_msg_rx_sptr(new Acquisition_msg_rx()); +} + + +void Acquisition_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + int64_t message = pmt::to_long(msg); + rx_message = message; + top_block->stop(); // stop the flowgraph + } + catch (boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_acquisition Bad cast!\n"; + rx_message = 0; + } +} + + +Acquisition_msg_rx::Acquisition_msg_rx() : gr::block("Acquisition_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&Acquisition_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + + +Acquisition_msg_rx::~Acquisition_msg_rx() {} diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.h b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.h new file mode 100644 index 000000000..2095bff72 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_msg_rx.h @@ -0,0 +1,62 @@ +/*! + * \file acquisition_msg_rx.h + * \brief This is a helper class to catch the asynchronous messages + * emitted by an acquisition block. + * \author Carles Fernandez-Prades, 2018. cfernandez(at)cttc.cat + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_ACQUISITION_MSG_RX_H +#define GNSS_SDR_ACQUISITION_MSG_RX_H + +#include +#include +#include + +// ######## GNURADIO ACQUISITION BLOCK MESSAGE RECEVER ######### +class Acquisition_msg_rx; + +typedef boost::shared_ptr Acquisition_msg_rx_sptr; + +Acquisition_msg_rx_sptr Acquisition_msg_rx_make(); + + +class Acquisition_msg_rx : public gr::block +{ +private: + friend Acquisition_msg_rx_sptr Acquisition_msg_rx_make(); + void msg_handler_events(pmt::pmt_t msg); + Acquisition_msg_rx(); + +public: + int rx_message; + gr::top_block_sptr top_block; + ~Acquisition_msg_rx(); //!< Default destructor +}; + + +#endif 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 961757087..81bb788ee 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 @@ -49,6 +49,15 @@ #include "gnss_satellite.h" #include "gnss_block_factory.h" #include "gnss_block_interface.h" +#include "acquisition_msg_rx.h" +#include "gps_l1_ca_pcps_acquisition.h" +#include "galileo_e1_pcps_ambiguous_acquisition.h" +#include "galileo_e5a_pcps_acquisition.h" +#include "galileo_e5a_noncoherent_iq_acquisition_caf.h" +#include "glonass_l1_ca_pcps_acquisition.h" +#include "glonass_l2_ca_pcps_acquisition.h" +#include "gps_l2_m_pcps_acquisition.h" +#include "gps_l5i_pcps_acquisition.h" #include "tracking_interface.h" #include "telemetry_decoder_interface.h" #include "in_memory_configuration.h" diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc index 9818d8977..0daa3cf59 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc @@ -30,24 +30,14 @@ * ------------------------------------------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + #include "GPS_L1_CA.h" #include "gnss_block_factory.h" #include "tracking_interface.h" #include "gps_l2_m_pcps_acquisition.h" #include "gps_l1_ca_pcps_acquisition.h" #include "gps_l1_ca_pcps_acquisition_fine_doppler.h" +#include "galileo_e1_pcps_ambiguous_acquisition.h" #include "galileo_e5a_noncoherent_iq_acquisition_caf.h" #include "galileo_e5a_pcps_acquisition.h" #include "gps_l5i_pcps_acquisition.h" @@ -58,61 +48,21 @@ #include "gnuplot_i.h" #include "test_flags.h" #include "tracking_tests_flags.h" +#include "acquisition_msg_rx.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -// ######## GNURADIO ACQUISITION BLOCK MESSAGE RECEVER ######### -class Acquisition_msg_rx; - -typedef boost::shared_ptr Acquisition_msg_rx_sptr; - -Acquisition_msg_rx_sptr Acquisition_msg_rx_make(); - - -class Acquisition_msg_rx : public gr::block -{ -private: - friend Acquisition_msg_rx_sptr Acquisition_msg_rx_make(); - void msg_handler_events(pmt::pmt_t msg); - Acquisition_msg_rx(); - -public: - int rx_message; - gr::top_block_sptr top_block; - ~Acquisition_msg_rx(); //!< Default destructor -}; - - -Acquisition_msg_rx_sptr Acquisition_msg_rx_make() -{ - return Acquisition_msg_rx_sptr(new Acquisition_msg_rx()); -} - - -void Acquisition_msg_rx::msg_handler_events(pmt::pmt_t msg) -{ - try - { - int64_t message = pmt::to_long(msg); - rx_message = message; - top_block->stop(); //stop the flowgraph - } - catch (boost::bad_any_cast& e) - { - LOG(WARNING) << "msg_handler_acquisition Bad cast!\n"; - rx_message = 0; - } -} - - -Acquisition_msg_rx::Acquisition_msg_rx() : gr::block("Acquisition_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) -{ - this->message_port_register_in(pmt::mp("events")); - this->set_msg_handler(pmt::mp("events"), boost::bind(&Acquisition_msg_rx::msg_handler_events, this, _1)); - rx_message = 0; -} - - -Acquisition_msg_rx::~Acquisition_msg_rx() {} // ######## GNURADIO TRACKING BLOCK MESSAGE RECEVER ######### class TrackingPullInTest_msg_rx;