diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index a94d4ddeb..39405522f 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -39,10 +39,12 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr::sync_decimator("sample_counter", gr::io_signature::make(1, 1, _size), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), - static_cast(std::floor(_fs * 0.001))) + static_cast(std::round(_fs * 0.001))) { message_port_register_out(pmt::mp("sample_counter")); set_max_noutput_items(1); + samples_per_output = std::round(_fs * 0.001); + sample_counter = 0; current_T_rx_ms = 0; current_s = 0; current_m = 0; @@ -69,6 +71,9 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), { Gnss_Synchro *out = reinterpret_cast(output_items[0]); out[0] = Gnss_Synchro(); + out[0].Flag_valid_symbol_output = false; + out[0].Flag_valid_word = false; + out[0].Channel_ID = -1; if ((current_T_rx_ms % report_interval_ms) == 0) { current_s++; @@ -127,6 +132,8 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); } } + sample_counter += samples_per_output; + out[0].Tracking_sample_counter = sample_counter; current_T_rx_ms++; return 1; } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index f398684f5..3c3378c4d 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -45,6 +45,8 @@ class gnss_sdr_sample_counter : public gr::sync_decimator { private: gnss_sdr_sample_counter(double _fs, size_t _size); + unsigned int samples_per_output; + unsigned long int sample_counter; long long int current_T_rx_ms; // Receiver time in ms since the beginning of the run unsigned int current_s; // Receiver time in seconds, modulo 60 bool flag_m; // True if the receiver has been running for at least 1 minute diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 395f7a7fd..899913024 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -259,41 +259,48 @@ bool gps_l1_ca_telemetry_decoder_cc::decode_subframe() if (CRC_ok) { int subframe_ID = d_nav.subframe_decoder(subframe); //decode the subframe - std::cout << "New GPS NAV message received in channel " << this->d_channel << ": " - << "subframe " - << subframe_ID << " from satellite " - << Gnss_Satellite(std::string("GPS"), d_nav.i_satellite_PRN) << std::endl; - - switch (subframe_ID) + if (subframe_ID > 0 and subframe_ID < 6) { - case 3: //we have a new set of ephemeris data for the current SV - if (d_nav.satellite_validation() == true) + std::cout << "New GPS NAV message received in channel " << this->d_channel << ": " + << "subframe " + << subframe_ID << " from satellite " + << Gnss_Satellite(std::string("GPS"), d_nav.i_satellite_PRN) << std::endl; + + switch (subframe_ID) { - // get ephemeris object for this SV (mandatory) - std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + case 3: //we have a new set of ephemeris data for the current SV + if (d_nav.satellite_validation() == true) + { + // get ephemeris object for this SV (mandatory) + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 4: // Possible IONOSPHERE and UTC model update (page 18) + if (d_nav.flag_iono_valid == true) + { + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_iono()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + if (d_nav.flag_utc_model_valid == true) + { + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_utc_model()); + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + break; + case 5: + // get almanac (if available) + //TODO: implement almanac reader in navigation_message + break; + default: + break; } - break; - case 4: // Possible IONOSPHERE and UTC model update (page 18) - if (d_nav.flag_iono_valid == true) - { - std::shared_ptr tmp_obj = std::make_shared(d_nav.get_iono()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - if (d_nav.flag_utc_model_valid == true) - { - std::shared_ptr tmp_obj = std::make_shared(d_nav.get_utc_model()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - } - break; - case 5: - // get almanac (if available) - //TODO: implement almanac reader in navigation_message - break; - default: - break; + d_flag_new_tow_available = true; + } + else + { + return false; } - d_flag_new_tow_available = true; } return subframe_synchro_confirmation; @@ -419,7 +426,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ //2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_flag_new_tow_available == true) { - d_TOW_at_current_symbol_ms = static_cast(d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS; + d_TOW_at_current_symbol_ms = static_cast(d_nav.d_TOW) * 1000 + GPS_CA_PREAMBLE_DURATION_MS; d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms; flag_TOW_set = true; d_flag_new_tow_available = false; diff --git a/src/tests/common-files/observable_tests_flags.h b/src/tests/common-files/observable_tests_flags.h new file mode 100644 index 000000000..6e4f1a6ea --- /dev/null +++ b/src/tests/common-files/observable_tests_flags.h @@ -0,0 +1,40 @@ +/*! + * \file tracking_tests_flags.h + * \brief Helper file for unit testing + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-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_OBSERVABLE_TESTS_FLAGS_H_ +#define GNSS_SDR_OBSERVABLE_TESTS_FLAGS_H_ + +#include +#include + +DEFINE_double(skip_obs_transitory_s, 30.0, "Skip the initial observable outputs to avoid transitory results [s]"); + + +#endif diff --git a/src/tests/common-files/signal_generator_flags.h b/src/tests/common-files/signal_generator_flags.h index e65e98846..399c1ce26 100644 --- a/src/tests/common-files/signal_generator_flags.h +++ b/src/tests/common-files/signal_generator_flags.h @@ -45,6 +45,7 @@ DEFINE_string(filename_raw_data, "signal_out.bin", "Filename of output raw data DEFINE_int32(fs_gen_sps, 2600000, "Sampling frequency [sps]"); DEFINE_int32(test_satellite_PRN, 1, "PRN of the satellite under test (must be visible during the observation time)"); DEFINE_int32(test_satellite_PRN2, 2, "PRN of the satellite under test (must be visible during the observation time)"); +DEFINE_string(test_satellite_PRN_list, "1,2,3,6,9,10,12,17,20,23,28", "List of PRN of the satellites under test (must be visible during the observation time)"); DEFINE_double(CN0_dBHz, std::numeric_limits::infinity(), "Enable noise generator and set the CN0 [dB-Hz]"); #endif diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.cc index b1f9d1d9b..4e670c5d8 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.cc @@ -38,7 +38,7 @@ bool true_observables_reader::read_binary_obs() for (int i = 0; i < 12; i++) { d_dump_file.read(reinterpret_cast(&gps_time_sec[i]), sizeof(double)); - d_dump_file.read(reinterpret_cast(&doppler_l1_hz), sizeof(double)); + d_dump_file.read(reinterpret_cast(&doppler_l1_hz[i]), sizeof(double)); d_dump_file.read(reinterpret_cast(&acc_carrier_phase_l1_cycles[i]), sizeof(double)); d_dump_file.read(reinterpret_cast(&dist_m[i]), sizeof(double)); d_dump_file.read(reinterpret_cast(&true_dist_m[i]), sizeof(double)); 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 a3247d2a5..4c23d9f25 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 @@ -59,6 +59,7 @@ #include "gnss_sdr_sample_counter.h" #include #include "test_flags.h" +#include "observable_tests_flags.h" #include "gnuplot_i.h" @@ -188,24 +189,27 @@ public: bool save_mat_xy(std::vector& x, std::vector& y, std::string filename); void check_results_carrier_phase( arma::mat& true_ch0, - arma::mat& true_ch1, arma::vec& true_tow_s, arma::mat& measured_ch0, - arma::mat& measured_ch1); - void check_results_code_psudorange( + std::string data_title); + void check_results_carrier_doppler( + arma::mat& true_ch0, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + std::string data_title); + void check_results_code_pseudorange( arma::mat& true_ch0, arma::mat& true_ch1, arma::vec& true_tow_s, arma::mat& measured_ch0, - arma::mat& measured_ch1); + arma::mat& measured_ch1, + std::string data_title); HybridObservablesTest() { factory = std::make_shared(); config = std::make_shared(); item_size = sizeof(gr_complex); - gnss_synchro_ch0 = Gnss_Synchro(); - gnss_synchro_ch1 = Gnss_Synchro(); } ~HybridObservablesTest() @@ -217,8 +221,7 @@ public: gr::top_block_sptr top_block; std::shared_ptr factory; std::shared_ptr config; - Gnss_Synchro gnss_synchro_ch0; - Gnss_Synchro gnss_synchro_ch1; + std::vector gnss_synchro_vec; size_t item_size; }; @@ -268,18 +271,6 @@ int HybridObservablesTest::generate_signal() void HybridObservablesTest::configure_receiver() { - gnss_synchro_ch0.Channel_ID = 0; - gnss_synchro_ch0.System = 'G'; - std::string signal = "1C"; - signal.copy(gnss_synchro_ch0.Signal, 2, 0); - gnss_synchro_ch0.PRN = FLAGS_test_satellite_PRN; - - gnss_synchro_ch1.Channel_ID = 1; - gnss_synchro_ch1.System = 'G'; - signal.copy(gnss_synchro_ch1.Signal, 2, 0); - gnss_synchro_ch1.PRN = FLAGS_test_satellite_PRN2; - - config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(baseband_sampling_freq)); // Set Tracking @@ -290,7 +281,7 @@ void HybridObservablesTest::configure_receiver() config->set_property("Tracking_1C.dll_bw_hz", "0.20"); config->set_property("Tracking_1C.pll_bw_narrow_hz", "1.0"); config->set_property("Tracking_1C.dll_bw_narrow_hz", "0.1"); - config->set_property("Tracking_1C.extend_correlation_symbols", "20"); + config->set_property("Tracking_1C.extend_correlation_symbols", "1"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); config->set_property("TelemetryDecoder_1C.dump", "true"); @@ -299,36 +290,31 @@ void HybridObservablesTest::configure_receiver() void HybridObservablesTest::check_results_carrier_phase( arma::mat& true_ch0, - arma::mat& true_ch1, arma::vec& true_tow_s, arma::mat& measured_ch0, - arma::mat& measured_ch1) + std::string data_title) { //1. True value interpolation to match the measurement times - double t0 = std::max(measured_ch0(0, 0), measured_ch1(0, 0)); + double t0 = measured_ch0(0, 0); int size1 = measured_ch0.col(0).n_rows; - int size2 = measured_ch1.col(0).n_rows; - double t1 = std::min(measured_ch0(size1 - 1, 0), measured_ch1(size2 - 1, 0)); + double t1 = measured_ch0(size1 - 1, 0); arma::vec t = arma::linspace(t0, t1, floor((t1 - t0) * 1e3)); + //conversion between arma::vec and std:vector + arma::vec t_from_start = arma::linspace(0, t1 - t0, floor((t1 - t0) * 1e3)); + std::vector time_vector(t_from_start.colptr(0), t_from_start.colptr(0) + t_from_start.n_rows); arma::vec true_ch0_phase_interp; - arma::vec true_ch1_phase_interp; arma::interp1(true_tow_s, true_ch0.col(3), t, true_ch0_phase_interp); - arma::interp1(true_tow_s, true_ch1.col(3), t, true_ch1_phase_interp); arma::vec meas_ch0_phase_interp; - arma::vec meas_ch1_phase_interp; arma::interp1(measured_ch0.col(0), measured_ch0.col(3), t, meas_ch0_phase_interp); - arma::interp1(measured_ch1.col(0), measured_ch1.col(3), t, meas_ch1_phase_interp); //2. RMSE arma::vec err_ch0_cycles; - arma::vec err_ch1_cycles; //compute error without the accumulated carrier phase offsets (which depends on the receiver starting time) err_ch0_cycles = meas_ch0_phase_interp - true_ch0_phase_interp - meas_ch0_phase_interp(0) + true_ch0_phase_interp(0); - err_ch1_cycles = meas_ch1_phase_interp - true_ch1_phase_interp - meas_ch1_phase_interp(0) + true_ch1_phase_interp(0); arma::vec err2_ch0 = arma::square(err_ch0_cycles); double rmse_ch0 = sqrt(arma::mean(err2_ch0)); @@ -341,21 +327,9 @@ void HybridObservablesTest::check_results_carrier_phase( double max_error_ch0 = arma::max(err_ch0_cycles); double min_error_ch0 = arma::min(err_ch0_cycles); - arma::vec err2_ch1 = arma::square(err_ch1_cycles); - double rmse_ch1 = sqrt(arma::mean(err2_ch1)); - - //3. Mean err and variance - double error_mean_ch1 = arma::mean(err_ch1_cycles); - double error_var_ch1 = arma::var(err_ch1_cycles); - - // 4. Peaks - double max_error_ch1 = arma::max(err_ch1_cycles); - double min_error_ch1 = arma::min(err_ch1_cycles); - - //5. report std::streamsize ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Channel 0 Carrier phase RMSE = " + std::cout << std::setprecision(10) << data_title << " Accumulated Carrier phase RMSE = " << rmse_ch0 << ", mean = " << error_mean_ch0 << ", stdev = " << sqrt(error_var_ch0) << " (max,min) = " << max_error_ch0 @@ -363,31 +337,115 @@ void HybridObservablesTest::check_results_carrier_phase( << " [cycles]" << std::endl; std::cout.precision(ss); + //plots + Gnuplot g3("linespoints"); + g3.set_title(data_title + "Accumulated Carrier phase error [cycles]"); + g3.set_grid(); + g3.set_xlabel("Time [s]"); + g3.set_ylabel("Carrier Phase error [cycles]"); + //conversion between arma::vec and std:vector + std::vector error_vec(err_ch0_cycles.colptr(0), err_ch0_cycles.colptr(0) + err_ch0_cycles.n_rows); + g3.cmd("set key box opaque"); + g3.plot_xy(time_vector, error_vec, + "Delta pseudorrange error"); + g3.set_legend(); + g3.savetops(data_title + "Carrier_phase_error"); + if (FLAGS_show_plots) + { + g3.showonscreen(); // window output + } + else + { + g3.disablescreen(); + } + + ASSERT_LT(rmse_ch0, 5e-2); ASSERT_LT(error_mean_ch0, 5e-2); ASSERT_GT(error_mean_ch0, -5e-2); ASSERT_LT(error_var_ch0, 5e-2); ASSERT_LT(max_error_ch0, 5e-2); ASSERT_GT(min_error_ch0, -5e-2); - - //5. report - ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Channel 1 Carrier phase RMSE = " - << rmse_ch1 << ", mean = " << error_mean_ch1 - << ", stdev = " << sqrt(error_var_ch1) - << " (max,min) = " << max_error_ch1 - << "," << min_error_ch1 - << " [cycles]" << std::endl; - std::cout.precision(ss); - - ASSERT_LT(rmse_ch1, 5e-2); - ASSERT_LT(error_mean_ch1, 5e-2); - ASSERT_GT(error_mean_ch1, -5e-2); - ASSERT_LT(error_var_ch1, 5e-2); - ASSERT_LT(max_error_ch1, 5e-2); - ASSERT_GT(min_error_ch1, -5e-2); } +void HybridObservablesTest::check_results_carrier_doppler( + arma::mat& true_ch0, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + std::string data_title) +{ + //1. True value interpolation to match the measurement times + + double t0 = measured_ch0(0, 0); + int size1 = measured_ch0.col(0).n_rows; + double t1 = measured_ch0(size1 - 1, 0); + arma::vec t = arma::linspace(t0, t1, floor((t1 - t0) * 1e3)); + //conversion between arma::vec and std:vector + arma::vec t_from_start = arma::linspace(0, t1 - t0, floor((t1 - t0) * 1e3)); + std::vector time_vector(t_from_start.colptr(0), t_from_start.colptr(0) + t_from_start.n_rows); + + arma::vec true_ch0_doppler_interp; + arma::interp1(true_tow_s, true_ch0.col(2), t, true_ch0_doppler_interp); + + arma::vec meas_ch0_doppler_interp; + arma::interp1(measured_ch0.col(0), measured_ch0.col(2), t, meas_ch0_doppler_interp); + + //2. RMSE + arma::vec err_ch0_hz; + + //compute error + err_ch0_hz = meas_ch0_doppler_interp - true_ch0_doppler_interp; + + arma::vec err2_ch0 = arma::square(err_ch0_hz); + double rmse_ch0 = sqrt(arma::mean(err2_ch0)); + + //3. Mean err and variance + double error_mean_ch0 = arma::mean(err_ch0_hz); + double error_var_ch0 = arma::var(err_ch0_hz); + + // 4. Peaks + double max_error_ch0 = arma::max(err_ch0_hz); + double min_error_ch0 = arma::min(err_ch0_hz); + + //5. report + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << data_title << "Carrier Doppler RMSE = " + << rmse_ch0 << ", mean = " << error_mean_ch0 + << ", stdev = " << sqrt(error_var_ch0) + << " (max,min) = " << max_error_ch0 + << "," << min_error_ch0 + << " [Hz]" << std::endl; + std::cout.precision(ss); + + //plots + Gnuplot g3("linespoints"); + g3.set_title(data_title + "Carrier Doppler error [Hz]"); + g3.set_grid(); + g3.set_xlabel("Time [s]"); + g3.set_ylabel("Carrier Doppler error [Hz]"); + //conversion between arma::vec and std:vector + std::vector error_vec(err_ch0_hz.colptr(0), err_ch0_hz.colptr(0) + err_ch0_hz.n_rows); + g3.cmd("set key box opaque"); + g3.plot_xy(time_vector, error_vec, + "Delta pseudorrange error"); + g3.set_legend(); + g3.savetops(data_title + "Carrier_doppler_error"); + if (FLAGS_show_plots) + { + g3.showonscreen(); // window output + } + else + { + g3.disablescreen(); + } + + ASSERT_LT(rmse_ch0, 5); + ASSERT_LT(error_mean_ch0, 5); + ASSERT_GT(error_mean_ch0, -5); + ASSERT_LT(error_var_ch0, 10); + ASSERT_LT(max_error_ch0, 10); + ASSERT_GT(min_error_ch0, -5); +} bool HybridObservablesTest::save_mat_xy(std::vector& x, std::vector& y, std::string filename) { @@ -424,12 +482,13 @@ bool HybridObservablesTest::save_mat_xy(std::vector& x, std::vector start, end; std::chrono::duration elapsed_seconds(0); + Gnss_Synchro tmp_gnss_synchro; + tmp_gnss_synchro.System = 'G'; + std::string signal = "1C"; + signal.copy(tmp_gnss_synchro.Signal, 2, 0); + + std::istringstream ss(FLAGS_test_satellite_PRN_list); + std::string token; + + while (std::getline(ss, token, ',')) + { + tmp_gnss_synchro.PRN = boost::lexical_cast(token); + gnss_synchro_vec.push_back(tmp_gnss_synchro); + } + configure_receiver(); //open true observables log file written by the simulator - tracking_true_obs_reader true_obs_data_ch0; - tracking_true_obs_reader true_obs_data_ch1; - int test_satellite_PRN = FLAGS_test_satellite_PRN; - int test_satellite_PRN2 = FLAGS_test_satellite_PRN2; - std::cout << "Testing satellite PRNs " << test_satellite_PRN << "," << test_satellite_PRN2 << std::endl; - std::string true_obs_file = std::string("./gps_l1_ca_obs_prn"); - true_obs_file.append(std::to_string(test_satellite_PRN)); - true_obs_file.append(".dat"); - ASSERT_NO_THROW({ - if (true_obs_data_ch0.open_obs_file(true_obs_file) == false) - { - throw std::exception(); - }; - }) << "Failure opening true observables file"; + std::vector> true_reader_vec; + std::vector> tracking_ch_vec; + std::vector> tlm_ch_vec; - true_obs_file = std::string("./gps_l1_ca_obs_prn"); - true_obs_file.append(std::to_string(test_satellite_PRN2)); - true_obs_file.append(".dat"); - ASSERT_NO_THROW({ - if (true_obs_data_ch1.open_obs_file(true_obs_file) == false) - { - throw std::exception(); - }; - }) << "Failure opening true observables file"; + std::vector null_sink_vec; + for (unsigned int n = 0; n < gnss_synchro_vec.size(); n++) + { + //set channels ids + gnss_synchro_vec.at(n).Channel_ID = n; + //read true data from the generator logs + true_reader_vec.push_back(std::make_shared()); + std::cout << "Loading true observable data for PRN " << gnss_synchro_vec.at(n).PRN << std::endl; + std::string true_obs_file = std::string("./gps_l1_ca_obs_prn"); + true_obs_file.append(std::to_string(gnss_synchro_vec.at(n).PRN)); + true_obs_file.append(".dat"); + ASSERT_NO_THROW({ + if (true_reader_vec.back()->open_obs_file(true_obs_file) == false) + { + throw std::exception(); + }; + }) << "Failure opening true observables file"; + + // load acquisition data based on the first epoch of the true observations + ASSERT_NO_THROW({ + if (true_reader_vec.back()->read_binary_obs() == false) + { + throw std::exception(); + }; + }) << "Failure reading true observables file"; + + //restart the epoch counter + true_reader_vec.back()->restart(); + + std::cout << "Initial Doppler [Hz]=" << true_reader_vec.back()->doppler_l1_hz << " Initial code delay [Chips]=" + << true_reader_vec.back()->prn_delay_chips << std::endl; + + gnss_synchro_vec.at(n).Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_reader_vec.back()->prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; + gnss_synchro_vec.at(n).Acq_doppler_hz = true_reader_vec.back()->doppler_l1_hz; + gnss_synchro_vec.at(n).Acq_samplestamp_samples = 0; + + + //create the tracking channels + tracking_ch_vec.push_back(std::make_shared(config.get(), "Tracking_1C", 1, 1)); + //create the telemetry decoders + tlm_ch_vec.push_back(std::make_shared(config.get(), "TelemetryDecoder_1C", 1, 1)); + //create null sinks for observables output + null_sink_vec.push_back(gr::blocks::null_sink::make(sizeof(Gnss_Synchro))); + + ASSERT_NO_THROW({ + tlm_ch_vec.back()->set_channel(gnss_synchro_vec.at(n).Channel_ID); + tlm_ch_vec.back()->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_vec.at(n).PRN)); + }) << "Failure setting gnss_synchro."; + + ASSERT_NO_THROW({ + tracking_ch_vec.back()->set_channel(gnss_synchro_vec.at(n).Channel_ID); + }) << "Failure setting channel."; + + ASSERT_NO_THROW({ + tracking_ch_vec.back()->set_gnss_synchro(&gnss_synchro_vec.at(n)); + }) << "Failure setting gnss_synchro."; + } top_block = gr::make_top_block("Telemetry_Decoder test"); - std::shared_ptr tracking_ch0 = std::make_shared(config.get(), "Tracking_1C", 1, 1); - std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); - - boost::shared_ptr msg_rx_ch0 = HybridObservablesTest_msg_rx_make(); - boost::shared_ptr msg_rx_ch1 = HybridObservablesTest_msg_rx_make(); - - // load acquisition data based on the first epoch of the true observations - ASSERT_NO_THROW({ - if (true_obs_data_ch0.read_binary_obs() == false) - { - throw std::exception(); - }; - }) << "Failure reading true observables file"; - - ASSERT_NO_THROW({ - if (true_obs_data_ch1.read_binary_obs() == false) - { - throw std::exception(); - }; - }) << "Failure reading true observables file"; - - //restart the epoch counter - true_obs_data_ch0.restart(); - true_obs_data_ch1.restart(); - - std::cout << "Initial Doppler [Hz]=" << true_obs_data_ch0.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data_ch0.prn_delay_chips << std::endl; - - gnss_synchro_ch0.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data_ch0.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; - gnss_synchro_ch0.Acq_doppler_hz = true_obs_data_ch0.doppler_l1_hz; - gnss_synchro_ch0.Acq_samplestamp_samples = 0; - - std::cout << "Initial Doppler [Hz]=" << true_obs_data_ch1.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data_ch1.prn_delay_chips << std::endl; - - gnss_synchro_ch1.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data_ch1.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; - gnss_synchro_ch1.Acq_doppler_hz = true_obs_data_ch1.doppler_l1_hz; - gnss_synchro_ch1.Acq_samplestamp_samples = 0; - - //telemetry decoders - std::shared_ptr tlm_ch0(new GpsL1CaTelemetryDecoder(config.get(), "TelemetryDecoder_1C", 1, 1)); - std::shared_ptr tlm_ch1(new GpsL1CaTelemetryDecoder(config.get(), "TelemetryDecoder_1C", 1, 1)); - - ASSERT_NO_THROW({ - tlm_ch0->set_channel(0); - tlm_ch1->set_channel(1); - - tlm_ch0->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_ch0.PRN)); - tlm_ch1->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_ch1.PRN)); - }) << "Failure setting gnss_synchro."; - - boost::shared_ptr tlm_msg_rx_ch1 = HybridObservablesTest_tlm_msg_rx_make(); - boost::shared_ptr tlm_msg_rx_ch2 = HybridObservablesTest_tlm_msg_rx_make(); - + boost::shared_ptr dummy_msg_rx_trk = HybridObservablesTest_msg_rx_make(); + boost::shared_ptr dummy_tlm_msg_rx = HybridObservablesTest_tlm_msg_rx_make(); //Observables - std::shared_ptr observables(new HybridObservables(config.get(), "Observables", 3, 2)); + std::shared_ptr observables(new HybridObservables(config.get(), "Observables", tracking_ch_vec.size() + 1, tracking_ch_vec.size())); - ASSERT_NO_THROW({ - tracking_ch0->set_channel(gnss_synchro_ch0.Channel_ID); - tracking_ch1->set_channel(gnss_synchro_ch1.Channel_ID); - }) << "Failure setting channel."; + for (unsigned int n = 0; n < tracking_ch_vec.size(); n++) + { + ASSERT_NO_THROW({ + tracking_ch_vec.at(n)->connect(top_block); + }) << "Failure connecting tracking to the top_block."; + } - ASSERT_NO_THROW({ - tracking_ch0->set_gnss_synchro(&gnss_synchro_ch0); - tracking_ch1->set_gnss_synchro(&gnss_synchro_ch1); - }) << "Failure setting gnss_synchro."; - - ASSERT_NO_THROW({ - tracking_ch0->connect(top_block); - tracking_ch1->connect(top_block); - }) << "Failure connecting tracking to the top_block."; ASSERT_NO_THROW({ std::string file = "./" + filename_raw_data; const char* file_name = file.c_str(); gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false); gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); - gr::blocks::null_sink::sptr sink_ch0 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); - gr::blocks::null_sink::sptr sink_ch1 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq), sizeof(gr_complex)); top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); top_block->connect(gr_interleaved_char_to_complex, 0, samp_counter, 0); - //ch0 - top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch0->get_left_block(), 0); - top_block->connect(tracking_ch0->get_right_block(), 0, tlm_ch0->get_left_block(), 0); - top_block->connect(tlm_ch0->get_right_block(), 0, observables->get_left_block(), 0); - top_block->msg_connect(tracking_ch0->get_right_block(), pmt::mp("events"), msg_rx_ch0, pmt::mp("events")); - //ch1 - top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch1->get_left_block(), 0); - top_block->connect(tracking_ch1->get_right_block(), 0, tlm_ch1->get_left_block(), 0); - top_block->connect(tlm_ch1->get_right_block(), 0, observables->get_left_block(), 1); - top_block->msg_connect(tracking_ch1->get_right_block(), pmt::mp("events"), msg_rx_ch1, pmt::mp("events")); - - top_block->connect(observables->get_right_block(), 0, sink_ch0, 0); - top_block->connect(observables->get_right_block(), 1, sink_ch1, 0); - top_block->connect(samp_counter, 0, observables->get_left_block(), 2); + for (unsigned int n = 0; n < tracking_ch_vec.size(); n++) + { + top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch_vec.at(n)->get_left_block(), 0); + top_block->connect(tracking_ch_vec.at(n)->get_right_block(), 0, tlm_ch_vec.at(n)->get_left_block(), 0); + top_block->connect(tlm_ch_vec.at(n)->get_right_block(), 0, observables->get_left_block(), n); + top_block->msg_connect(tracking_ch_vec.at(n)->get_right_block(), pmt::mp("events"), dummy_msg_rx_trk, pmt::mp("events")); + top_block->connect(observables->get_right_block(), n, null_sink_vec.at(n), 0); + } + //connect sample counter and timmer to the last channel in observables block (extra channel) + top_block->connect(samp_counter, 0, observables->get_left_block(), tracking_ch_vec.size()); }) << "Failure connecting the blocks."; - tracking_ch0->start_tracking(); - tracking_ch1->start_tracking(); + for (unsigned int n = 0; n < tracking_ch_vec.size(); n++) + { + tracking_ch_vec.at(n)->start_tracking(); + } EXPECT_NO_THROW({ start = std::chrono::system_clock::now(); @@ -684,40 +731,36 @@ TEST_F(HybridObservablesTest, ValidationOfResults) std::cout << "True observation epochs = " << nepoch << std::endl; // Matrices for storing columnwise true GPS time, Range, Doppler and Carrier phase - arma::mat true_ch0 = arma::zeros(nepoch, 4); - arma::mat true_ch1 = arma::zeros(nepoch, 4); - + std::vector true_obs_vec; true_observables.restart(); long int epoch_counter = 0; + for (unsigned int n = 0; n < tracking_ch_vec.size(); n++) + { + true_obs_vec.push_back(arma::zeros(nepoch, 4)); + } + ASSERT_NO_THROW({ while (true_observables.read_binary_obs()) { - if (round(true_observables.prn[0]) != gnss_synchro_ch0.PRN) + for (unsigned int n = 0; n < true_obs_vec.size(); n++) { - std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; - throw std::exception(); + if (round(true_observables.prn[n]) != gnss_synchro_vec.at(n).PRN) + { + std::cout << "True observables SV PRN does not match measured ones: " + << round(true_observables.prn[n]) << " vs. " << gnss_synchro_vec.at(n).PRN << std::endl; + throw std::exception(); + } + true_obs_vec.at(n)(epoch_counter, 0) = true_observables.gps_time_sec[n]; + true_obs_vec.at(n)(epoch_counter, 1) = true_observables.dist_m[n]; + true_obs_vec.at(n)(epoch_counter, 2) = true_observables.doppler_l1_hz[n]; + true_obs_vec.at(n)(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[n]; } - if (round(true_observables.prn[1]) != gnss_synchro_ch1.PRN) - { - std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; - throw std::exception(); - } - true_ch0(epoch_counter, 0) = true_observables.gps_time_sec[0]; - true_ch0(epoch_counter, 1) = true_observables.dist_m[0]; - true_ch0(epoch_counter, 2) = true_observables.doppler_l1_hz[0]; - true_ch0(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[0]; - - true_ch1(epoch_counter, 0) = true_observables.gps_time_sec[1]; - true_ch1(epoch_counter, 1) = true_observables.dist_m[1]; - true_ch1(epoch_counter, 2) = true_observables.doppler_l1_hz[1]; - true_ch1(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[1]; - epoch_counter++; } }); //read measured values - observables_dump_reader estimated_observables(2); //two channels + observables_dump_reader estimated_observables(tracking_ch_vec.size()); ASSERT_NO_THROW({ if (estimated_observables.open_obs_file(std::string("./observables.dat")) == false) { @@ -726,96 +769,114 @@ TEST_F(HybridObservablesTest, ValidationOfResults) }) << "Failure opening dump observables file"; nepoch = static_cast(estimated_observables.num_epochs()); - std::cout << "Measured observation epochs = " << nepoch << std::endl; + std::cout << "Measured observations epochs = " << nepoch << std::endl; // Matrices for storing columnwise measured RX_time, TOW, Doppler, Carrier phase and Pseudorange - arma::mat measured_ch0 = arma::zeros(nepoch, 5); - arma::mat measured_ch1 = arma::zeros(nepoch, 5); + std::vector measured_obs_vec; + std::vector epoch_counters_vec; + for (unsigned int n = 0; n < tracking_ch_vec.size(); n++) + { + measured_obs_vec.push_back(arma::zeros(nepoch, 5)); + epoch_counters_vec.push_back(0); + } estimated_observables.restart(); - epoch_counter = 0; - long int epoch_counter2 = 0; while (estimated_observables.read_binary_obs()) { - if (static_cast(estimated_observables.valid[0])) + for (unsigned int n = 0; n < measured_obs_vec.size(); n++) { - measured_ch0(epoch_counter, 0) = estimated_observables.RX_time[0]; - measured_ch0(epoch_counter, 1) = estimated_observables.TOW_at_current_symbol_s[0]; - measured_ch0(epoch_counter, 2) = estimated_observables.Carrier_Doppler_hz[0]; - measured_ch0(epoch_counter, 3) = estimated_observables.Acc_carrier_phase_hz[0]; - measured_ch0(epoch_counter, 4) = estimated_observables.Pseudorange_m[0]; - epoch_counter++; - } - if (static_cast(estimated_observables.valid[1])) - { - measured_ch1(epoch_counter2, 0) = estimated_observables.RX_time[1]; - measured_ch1(epoch_counter2, 1) = estimated_observables.TOW_at_current_symbol_s[1]; - measured_ch1(epoch_counter2, 2) = estimated_observables.Carrier_Doppler_hz[1]; - measured_ch1(epoch_counter2, 3) = estimated_observables.Acc_carrier_phase_hz[1]; - measured_ch1(epoch_counter2, 4) = estimated_observables.Pseudorange_m[1]; - epoch_counter2++; + if (static_cast(estimated_observables.valid[n])) + { + measured_obs_vec.at(n)(epoch_counters_vec.at(n), 0) = estimated_observables.RX_time[n]; + measured_obs_vec.at(n)(epoch_counters_vec.at(n), 1) = estimated_observables.TOW_at_current_symbol_s[n]; + measured_obs_vec.at(n)(epoch_counters_vec.at(n), 2) = estimated_observables.Carrier_Doppler_hz[n]; + measured_obs_vec.at(n)(epoch_counters_vec.at(n), 3) = estimated_observables.Acc_carrier_phase_hz[n]; + measured_obs_vec.at(n)(epoch_counters_vec.at(n), 4) = estimated_observables.Pseudorange_m[n]; + epoch_counters_vec.at(n)++; + } } } + //Cut measurement tail zeros - arma::uvec index = arma::find(measured_ch0.col(0) > 0.0, 1, "last"); - if ((index.size() > 0) and index(0) < (nepoch - 1)) + arma::uvec index; + for (unsigned int n = 0; n < measured_obs_vec.size(); n++) { - measured_ch0.shed_rows(index(0) + 1, nepoch - 1); - } - index = arma::find(measured_ch1.col(0) > 0.0, 1, "last"); - if ((index.size() > 0) and index(0) < (nepoch - 1)) - { - measured_ch1.shed_rows(index(0) + 1, nepoch - 1); + index = arma::find(measured_obs_vec.at(n).col(0) > 0.0, 1, "last"); + if ((index.size() > 0) and index(0) < (nepoch - 1)) + { + measured_obs_vec.at(n).shed_rows(index(0) + 1, nepoch - 1); + } } //Cut measurement initial transitory of the measurements - double initial_transitory_s = 30.0; + double initial_transitory_s = FLAGS_skip_obs_transitory_s; - index = arma::find(measured_ch0.col(0) >= (measured_ch0(0, 0) + initial_transitory_s), 1, "first"); - if ((index.size() > 0) and (index(0) > 0)) + for (unsigned int n = 0; n < measured_obs_vec.size(); n++) { - measured_ch0.shed_rows(0, index(0)); - } - index = arma::find(measured_ch1.col(0) >= (measured_ch1(0, 0) + initial_transitory_s), 1, "first"); - if ((index.size() > 0) and (index(0) > 0)) - { - measured_ch1.shed_rows(0, index(0)); + index = arma::find(measured_obs_vec.at(n).col(0) >= (measured_obs_vec.at(n)(0, 0) + initial_transitory_s), 1, "first"); + if ((index.size() > 0) and (index(0) > 0)) + { + measured_obs_vec.at(n).shed_rows(0, index(0)); + } + + index = arma::find(measured_obs_vec.at(n).col(0) >= true_obs_vec.at(n)(0, 0), 1, "first"); + if ((index.size() > 0) and (index(0) > 0)) + { + measured_obs_vec.at(n).shed_rows(0, index(0)); + } } - index = arma::find(measured_ch0.col(0) >= true_ch0(0, 0), 1, "first"); - if ((index.size() > 0) and (index(0) > 0)) - { - measured_ch0.shed_rows(0, index(0)); - } - index = arma::find(measured_ch1.col(0) >= true_ch1(0, 0), 1, "first"); - if ((index.size() > 0) and (index(0) > 0)) - { - measured_ch1.shed_rows(0, index(0)); - } - //Correct the clock error using true values (it is not possible for a receiver to correct //the receiver clock offset error at the observables level because it is required the //decoding of the ephemeris data and solve the PVT equations) //Find the reference satellite (the nearest) and compute the receiver time offset at observable level + + double min_pr = std::numeric_limits::max(); + unsigned int min_pr_ch_id = 0; arma::vec receiver_time_offset_s; - if (measured_ch0(0, 4) < measured_ch1(0, 4)) + for (unsigned int n = 0; n < measured_obs_vec.size(); n++) { - receiver_time_offset_s = true_ch0.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + if (measured_obs_vec.at(n)(0, 4) < min_pr) + { + min_pr = measured_obs_vec.at(n)(0, 4); + min_pr_ch_id = n; + } } - else + + receiver_time_offset_s = true_obs_vec.at(min_pr_ch_id).col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + + for (unsigned int n = 0; n < measured_obs_vec.size(); n++) { - receiver_time_offset_s = true_ch1.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + arma::vec corrected_reference_TOW_s = true_obs_vec.at(min_pr_ch_id).col(0) - receiver_time_offset_s; + std::cout << "[CH " << n << "] Receiver time offset " << receiver_time_offset_s(0) * 1e3 << " [ms]" << std::endl; + + //Compare measured observables + if (min_pr_ch_id != n) + { + check_results_code_pseudorange(true_obs_vec.at(n), + true_obs_vec.at(min_pr_ch_id), + corrected_reference_TOW_s, + measured_obs_vec.at(n), + measured_obs_vec.at(min_pr_ch_id), + "[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " "); + } + else + { + std::cout << "[CH " << std::to_string(n) << "] PRN " << std::to_string(gnss_synchro_vec.at(n).PRN) << " is the reference satellite" << std::endl; + } + std::cout << "true_obs_vec.at(n): " << true_obs_vec.at(n)(0, 2) << std::endl; + check_results_carrier_phase(true_obs_vec.at(n), + corrected_reference_TOW_s, + measured_obs_vec.at(n), + "[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " "); + check_results_carrier_doppler(true_obs_vec.at(n), + corrected_reference_TOW_s, + measured_obs_vec.at(n), + "[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " "); } - arma::vec corrected_reference_TOW_s = true_ch0.col(0) - receiver_time_offset_s; - std::cout << "Receiver time offset: " << receiver_time_offset_s(0) * 1e3 << " [ms]" << std::endl; - - //Compare measured observables - check_results_code_psudorange(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1); - check_results_carrier_phase(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1); - std::cout << "Test completed in " << elapsed_seconds.count() << " [s]" << std::endl; }