diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 7f3606051..6ca378e60 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -176,6 +176,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ band2 = true; } } + break; default: { } diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc index e0c743ee8..377938cef 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc @@ -76,16 +76,13 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( acq_parameters.max_dwells = max_dwells_; dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); acq_parameters.dump_filename = dump_filename_; + acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1); //--- Find number of samples per spreading code ------------------------- - code_length_ = static_cast(std::round(static_cast(fs_in_) / (GPS_L5i_CODE_RATE_HZ / static_cast(GPS_L5i_CODE_LENGTH_CHIPS)))); - - vector_length_ = code_length_; - - if (bit_transition_flag_) - { - vector_length_ *= 2; - } + code_length_ = static_cast(std::floor(static_cast(fs_in_) / (GPS_L5i_CODE_RATE_HZ / GPS_L5i_CODE_LENGTH_CHIPS))); + acq_parameters.samples_per_ms = static_cast(fs_in_) * 0.001; + acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast(GPS_L5i_PERIOD * 1000.0); + vector_length_ = std::floor(acq_parameters.sampled_ms * acq_parameters.samples_per_ms) * (acq_parameters.bit_transition_flag ? 2 : 1); code_ = new gr_complex[vector_length_]; if (item_type_.compare("cshort") == 0) @@ -96,11 +93,9 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( { item_size_ = sizeof(gr_complex); } - acq_parameters.samples_per_ms = static_cast(fs_in_) * 0.001; - acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast(GPS_L5i_PERIOD * 1000.0); + acq_parameters.ms_per_code = 1; acq_parameters.it_size = item_size_; - acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1); num_codes_ = acq_parameters.sampled_ms; acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); @@ -108,7 +103,6 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( acq_parameters.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false); acquisition_ = pcps_make_acquisition(acq_parameters); DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; - stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; @@ -117,7 +111,6 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( cbyte_to_float_x2_ = make_complex_byte_to_float_x2(); float_to_complex_ = gr::blocks::float_to_complex::make(); } - channel_ = 0; threshold_ = 0.0; doppler_step_ = 0; diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index 39405522f..b05f17906 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -36,14 +36,16 @@ #include #include -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::round(_fs * 0.001))) +gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, int _interval_ms, 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::round(_fs * static_cast(_interval_ms) / 1e3))) { message_port_register_out(pmt::mp("sample_counter")); set_max_noutput_items(1); - samples_per_output = std::round(_fs * 0.001); + interval_ms = _interval_ms; + fs = _fs; + samples_per_output = std::round(fs * static_cast(interval_ms) / 1e3); sample_counter = 0; current_T_rx_ms = 0; current_s = 0; @@ -58,9 +60,9 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr: } -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size) +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size) { - gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _size)); + gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _interval_ms, _size)); return sample_counter_; } @@ -74,6 +76,7 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), out[0].Flag_valid_symbol_output = false; out[0].Flag_valid_word = false; out[0].Channel_ID = -1; + out[0].fs = fs; if ((current_T_rx_ms % report_interval_ms) == 0) { current_s++; @@ -134,6 +137,6 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), } sample_counter += samples_per_output; out[0].Tracking_sample_counter = sample_counter; - current_T_rx_ms++; + current_T_rx_ms += interval_ms; return 1; } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index 3c3378c4d..5ff72951f 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -39,14 +39,16 @@ class gnss_sdr_sample_counter; typedef boost::shared_ptr gnss_sdr_sample_counter_sptr; -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size); +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size); class gnss_sdr_sample_counter : public gr::sync_decimator { private: - gnss_sdr_sample_counter(double _fs, size_t _size); + gnss_sdr_sample_counter(double _fs, int _interval_ms, size_t _size); unsigned int samples_per_output; - unsigned long int sample_counter; + double fs; + unsigned long long int sample_counter; + int interval_ms; 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 @@ -59,7 +61,7 @@ private: bool flag_enable_send_msg; public: - friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size); + friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 63dc94436..a2ef3e1c7 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -315,7 +315,7 @@ bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const long int old_abs_diff = std::numeric_limits::max(); for (unsigned int i = 0; i < d_gnss_synchro_history->size(ch); i++) { - abs_diff = labs(rx_clock - d_gnss_synchro_history->at(ch, i).Tracking_sample_counter); + abs_diff = labs(static_cast(rx_clock) - static_cast(d_gnss_synchro_history->at(ch, i).Tracking_sample_counter)); if (old_abs_diff > abs_diff) { old_abs_diff = abs_diff; @@ -397,7 +397,7 @@ bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const } void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) { - for (int n = 0; n < static_cast(int)(d_nchannels_in) - 1; n++) + for (int n = 0; n < static_cast(d_nchannels_in) - 1; n++) { ninput_items_required[n] = 0; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index c2008ceaf..a5ffddcd9 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -65,26 +65,25 @@ private: friend hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); - void clean_history(unsigned int pos); - double compute_T_rx_s(const Gnss_Synchro& a); bool interpolate_data(Gnss_Synchro& out, const unsigned int& ch, const double& ti); - void find_interp_elements(const unsigned int& ch, const double& ti); - void correct_TOW_and_compute_prange(std::vector& data); + bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const unsigned int& ch, const unsigned long int& rx_clock); + double compute_T_rx_s(const Gnss_Synchro& a); + void compute_pranges(std::vector& data); + void update_TOW(std::vector& data); int save_matfile(); + //time history + boost::circular_buffer d_Rx_clock_buffer; //Tracking observable history Gnss_circular_deque* d_gnss_synchro_history; - boost::dynamic_bitset<> valid_channels; - double T_rx_s; - unsigned int T_rx_step_ms; + unsigned int T_rx_clock_step_samples; //rx time follow GPST bool T_rx_TOW_set; unsigned int T_rx_TOW_ms; - double max_delta; - double d_latency; + unsigned int T_rx_TOW_offset_ms; bool d_dump; - unsigned int d_nchannels; - unsigned int d_num_valid_channels; + unsigned int d_nchannels_in; + unsigned int d_nchannels_out; std::string d_dump_filename; std::ofstream d_dump_file; }; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc index 9e1dc3394..2e4256b42 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc @@ -117,7 +117,8 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc( d_flag_frame_sync = false; d_flag_parity = false; - d_TOW_at_current_symbol = 0; + d_TOW_at_current_symbol_ms = 0; + d_TOW_at_Preamble_ms = 0; delta_t = 0; d_CRC_error_counter = 0; flag_even_word_arrived = 0; @@ -251,9 +252,9 @@ void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols, in DLOG(INFO) << "T0G=" << tmp_obj->t_0G_10; DLOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10; DLOG(INFO) << "Current parameters:"; - DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol; + DLOG(INFO) << "d_TOW_at_current_symbol_ms=" << d_TOW_at_current_symbol_ms; DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0; - delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64))); + delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (static_cast(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64))); DLOG(INFO) << "delta_t=" << delta_t << "[s]"; } } @@ -406,6 +407,9 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; d_flag_frame_sync = false; d_stat = 0; + d_TOW_at_current_symbol_ms = 0; + d_TOW_at_Preamble_ms = 0; + d_nav.flag_TOW_set = false; } } } @@ -419,73 +423,76 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute if (d_nav.flag_TOW_5 == true) // page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) { // TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_5 + static_cast(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast(required_symbols + 1) * GALILEO_E1_CODE_PERIOD; + d_TOW_at_Preamble_ms = static_cast(d_nav.TOW_5 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS; d_nav.flag_TOW_5 = false; } else if (d_nav.flag_TOW_6 == true) // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) { // TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_6 + static_cast(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast(required_symbols + 1) * GALILEO_E1_CODE_PERIOD; + d_TOW_at_Preamble_ms = static_cast(d_nav.TOW_6 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS; d_nav.flag_TOW_6 = false; } else { // this page has no timing information - d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; } } else // if there is not a new preamble, we define the TOW of the current symbol { - d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; - } - - // if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) - - if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived - { - delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0))); - } - - if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) - { - current_symbol.Flag_valid_word = true; - } - else - { - current_symbol.Flag_valid_word = false; - } - - current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); - // todo: Galileo to GPS time conversion should be moved to observable block. - // current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW - - if (d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try + if (d_nav.flag_TOW_set == true) { - double tmp_double; - unsigned long int tmp_ulong_int; - tmp_double = d_TOW_at_current_symbol; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); - tmp_double = 0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS; } } // remove used symbols from history + // todo: Use circular buffer here if (d_symbol_history.size() > required_symbols) { d_symbol_history.pop_front(); } - // 3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; - return 1; + + if (d_nav.flag_TOW_set) + { + if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived + { + delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (static_cast(d_TOW_at_current_symbol_ms) / 1000.0 - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0))); + } + + current_symbol.Flag_valid_word = d_nav.flag_TOW_set; + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + // todo: Galileo to GPS time conversion should be moved to observable block. + // current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW + + if (d_dump == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + // 3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + return 1; + } + else + { + return 0; + } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h index 8f434f0d5..36803eccf 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h @@ -105,7 +105,8 @@ private: Gnss_Satellite d_satellite; int d_channel; - double d_TOW_at_current_symbol; + unsigned int d_TOW_at_Preamble_ms; + unsigned int d_TOW_at_current_symbol_ms; bool flag_TOW_set; double delta_t; //GPS-GALILEO time offset diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc index 96431880b..a4ef0da6e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc @@ -154,7 +154,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( // initialize internal vars d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); - LOG(INFO) << "GALILEO E5A TELEMETRY PROCESSING: satellite " << d_satellite; // set the preamble for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++) @@ -182,7 +181,8 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( d_flag_preamble = false; d_preamble_index = 0; d_flag_frame_sync = false; - d_TOW_at_current_symbol = 0.0; + d_TOW_at_current_symbol_ms = 0; + d_TOW_at_Preamble_ms = 0; flag_TOW_set = false; d_CRC_error_counter = 0; d_channel = 0; @@ -345,7 +345,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute // ****************** Frame sync ****************** if ((d_stat == 0) && new_symbol) // no preamble information { - if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS) + if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS) { d_preamble_index = d_sample_counter; // record the preamble sample stamp LOG(INFO) << "Preamble detection for Galileo E5a satellite " << d_satellite; @@ -354,7 +354,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute } else if ((d_stat == 1) && new_symbol) // possible preamble lock { - if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS) + if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS) { // check preamble separation preamble_diff = d_sample_counter - d_preamble_index; @@ -418,6 +418,9 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute d_flag_frame_sync = false; d_stat = 0; flag_bit_start = false; + d_nav.flag_TOW_set = false; + d_TOW_at_current_symbol_ms = 0; + d_TOW_at_Preamble_ms = 0; } } } @@ -432,73 +435,72 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute { if (d_nav.flag_TOW_1 == true) { - d_TOW_at_current_symbol = d_nav.FNAV_TOW_1 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_1 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; d_nav.flag_TOW_1 = false; } else if (d_nav.flag_TOW_2 == true) { - d_TOW_at_current_symbol = d_nav.FNAV_TOW_2 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_2 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; d_nav.flag_TOW_2 = false; } else if (d_nav.flag_TOW_3 == true) { - d_TOW_at_current_symbol = d_nav.FNAV_TOW_3 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_3 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; d_nav.flag_TOW_3 = false; } else if (d_nav.flag_TOW_4 == true) { - d_TOW_at_current_symbol = d_nav.FNAV_TOW_4 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_Preamble_ms = static_cast(d_nav.FNAV_TOW_4 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS; d_nav.flag_TOW_4 = false; } else { - d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD; + d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS; } } else // if there is not a new preamble, we define the TOW of the current symbol { - d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD; - } - - //if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) - if (d_flag_frame_sync and d_nav.flag_TOW_set) - { - current_sample.Flag_valid_word = true; - } - else - { - current_sample.Flag_valid_word = false; - } - - current_sample.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); - - if (d_dump) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try + if (d_nav.flag_TOW_set == true) { - double tmp_double; - unsigned long int tmp_ulong_int; - tmp_double = d_TOW_at_current_symbol; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_sample.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); - tmp_double = 0.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what(); + d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS; } } + // remove used symbols from history + // todo: Use circular buffer here while (d_symbol_history.size() > required_symbols) { d_symbol_history.pop_front(); } - // 3. Make the output - if (current_sample.Flag_valid_word) + + if (d_nav.flag_TOW_set) { + current_sample.Flag_valid_word = true; + current_sample.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + if (d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_sample.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what(); + } + } + // 3. Make the output out[0] = current_sample; return 1; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h index 32400ca01..5416af8f5 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h @@ -104,7 +104,8 @@ private: bool new_symbol; double d_prompt_acum; double page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS]; - double d_TOW_at_current_symbol; + unsigned int d_TOW_at_Preamble_ms; + unsigned int d_TOW_at_current_symbol_ms; double delta_t; //GPS-GALILEO time offset std::string d_dump_filename; std::ofstream d_dump_file; 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 899913024..b07b467f6 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 @@ -419,6 +419,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ flag_TOW_set = false; d_current_subframe_symbol = 0; d_crc_error_synchronization_counter = 0; + d_TOW_at_current_symbol_ms = 0; } } } @@ -426,47 +427,57 @@ 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_CA_PREAMBLE_DURATION_MS; - d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms; + d_TOW_at_current_symbol_ms = static_cast(d_nav.d_TOW * 1000.0) + GPS_CA_PREAMBLE_DURATION_MS; + d_TOW_at_Preamble_ms = static_cast(d_nav.d_TOW * 1000.0); flag_TOW_set = true; d_flag_new_tow_available = false; } else { - d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; - } - - current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; - current_symbol.Flag_valid_word = flag_TOW_set; - - if (flag_PLL_180_deg_phase_locked == true) - { - //correct the accumulated phase for the Costas loop phase shift, if required - current_symbol.Carrier_phase_rads += GPS_PI; - } - - if (d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try + if (flag_TOW_set == true) { - double tmp_double; - unsigned long int tmp_ulong_int; - tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); - tmp_double = static_cast(d_TOW_at_Preamble_ms) * 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; } } - //3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_symbol; + if (flag_TOW_set == true) + { + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + current_symbol.Flag_valid_word = flag_TOW_set; - return 1; + if (flag_PLL_180_deg_phase_locked == true) + { + //correct the accumulated phase for the Costas loop phase shift, if required + current_symbol.Carrier_phase_rads += GPS_PI; + } + + if (d_dump == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + + //3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_symbol; + + return 1; + } + else + { + return 0; + } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index 87412096e..ca050a575 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -64,8 +64,8 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite; d_channel = 0; d_flag_valid_word = false; - d_TOW_at_current_symbol = 0.0; - d_TOW_at_Preamble = 0.0; + d_TOW_at_current_symbol_ms = 0; + d_TOW_at_Preamble_ms = 0; //initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); for (int aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++) @@ -236,47 +236,56 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u } //update TOW at the preamble instant - d_TOW_at_Preamble = static_cast(msg.tow) * 6.0; + d_TOW_at_Preamble_ms = msg.tow * 6000; //* The time of the last input symbol can be computed from the message ToW and //* delay by the formulae: //* \code //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory - d_TOW_at_current_symbol = (static_cast(msg.tow) * 6.0) + (static_cast(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD; - d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; + //d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5i_SYMBOL_PERIOD_MS; + + d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5i_SYMBOL_PERIOD_MS; d_flag_valid_word = true; } else { - d_TOW_at_current_symbol += GPS_L5i_PERIOD; + d_TOW_at_current_symbol_ms += GPS_L5i_PERIOD_MS; if (current_synchro_data.Flag_valid_symbol_output == false) { d_flag_valid_word = false; } } - current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); - current_synchro_data.Flag_valid_word = d_flag_valid_word; - if (d_dump == true) + if (d_flag_valid_word == true) { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - unsigned long int tmp_ulong_int; - tmp_double = d_TOW_at_current_symbol; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_synchro_data.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); - tmp_double = d_TOW_at_Preamble; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); - } - } + current_synchro_data.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; + current_synchro_data.Flag_valid_word = d_flag_valid_word; - //3. Make the output (copy the object contents to the GNURadio reserved memory) - out[0] = current_synchro_data; - return 1; + if (d_dump == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_synchro_data.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); + } + } + + //3. Make the output (copy the object contents to the GNURadio reserved memory) + out[0] = current_synchro_data; + return 1; + } + else + { + return 0; + } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h index c5e838382..2970fe7c9 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h @@ -41,8 +41,7 @@ #include #include -extern "C" -{ +extern "C" { #include "cnav_msg.h" #include "edc.h" #include "bits.h" @@ -85,8 +84,8 @@ private: cnav_msg_decoder_t d_cnav_decoder; - double d_TOW_at_current_symbol; - double d_TOW_at_Preamble; + unsigned int d_TOW_at_current_symbol_ms; + unsigned int d_TOW_at_Preamble_ms; bool d_flag_valid_word; Gps_CNAV_Navigation_Message d_CNAV_Message; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index 4cecdfcc5..c514fee5f 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -41,7 +41,9 @@ Dll_Pll_Conf::Dll_Pll_Conf() vector_length = 0; dump = false; dump_filename = "./dll_pll_dump.dat"; - pll_bw_hz = 40.0; + pll_pull_in_bw_hz = 50.0; + dll_pull_in_bw_hz = 3.0; + pll_bw_hz = 35.0; dll_bw_hz = 2.0; pll_bw_narrow_hz = 5.0; dll_bw_narrow_hz = 0.75; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index a8f3ad10b..a6a754363 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -44,6 +44,8 @@ public: unsigned int vector_length; bool dump; std::string dump_filename; + float pll_pull_in_bw_hz; + float dll_pull_in_bw_hz; float pll_bw_hz; float dll_bw_hz; float pll_bw_narrow_hz; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index cb9f4c937..d8a1e6332 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -277,7 +277,8 @@ void GNSSFlowgraph::connect() std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); } - ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0)); + int observable_interval_ms = static_cast(configuration_->property("GNSS-SDR.observable_interval_ms", 20)); + ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, observable_interval_ms, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0)); top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse } @@ -296,8 +297,9 @@ void GNSSFlowgraph::connect() { //null source null_source_ = gr::blocks::null_source::make(sizeof(Gnss_Synchro)); - //throttle 1kHz - throttle_ = gr::blocks::throttle::make(sizeof(Gnss_Synchro), 1000); // 1000 samples per second (1kHz) + //throttle to observable interval + int observable_interval_ms = static_cast(configuration_->property("GNSS-SDR.observable_interval_ms", 20)); + throttle_ = gr::blocks::throttle::make(sizeof(Gnss_Synchro), std::round(1.0 / static_cast(observable_interval_ms))); // 1000 samples per second (1kHz) time_counter_ = gnss_sdr_make_time_counter(); top_block_->connect(null_source_, 0, throttle_, 0); top_block_->connect(throttle_, 0, time_counter_, 0); @@ -323,7 +325,9 @@ void GNSSFlowgraph::connect() std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); } - ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0)); + + int observable_interval_ms = static_cast(configuration_->property("GNSS-SDR.observable_interval_ms", 20)); + ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, observable_interval_ms, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0)); top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse } diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index be6192795..d0b377cc0 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -68,7 +68,7 @@ const double MAX_TOA_DELAY_MS = 20; //#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here //const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) -const double GPS_STARTOFFSET_ms = 69.0; +const double GPS_STARTOFFSET_ms = 60.0; // OBSERVABLE HISTORY DEEP FOR INTERPOLATION const int GPS_L1_CA_HISTORY_DEEP = 100; diff --git a/src/core/system_parameters/GPS_L5.h b/src/core/system_parameters/GPS_L5.h index 5a919820d..31d36e89d 100644 --- a/src/core/system_parameters/GPS_L5.h +++ b/src/core/system_parameters/GPS_L5.h @@ -55,7 +55,9 @@ const double GPS_L5_FREQ_HZ = FREQ5; //!< L5 [Hz] const double GPS_L5i_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s] const int GPS_L5i_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips] const double GPS_L5i_PERIOD = 0.001; //!< GPS L5 code period [seconds] +const int GPS_L5i_PERIOD_MS = 1; //!< GPS L5 code period [ms] const double GPS_L5i_SYMBOL_PERIOD = 0.01; //!< GPS L5 symbol period [seconds] +const int GPS_L5i_SYMBOL_PERIOD_MS = 10; //!< GPS L5 symbol period [ms] const double GPS_L5q_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s] const int GPS_L5q_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips] diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index 951612e02..da005c877 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -80,6 +80,7 @@ const int GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS = 250; const int GALILEO_INAV_PAGE_PART_SYMBOLS = 250; //!< Each Galileo INAV pages are composed of two parts (even and odd) each of 250 symbols, including preamble. See Galileo ICD 4.3.2 const int GALILEO_INAV_PAGE_SYMBOLS = 500; //!< The complete Galileo INAV page length const int GALILEO_INAV_PAGE_PART_SECONDS = 1; // a page part last 1 sec +const int GALILEO_INAV_PAGE_PART_MS = 1000; // a page part last 1 sec const int GALILEO_INAV_PAGE_SECONDS = 2; // a full page last 2 sec const int GALILEO_INAV_INTERLEAVER_ROWS = 8; const int GALILEO_INAV_INTERLEAVER_COLS = 30; @@ -89,6 +90,7 @@ const int GALILEO_DATA_JK_BITS = 128; const int GALILEO_DATA_FRAME_BITS = 196; const int GALILEO_DATA_FRAME_BYTES = 25; const double GALILEO_E1_CODE_PERIOD = 0.004; +const int GALILEO_E1_CODE_PERIOD_MS = 4; const std::vector> type({{1, 6}}); const std::vector> PAGE_TYPE_bit({{1, 6}}); diff --git a/src/tests/common-files/observable_tests_flags.h b/src/tests/common-files/observable_tests_flags.h index 6e4f1a6ea..594429f36 100644 --- a/src/tests/common-files/observable_tests_flags.h +++ b/src/tests/common-files/observable_tests_flags.h @@ -35,6 +35,7 @@ #include DEFINE_double(skip_obs_transitory_s, 30.0, "Skip the initial observable outputs to avoid transitory results [s]"); +DEFINE_bool(compute_single_diffs, false, "Compute also the signel difference errors for Accumulated Carrier Phase and Carrier Doppler (requires LO synchronization between receivers)"); #endif diff --git a/src/tests/common-files/tracking_tests_flags.h b/src/tests/common-files/tracking_tests_flags.h index 914631d6e..9a5064050 100644 --- a/src/tests/common-files/tracking_tests_flags.h +++ b/src/tests/common-files/tracking_tests_flags.h @@ -48,12 +48,12 @@ DEFINE_double(CN0_dBHz_start, std::numeric_limits::infinity(), "Enable n DEFINE_double(CN0_dBHz_stop, std::numeric_limits::infinity(), "Enable noise generator and set the CN0 stop sweep value [dB-Hz]"); DEFINE_double(CN0_dB_step, 3.0, "Noise generator CN0 sweep step value [dB]"); -DEFINE_double(PLL_bw_hz_start, 40.0, "PLL Wide configuration start sweep value [Hz]"); -DEFINE_double(PLL_bw_hz_stop, 40.0, "PLL Wide configuration stop sweep value [Hz]"); +DEFINE_double(PLL_bw_hz_start, 20.0, "PLL Wide configuration start sweep value [Hz]"); +DEFINE_double(PLL_bw_hz_stop, 20.0, "PLL Wide configuration stop sweep value [Hz]"); DEFINE_double(PLL_bw_hz_step, 5.0, "PLL Wide configuration sweep step value [Hz]"); -DEFINE_double(DLL_bw_hz_start, 1.5, "DLL Wide configuration start sweep value [Hz]"); -DEFINE_double(DLL_bw_hz_stop, 1.5, "DLL Wide configuration stop sweep value [Hz]"); +DEFINE_double(DLL_bw_hz_start, 1.0, "DLL Wide configuration start sweep value [Hz]"); +DEFINE_double(DLL_bw_hz_stop, 1.0, "DLL Wide configuration stop sweep value [Hz]"); DEFINE_double(DLL_bw_hz_step, 0.25, "DLL Wide configuration sweep step value [Hz]"); DEFINE_double(PLL_narrow_bw_hz, 5.0, "PLL Narrow configuration value [Hz]"); 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 4c23d9f25..e9a853aa2 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 @@ -39,6 +39,11 @@ #include #include #include +#include +#include +#include +#include +#include #include "GPS_L1_CA.h" #include "gnss_satellite.h" #include "gnss_block_factory.h" @@ -59,6 +64,7 @@ #include "gnss_sdr_sample_counter.h" #include #include "test_flags.h" +#include "tracking_tests_flags.h" #include "observable_tests_flags.h" #include "gnuplot_i.h" @@ -178,6 +184,7 @@ public: std::string p3; std::string p4; std::string p5; + std::string implementation = FLAGS_trk_test_implementation; const int baseband_sampling_freq = FLAGS_fs_gen_sps; @@ -192,15 +199,31 @@ public: arma::vec& true_tow_s, arma::mat& measured_ch0, std::string data_title); - void check_results_carrier_doppler( + void check_results_carrier_phase_double_diff( arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_ch0_s, + arma::vec& true_tow_ch1_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1, + 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_carrier_doppler_double_diff( + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_ch0_s, + arma::vec& true_tow_ch1_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1, + std::string data_title); void check_results_code_pseudorange( arma::mat& true_ch0, arma::mat& true_ch1, - arma::vec& true_tow_s, + arma::vec& true_tow_ch0_s, + arma::vec& true_tow_ch1_s, arma::mat& measured_ch0, arma::mat& measured_ch1, std::string data_title); @@ -216,11 +239,20 @@ public: { } - void configure_receiver(); + bool ReadRinexObs(std::vector* obs_vec, Gnss_Synchro gnss); + + bool acquire_signal(); + void configure_receiver( + double PLL_wide_bw_hz, + double DLL_wide_bw_hz, + double PLL_narrow_bw_hz, + double DLL_narrow_bw_hz, + int extend_correlation_symbols); gr::top_block_sptr top_block; std::shared_ptr factory; std::shared_ptr config; + Gnss_Synchro gnss_synchro_master; std::vector gnss_synchro_vec; size_t item_size; }; @@ -269,23 +301,338 @@ int HybridObservablesTest::generate_signal() } -void HybridObservablesTest::configure_receiver() +bool HybridObservablesTest::acquire_signal() { + // 1. Setup GNU Radio flowgraph (file_source -> Acquisition_10m) + gr::top_block_sptr top_block; + top_block = gr::make_top_block("Acquisition test"); + int SV_ID = 1; //initial sv id + // Satellite signal definition + Gnss_Synchro tmp_gnss_synchro; + tmp_gnss_synchro.Channel_ID = 0; + config = std::make_shared(); + config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(baseband_sampling_freq)); + config->set_property("Acquisition.blocking_on_standby", "true"); + config->set_property("Acquisition.blocking", "true"); + config->set_property("Acquisition.dump", "false"); + config->set_property("Acquisition.dump_filename", "./data/acquisition.dat"); + config->set_property("Acquisition.use_CFAR_algorithm", "false"); + + std::shared_ptr acquisition; + + std::string System_and_Signal; + //create the correspondign acquisition block according to the desired tracking signal + if (implementation.compare("GPS_L1_CA_DLL_PLL_Tracking") == 0) + { + tmp_gnss_synchro.System = 'G'; + std::string signal = "1C"; + signal.copy(tmp_gnss_synchro.Signal, 2, 0); + tmp_gnss_synchro.PRN = SV_ID; + System_and_Signal = "GPS L1 CA"; + config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); + //acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + } + else if (implementation.compare("Galileo_E1_DLL_PLL_VEML_Tracking") == 0) + { + tmp_gnss_synchro.System = 'E'; + std::string signal = "1B"; + signal.copy(tmp_gnss_synchro.Signal, 2, 0); + tmp_gnss_synchro.PRN = SV_ID; + System_and_Signal = "Galileo E1B"; + config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); + acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + } + else if (implementation.compare("GPS_L2_M_DLL_PLL_Tracking") == 0) + { + tmp_gnss_synchro.System = 'G'; + std::string signal = "2S"; + signal.copy(tmp_gnss_synchro.Signal, 2, 0); + tmp_gnss_synchro.PRN = SV_ID; + System_and_Signal = "GPS L2CM"; + config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); + acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + } + else if (implementation.compare("Galileo_E5a_DLL_PLL_Tracking_b") == 0) + { + tmp_gnss_synchro.System = 'E'; + std::string signal = "5X"; + signal.copy(tmp_gnss_synchro.Signal, 2, 0); + tmp_gnss_synchro.PRN = SV_ID; + System_and_Signal = "Galileo E5a"; + config->set_property("Acquisition_5X.coherent_integration_time_ms", "1"); + config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); + config->set_property("Acquisition.CAF_window_hz", "0"); // **Only for E5a** Resolves doppler ambiguity averaging the specified BW in the winner code delay. If set to 0 CAF filter is desactivated. Recommended value 3000 Hz + config->set_property("Acquisition.Zero_padding", "0"); //**Only for E5a** Avoids power loss and doppler ambiguity in bit transitions by correlating one code with twice the input data length, ensuring that at least one full code is present without transitions. If set to 1 it is ON, if set to 0 it is OFF. + config->set_property("Acquisition.bit_transition_flag", "false"); + acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + } + + else if (implementation.compare("Galileo_E5a_DLL_PLL_Tracking") == 0) + { + tmp_gnss_synchro.System = 'E'; + std::string signal = "5X"; + signal.copy(tmp_gnss_synchro.Signal, 2, 0); + tmp_gnss_synchro.PRN = SV_ID; + System_and_Signal = "Galileo E5a"; + config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); + acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + } + else if (implementation.compare("GPS_L5_DLL_PLL_Tracking") == 0) + { + tmp_gnss_synchro.System = 'G'; + std::string signal = "L5"; + signal.copy(tmp_gnss_synchro.Signal, 2, 0); + tmp_gnss_synchro.PRN = SV_ID; + System_and_Signal = "GPS L5I"; + config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); + acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + } + else + { + std::cout << "The test can not run with the selected tracking implementation\n "; + throw(std::exception()); + } + + acquisition->set_gnss_synchro(&tmp_gnss_synchro); + acquisition->set_channel(0); + acquisition->set_doppler_max(config->property("Acquisition.doppler_max", FLAGS_external_signal_acquisition_doppler_max_hz)); + acquisition->set_doppler_step(config->property("Acquisition.doppler_step", FLAGS_external_signal_acquisition_doppler_step_hz)); + acquisition->set_threshold(config->property("Acquisition.threshold", FLAGS_external_signal_acquisition_threshold)); + acquisition->init(); + acquisition->set_local_code(); + acquisition->set_state(1); // Ensure that acquisition starts at the first sample + acquisition->connect(top_block); + + gr::blocks::file_source::sptr file_source; + std::string file = FLAGS_signal_file; + const char* file_name = file.c_str(); + file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false); + file_source->seek(2 * FLAGS_skip_samples, 0); //skip head. ibyte, two bytes per complex sample + gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); + //gr::blocks::head::sptr head_samples = gr::blocks::head::make(sizeof(gr_complex), baseband_sampling_freq * FLAGS_duration); + + top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); + top_block->connect(gr_interleaved_char_to_complex, 0, acquisition->get_left_block(), 0); + //top_block->connect(head_samples, 0, acquisition->get_left_block(), 0); + + boost::shared_ptr msg_rx; + try + { + msg_rx = Acquisition_msg_rx_make(); + } + catch (const std::exception& e) + { + std::cout << "Failure connecting the message port system: " << e.what() << std::endl; + exit(0); + } + + msg_rx->top_block = top_block; + top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + + // 5. Run the flowgraph + // Get visible GPS satellites (positive acquisitions with Doppler measurements) + // record startup time + std::chrono::time_point start, end; + std::chrono::duration elapsed_seconds; + start = std::chrono::system_clock::now(); + + bool start_msg = true; + + unsigned int MAX_PRN_IDX = 0; + + switch (tmp_gnss_synchro.System) + { + case 'G': + MAX_PRN_IDX = 33; + break; + case 'E': + MAX_PRN_IDX = 37; + break; + default: + MAX_PRN_IDX = 33; + } + + for (unsigned int PRN = 1; PRN < MAX_PRN_IDX; PRN++) + { + tmp_gnss_synchro.PRN = PRN; + acquisition->set_gnss_synchro(&tmp_gnss_synchro); + acquisition->init(); + acquisition->set_local_code(); + acquisition->reset(); + acquisition->set_state(1); + msg_rx->rx_message = 0; + top_block->run(); + if (start_msg == true) + { + std::cout << "Reading external signal file: " << FLAGS_signal_file << std::endl; + std::cout << "Searching for " << System_and_Signal << " Satellites..." << std::endl; + std::cout << "["; + start_msg = false; + } + while (msg_rx->rx_message == 0) + { + usleep(100000); + } + if (msg_rx->rx_message == 1) + { + std::cout << " " << PRN << " "; + gnss_synchro_vec.push_back(tmp_gnss_synchro); + } + else + { + std::cout << " . "; + } + top_block->stop(); + file_source->seek(2 * FLAGS_skip_samples, 0); //skip head. ibyte, two bytes per complex sample + std::cout.flush(); + } + std::cout << "]" << std::endl; + std::cout << "-------------------------------------------\n"; + + for (auto& x : gnss_synchro_vec) + { + std::cout << "DETECTED SATELLITE " << System_and_Signal + << " PRN: " << x.PRN + << " with Doppler: " << x.Acq_doppler_hz + << " [Hz], code phase: " << x.Acq_delay_samples + << " [samples] at signal SampleStamp " << x.Acq_samplestamp_samples << "\n"; + } + + // report the elapsed time + end = std::chrono::system_clock::now(); + elapsed_seconds = end - start; + std::cout << "Total signal acquisition run time " + << elapsed_seconds.count() + << " [seconds]" << std::endl; + if (gnss_synchro_vec.size() > 0) + { + return true; + } + else + { + return false; + } +} + + +void HybridObservablesTest::configure_receiver( + double PLL_wide_bw_hz, + double DLL_wide_bw_hz, + double PLL_narrow_bw_hz, + double DLL_narrow_bw_hz, + int extend_correlation_symbols) +{ + config = std::make_shared(); + config->set_property("Tracking.dump", "true"); + config->set_property("Tracking.dump_filename", "./tracking_ch_"); + config->set_property("Tracking.implementation", implementation); + config->set_property("Tracking.item_type", "gr_complex"); + config->set_property("Tracking.pll_bw_hz", std::to_string(PLL_wide_bw_hz)); + config->set_property("Tracking.dll_bw_hz", std::to_string(DLL_wide_bw_hz)); + config->set_property("Tracking.extend_correlation_symbols", std::to_string(extend_correlation_symbols)); + config->set_property("Tracking.pll_bw_narrow_hz", std::to_string(PLL_narrow_bw_hz)); + config->set_property("Tracking.dll_bw_narrow_hz", std::to_string(DLL_narrow_bw_hz)); + config->set_property("Observables.implementation", "Hybrid_Observables"); + config->set_property("Observables.dump", "true"); + config->set_property("TelemetryDecoder.dump", "true"); + + gnss_synchro_master.Channel_ID = 0; config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(baseband_sampling_freq)); - // Set Tracking - config->set_property("Tracking_1C.item_type", "gr_complex"); - config->set_property("Tracking_1C.dump", "true"); - config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); - config->set_property("Tracking_1C.pll_bw_hz", "5.0"); - 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", "1"); - config->set_property("Tracking_1C.early_late_space_chips", "0.5"); + std::string System_and_Signal; + if (implementation.compare("GPS_L1_CA_DLL_PLL_Tracking") == 0) + { + gnss_synchro_master.System = 'G'; + std::string signal = "1C"; + System_and_Signal = "GPS L1 CA"; + const char* str = signal.c_str(); // get a C style null terminated string + std::memcpy(static_cast(gnss_synchro_master.Signal), str, 3); // copy string into synchro char array: 2 char + null - config->set_property("TelemetryDecoder_1C.dump", "true"); - config->set_property("Observables.dump", "true"); + config->set_property("Tracking.early_late_space_chips", "0.5"); + config->set_property("Tracking.early_late_space_narrow_chips", "0.5"); + + config->set_property("TelemetryDecoder.implementation", "GPS_L1_CA_Telemetry_Decoder"); + } + else if (implementation.compare("Galileo_E1_DLL_PLL_VEML_Tracking") == 0) + { + gnss_synchro_master.System = 'E'; + std::string signal = "1B"; + System_and_Signal = "Galileo E1B"; + const char* str = signal.c_str(); // get a C style null terminated string + std::memcpy(static_cast(gnss_synchro_master.Signal), str, 3); // copy string into synchro char array: 2 char + null + + config->set_property("Tracking.early_late_space_chips", "0.15"); + config->set_property("Tracking.very_early_late_space_chips", "0.6"); + config->set_property("Tracking.early_late_space_narrow_chips", "0.15"); + config->set_property("Tracking.very_early_late_space_narrow_chips", "0.6"); + config->set_property("Tracking.track_pilot", "true"); + + config->set_property("TelemetryDecoder.implementation", "Galileo_E1B_Telemetry_Decoder"); + } + else if (implementation.compare("GPS_L2_M_DLL_PLL_Tracking") == 0) + { + gnss_synchro_master.System = 'G'; + std::string signal = "2S"; + System_and_Signal = "GPS L2CM"; + const char* str = signal.c_str(); // get a C style null terminated string + std::memcpy(static_cast(gnss_synchro_master.Signal), str, 3); // copy string into synchro char array: 2 char + null + + config->set_property("Tracking.early_late_space_chips", "0.5"); + config->set_property("Tracking.track_pilot", "false"); + + config->set_property("TelemetryDecoder.implementation", "GPS_L2C_Telemetry_Decoder"); + } + else if (implementation.compare("Galileo_E5a_DLL_PLL_Tracking") == 0 or implementation.compare("Galileo_E5a_DLL_PLL_Tracking_b") == 0) + { + gnss_synchro_master.System = 'E'; + std::string signal = "5X"; + System_and_Signal = "Galileo E5a"; + const char* str = signal.c_str(); // get a C style null terminated string + std::memcpy(static_cast(gnss_synchro_master.Signal), str, 3); // copy string into synchro char array: 2 char + null + + if (implementation.compare("Galileo_E5a_DLL_PLL_Tracking_b") == 0) + { + config->supersede_property("Tracking.implementation", std::string("Galileo_E5a_DLL_PLL_Tracking")); + } + config->set_property("Tracking.early_late_space_chips", "0.5"); + config->set_property("Tracking.track_pilot", "false"); + config->set_property("Tracking.order", "2"); + + config->set_property("TelemetryDecoder.implementation", "Galileo_E5a_Telemetry_Decoder"); + } + else if (implementation.compare("GPS_L5_DLL_PLL_Tracking") == 0) + { + gnss_synchro_master.System = 'G'; + std::string signal = "L5"; + System_and_Signal = "GPS L5I"; + const char* str = signal.c_str(); // get a C style null terminated string + std::memcpy(static_cast(gnss_synchro_master.Signal), str, 3); // copy string into synchro char array: 2 char + null + + config->set_property("Tracking.early_late_space_chips", "0.5"); + config->set_property("Tracking.track_pilot", "false"); + config->set_property("Tracking.order", "2"); + + config->set_property("TelemetryDecoder.implementation", "GPS_L5_Telemetry_Decoder"); + } + else + { + std::cout << "The test can not run with the selected tracking implementation\n "; + throw(std::exception()); + } + + std::cout << "*****************************************\n"; + std::cout << "*** Tracking configuration parameters ***\n"; + std::cout << "*****************************************\n"; + std::cout << "Signal: " << System_and_Signal << "\n"; + std::cout << "implementation: " << config->property("Tracking.implementation", std::string("undefined")) << " \n"; + std::cout << "pll_bw_hz: " << config->property("Tracking.pll_bw_hz", 0.0) << " Hz\n"; + std::cout << "dll_bw_hz: " << config->property("Tracking.dll_bw_hz", 0.0) << " Hz\n"; + std::cout << "pll_bw_narrow_hz: " << config->property("Tracking.pll_bw_narrow_hz", 0.0) << " Hz\n"; + std::cout << "dll_bw_narrow_hz: " << config->property("Tracking.dll_bw_narrow_hz", 0.0) << " Hz\n"; + std::cout << "extend_correlation_symbols: " << config->property("Tracking.extend_correlation_symbols", 0) << " Symbols\n"; + std::cout << "*****************************************\n"; + std::cout << "*****************************************\n"; } void HybridObservablesTest::check_results_carrier_phase( @@ -338,36 +685,216 @@ void HybridObservablesTest::check_results_carrier_phase( 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) { + 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, + "Carrier Phase error"); + g3.set_legend(); + g3.savetops(data_title + "Carrier_phase_error"); + g3.showonscreen(); // window output } - else + + //check results against the test tolerance + ASSERT_LT(rmse_ch0, 0.25); + ASSERT_LT(error_mean_ch0, 0.2); + ASSERT_GT(error_mean_ch0, -0.2); + ASSERT_LT(error_var_ch0, 0.5); + ASSERT_LT(max_error_ch0, 0.5); + ASSERT_GT(min_error_ch0, -0.5); +} + + +void HybridObservablesTest::check_results_carrier_phase_double_diff( + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_ch0_s, + arma::vec& true_tow_ch1_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)); + 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)); + + 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_carrier_phase_interp; + arma::vec true_ch1_carrier_phase_interp; + arma::interp1(true_tow_ch0_s, true_ch0.col(3), t, true_ch0_carrier_phase_interp); + arma::interp1(true_tow_ch1_s, true_ch1.col(3), t, true_ch1_carrier_phase_interp); + + arma::vec meas_ch0_carrier_phase_interp; + arma::vec meas_ch1_carrier_phase_interp; + arma::interp1(measured_ch0.col(0), measured_ch0.col(3), t, meas_ch0_carrier_phase_interp); + arma::interp1(measured_ch1.col(0), measured_ch1.col(3), t, meas_ch1_carrier_phase_interp); + + // generate double difference accumulated carrier phases + //compute error without the accumulated carrier phase offsets (which depends on the receiver starting time) + arma::vec delta_true_carrier_phase_cycles = (true_ch0_carrier_phase_interp - true_ch0_carrier_phase_interp(0)) - (true_ch1_carrier_phase_interp - true_ch1_carrier_phase_interp(0)); + arma::vec delta_measured_carrier_phase_cycles = (meas_ch0_carrier_phase_interp - meas_ch0_carrier_phase_interp(0)) - (meas_ch1_carrier_phase_interp - meas_ch1_carrier_phase_interp(0)); + + //2. RMSE + arma::vec err; + + err = delta_measured_carrier_phase_cycles - delta_true_carrier_phase_cycles; + arma::vec err2 = arma::square(err); + double rmse = sqrt(arma::mean(err2)); + + //3. Mean err and variance + double error_mean = arma::mean(err); + double error_var = arma::var(err); + + // 4. Peaks + double max_error = arma::max(err); + double min_error = arma::min(err); + + //5. report + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << data_title << "Double diff Carrier Phase RMSE = " + << rmse << ", mean = " << error_mean + << ", stdev = " << sqrt(error_var) + << " (max,min) = " << max_error + << "," << min_error + << " [Cycles]" << std::endl; + std::cout.precision(ss); + + //plots + if (FLAGS_show_plots) { - g3.disablescreen(); + Gnuplot g3("linespoints"); + g3.set_title(data_title + "Double diff Carrier Phase error [Cycles]"); + g3.set_grid(); + g3.set_xlabel("Time [s]"); + g3.set_ylabel("Double diff Carrier Phase error [Cycles]"); + //conversion between arma::vec and std:vector + std::vector range_error_m(err.colptr(0), err.colptr(0) + err.n_rows); + g3.cmd("set key box opaque"); + g3.plot_xy(time_vector, range_error_m, + "Double diff Carrier Phase error"); + g3.set_legend(); + g3.savetops(data_title + "double_diff_carrier_phase_error"); + + g3.showonscreen(); // window output } - - 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); + //check results against the test tolerance + ASSERT_LT(rmse, 0.25); + ASSERT_LT(error_mean, 0.2); + ASSERT_GT(error_mean, -0.2); + ASSERT_LT(error_var, 0.5); + ASSERT_LT(max_error, 0.5); + ASSERT_GT(min_error, -0.5); } + +void HybridObservablesTest::check_results_carrier_doppler_double_diff( + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_ch0_s, + arma::vec& true_tow_ch1_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)); + 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)); + + 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_carrier_doppler_interp; + arma::vec true_ch1_carrier_doppler_interp; + arma::interp1(true_tow_ch0_s, true_ch0.col(2), t, true_ch0_carrier_doppler_interp); + arma::interp1(true_tow_ch1_s, true_ch1.col(2), t, true_ch1_carrier_doppler_interp); + + arma::vec meas_ch0_carrier_doppler_interp; + arma::vec meas_ch1_carrier_doppler_interp; + arma::interp1(measured_ch0.col(0), measured_ch0.col(2), t, meas_ch0_carrier_doppler_interp); + arma::interp1(measured_ch1.col(0), measured_ch1.col(2), t, meas_ch1_carrier_doppler_interp); + + // generate double difference carrier Doppler + arma::vec delta_true_carrier_doppler_cycles = true_ch0_carrier_doppler_interp - true_ch1_carrier_doppler_interp; + arma::vec delta_measured_carrier_doppler_cycles = meas_ch0_carrier_doppler_interp - meas_ch1_carrier_doppler_interp; + + //2. RMSE + arma::vec err; + + err = delta_measured_carrier_doppler_cycles - delta_true_carrier_doppler_cycles; + arma::vec err2 = arma::square(err); + double rmse = sqrt(arma::mean(err2)); + + //3. Mean err and variance + double error_mean = arma::mean(err); + double error_var = arma::var(err); + + // 4. Peaks + double max_error = arma::max(err); + double min_error = arma::min(err); + + //5. report + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << data_title << "Double diff Carrier Doppler RMSE = " + << rmse << ", mean = " << error_mean + << ", stdev = " << sqrt(error_var) + << " (max,min) = " << max_error + << "," << min_error + << " [Hz]" << std::endl; + std::cout.precision(ss); + + //plots + if (FLAGS_show_plots) + { + Gnuplot g3("linespoints"); + g3.set_title(data_title + "Double diff Carrier Doppler error [Hz]"); + g3.set_grid(); + g3.set_xlabel("Time [s]"); + g3.set_ylabel("Double diff Carrier Doppler error [Hz]"); + //conversion between arma::vec and std:vector + std::vector range_error_m(err.colptr(0), err.colptr(0) + err.n_rows); + g3.cmd("set key box opaque"); + g3.plot_xy(time_vector, range_error_m, + "Double diff Carrier Doppler error"); + g3.set_legend(); + g3.savetops(data_title + "double_diff_carrier_doppler_error"); + + g3.showonscreen(); // window output + } + + //check results against the test tolerance + ASSERT_LT(error_mean, 5); + ASSERT_GT(error_mean, -5); + //assuming PLL BW=35 + ASSERT_LT(error_var, 200); + ASSERT_LT(max_error, 70); + ASSERT_GT(min_error, -70); + ASSERT_LT(rmse, 30); +} + + void HybridObservablesTest::check_results_carrier_doppler( arma::mat& true_ch0, arma::vec& true_tow_s, @@ -418,33 +945,32 @@ void HybridObservablesTest::check_results_carrier_doppler( 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) { + 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, + "Carrier Doppler error"); + g3.set_legend(); + g3.savetops(data_title + "Carrier_doppler_error"); + g3.showonscreen(); // window output } - else - { - g3.disablescreen(); - } - ASSERT_LT(rmse_ch0, 5); + //check results against the test tolerance 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); + //assuming PLL BW=35 + ASSERT_LT(error_var_ch0, 200); + ASSERT_LT(max_error_ch0, 70); + ASSERT_GT(min_error_ch0, -70); + ASSERT_LT(rmse_ch0, 30); } bool HybridObservablesTest::save_mat_xy(std::vector& x, std::vector& y, std::string filename) @@ -485,7 +1011,8 @@ bool HybridObservablesTest::save_mat_xy(std::vector& x, std::vector range_error_m(err.colptr(0), err.colptr(0) + err.n_rows); - g3.cmd("set key box opaque"); - g3.plot_xy(time_vector, range_error_m, - "Delta pseudorrange error"); - g3.set_legend(); - g3.savetops(data_title + "Delta_pseudorrange_error"); if (FLAGS_show_plots) { + Gnuplot g3("linespoints"); + g3.set_title(data_title + "Double diff Pseudorange error [m]"); + g3.set_grid(); + g3.set_xlabel("Time [s]"); + g3.set_ylabel("Double diff Pseudorange error [m]"); + //conversion between arma::vec and std:vector + std::vector range_error_m(err.colptr(0), err.colptr(0) + err.n_rows); + g3.cmd("set key box opaque"); + g3.plot_xy(time_vector, range_error_m, + "Double diff Pseudorrange error"); + g3.set_legend(); + g3.savetops(data_title + "double_diff_pseudorrange_error"); + g3.showonscreen(); // window output } - else - { - g3.disablescreen(); - } //check results against the test tolerance - ASSERT_LT(rmse, 0.5); - ASSERT_LT(error_mean, 0.5); - ASSERT_GT(error_mean, -0.5); - ASSERT_LT(error_var, 0.5); - ASSERT_LT(max_error, 2.0); - ASSERT_GT(min_error, -2.0); + ASSERT_LT(rmse, 3.0); + ASSERT_LT(error_mean, 1.0); + ASSERT_GT(error_mean, -1.0); + ASSERT_LT(error_var, 10.0); + ASSERT_LT(max_error, 10.0); + ASSERT_GT(min_error, -10.0); } +bool HybridObservablesTest::ReadRinexObs(std::vector* obs_vec, Gnss_Synchro gnss) +{ + // Open and read reference RINEX observables file + try + { + gpstk::Rinex3ObsStream r_ref(FLAGS_filename_rinex_obs); + r_ref.exceptions(std::ios::failbit); + gpstk::Rinex3ObsData r_ref_data; + gpstk::Rinex3ObsHeader r_ref_header; + gpstk::RinexDatum dataobj; + + r_ref >> r_ref_header; + + std::vector first_row; + gpstk::SatID prn; + for (unsigned int n = 0; n < gnss_synchro_vec.size(); n++) + { + first_row.push_back(true); + obs_vec->push_back(arma::zeros(1, 4)); + } + while (r_ref >> r_ref_data) + { + for (unsigned int n = 0; n < gnss_synchro_vec.size(); n++) + { + int myprn = gnss_synchro_vec.at(n).PRN; + + switch (gnss.System) + { + case 'G': + prn = gpstk::SatID(myprn, gpstk::SatID::systemGPS); + break; + case 'E': + prn = gpstk::SatID(myprn, gpstk::SatID::systemGalileo); + break; + default: + prn = gpstk::SatID(myprn, gpstk::SatID::systemGPS); + } + + gpstk::CommonTime time = r_ref_data.time; + double sow(static_cast(time).sow); + + gpstk::Rinex3ObsData::DataMap::iterator pointer = r_ref_data.obs.find(prn); + if (pointer == r_ref_data.obs.end()) + { + // PRN not present; do nothing + } + else + { + if (first_row.at(n) == false) + { + //insert next column + obs_vec->at(n).insert_rows(obs_vec->at(n).n_rows, 1); + } + else + { + first_row.at(n) = false; + } + if (strcmp("1C\0", gnss.Signal) == 0) + { + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 0) = sow; + dataobj = r_ref_data.getObs(prn, "C1C", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 1) = dataobj.data; //C1C P1 (psudorange L1) + dataobj = r_ref_data.getObs(prn, "D1C", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 2) = dataobj.data; //D1C Carrier Doppler + dataobj = r_ref_data.getObs(prn, "L1C", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 3) = dataobj.data; //L1C Carrier Phase + } + else if (strcmp("1B\0", gnss.Signal) == 0) + { + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 0) = sow; + dataobj = r_ref_data.getObs(prn, "C1B", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 1) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "D1B", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 2) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "L1B", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 3) = dataobj.data; + } + else if (strcmp("2S\0", gnss.Signal) == 0) //L2M + { + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 0) = sow; + dataobj = r_ref_data.getObs(prn, "C2S", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 1) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "D2S", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 2) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "L2S", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 3) = dataobj.data; + } + else if (strcmp("L5\0", gnss.Signal) == 0) + { + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 0) = sow; + dataobj = r_ref_data.getObs(prn, "C5I", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 1) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "D5I", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 2) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "L5I", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 3) = dataobj.data; + } + else if (strcmp("5X\0", gnss.Signal) == 0) //Simulator gives RINEX with E5a+E5b + { + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 0) = sow; + dataobj = r_ref_data.getObs(prn, "C8I", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 1) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "D8I", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 2) = dataobj.data; + dataobj = r_ref_data.getObs(prn, "L8I", r_ref_header); + obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 3) = dataobj.data; + } + else + { + std::cout << "ReadRinexObs unknown signal requested: " << gnss.Signal << std::endl; + return false; + } + } + } + } // end while + } // End of 'try' block + catch (const gpstk::FFStreamError& e) + { + std::cout << e; + return false; + } + catch (const gpstk::Exception& e) + { + std::cout << e; + return false; + } + catch (const std::exception& e) + { + std::cout << "Exception: " << e.what(); + std::cout << "unknown error. I don't feel so well..." << std::endl; + return false; + } + std::cout << "ReadRinexObs info:" << std::endl; + for (unsigned int n = 0; n < gnss_synchro_vec.size(); n++) + { + std::cout << "SAT PRN " << gnss_synchro_vec.at(n).PRN << " RINEX epoch readed: " << obs_vec->at(n).n_rows << std::endl; + } + return true; +} TEST_F(HybridObservablesTest, ValidationOfResults) { // Configure the signal generator @@ -588,24 +1251,85 @@ TEST_F(HybridObservablesTest, ValidationOfResults) std::chrono::time_point 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, ',')) + // use generator or use an external capture file + if (FLAGS_enable_external_signal_file) { - tmp_gnss_synchro.PRN = boost::lexical_cast(token); - gnss_synchro_vec.push_back(tmp_gnss_synchro); + //create and configure an acquisition block and perform an acquisition to obtain the synchronization parameters + ASSERT_EQ(acquire_signal(), true); + } + else + { + 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 - std::vector> true_reader_vec; + configure_receiver(FLAGS_PLL_bw_hz_start, + FLAGS_DLL_bw_hz_start, + FLAGS_PLL_narrow_bw_hz, + FLAGS_DLL_narrow_bw_hz, + FLAGS_extend_correlation_symbols); + + + for (unsigned int n = 0; n < gnss_synchro_vec.size(); n++) + { + //setup the signal synchronization, simulating an acquisition + if (!FLAGS_enable_external_signal_file) + { + //based on true observables metadata (for custom sdr generator) + //open true observables log file written by the simulator or based on provided RINEX obs + std::vector> true_reader_vec; + //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; + } + else + { + //based on the signal acquisition process + std::cout << "Estimated Initial Doppler " << gnss_synchro_vec.at(n).Acq_doppler_hz + << " [Hz], estimated Initial code delay " << gnss_synchro_vec.at(n).Acq_delay_samples << " [Samples]" + << " Acquisition SampleStamp is " << gnss_synchro_vec.at(n).Acq_samplestamp_samples << std::endl; + gnss_synchro_vec.at(n).Acq_samplestamp_samples = 0; + } + } + std::vector> tracking_ch_vec; std::vector> tlm_ch_vec; @@ -614,48 +1338,33 @@ TEST_F(HybridObservablesTest, ValidationOfResults) { //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"; + //create the tracking channels and create the telemetry decoders - //restart the epoch counter - true_reader_vec.back()->restart(); + std::shared_ptr trk_ = factory->GetBlock(config, "Tracking", config->property("Tracking.implementation", std::string("undefined")), 1, 1); + tracking_ch_vec.push_back(std::dynamic_pointer_cast(trk_)); + std::shared_ptr tlm_ = factory->GetBlock(config, "TelemetryDecoder", config->property("TelemetryDecoder.implementation", std::string("undefined")), 1, 1); + tlm_ch_vec.push_back(std::dynamic_pointer_cast(tlm_)); - 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)); + + switch (gnss_synchro_master.System) + { + case 'G': + tlm_ch_vec.back()->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_vec.at(n).PRN)); + break; + case 'E': + tlm_ch_vec.back()->set_satellite(Gnss_Satellite(std::string("Galileo"), gnss_synchro_vec.at(n).PRN)); + break; + default: + tlm_ch_vec.back()->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_vec.at(n).PRN)); + } + + }) << "Failure setting gnss_synchro."; ASSERT_NO_THROW({ @@ -682,11 +1391,20 @@ TEST_F(HybridObservablesTest, ValidationOfResults) ASSERT_NO_THROW({ - std::string file = "./" + filename_raw_data; + std::string file; + if (!FLAGS_enable_external_signal_file) + { + file = "./" + filename_raw_data; + } + else + { + file = FLAGS_signal_file; + } 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(); - gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq), sizeof(gr_complex)); + int observable_interval_ms = 20; + gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq), observable_interval_ms, 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); @@ -701,6 +1419,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) //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()); + file_source->seek(2 * FLAGS_skip_samples, 0); //skip head. ibyte, two bytes per complex sample }) << "Failure connecting the blocks."; for (unsigned int n = 0; n < tracking_ch_vec.size(); n++) @@ -716,49 +1435,56 @@ TEST_F(HybridObservablesTest, ValidationOfResults) }) << "Failure running the top_block."; //check results - //load the true values - - true_observables_reader true_observables; - - ASSERT_NO_THROW({ - if (true_observables.open_obs_file(std::string("./obs_out.bin")) == false) - { - throw std::exception(); - } - }) << "Failure opening true observables file"; - - unsigned int nepoch = static_cast(true_observables.num_epochs()); - - std::cout << "True observation epochs = " << nepoch << std::endl; // Matrices for storing columnwise true GPS time, Range, Doppler and Carrier phase std::vector true_obs_vec; - true_observables.restart(); - long int epoch_counter = 0; - for (unsigned int n = 0; n < tracking_ch_vec.size(); n++) + + if (!FLAGS_enable_external_signal_file) { - true_obs_vec.push_back(arma::zeros(nepoch, 4)); - } - - ASSERT_NO_THROW({ - while (true_observables.read_binary_obs()) - { - for (unsigned int n = 0; n < true_obs_vec.size(); n++) + //load the true values + true_observables_reader true_observables; + ASSERT_NO_THROW({ + if (true_observables.open_obs_file(std::string("./obs_out.bin")) == false) { - 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]; + throw std::exception(); } - epoch_counter++; - } - }); + }) << "Failure opening true observables file"; + unsigned int nepoch = static_cast(true_observables.num_epochs()); + + std::cout << "True observation epochs = " << nepoch << std::endl; + + 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()) + { + for (unsigned int n = 0; n < true_obs_vec.size(); n++) + { + 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]; + } + epoch_counter++; + } + }); + } + else + { + ASSERT_EQ(ReadRinexObs(&true_obs_vec, gnss_synchro_master), true) + << "Failure reading RINEX file"; + } //read measured values observables_dump_reader estimated_observables(tracking_ch_vec.size()); ASSERT_NO_THROW({ @@ -768,7 +1494,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) } }) << "Failure opening dump observables file"; - nepoch = static_cast(estimated_observables.num_epochs()); + unsigned int nepoch = static_cast(estimated_observables.num_epochs()); std::cout << "Measured observations epochs = " << nepoch << std::endl; // Matrices for storing columnwise measured RX_time, TOW, Doppler, Carrier phase and Pseudorange @@ -834,49 +1560,108 @@ TEST_F(HybridObservablesTest, ValidationOfResults) //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; for (unsigned int n = 0; n < measured_obs_vec.size(); n++) { - if (measured_obs_vec.at(n)(0, 4) < min_pr) + if (epoch_counters_vec.at(n) > 10) //discard non-valid channels { - min_pr = measured_obs_vec.at(n)(0, 4); - min_pr_ch_id = n; - } - } - - 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++) - { - 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) + " "); + { + 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 { - 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 << "PRN " << gnss_synchro_vec.at(n).PRN << " has NO observations!\n"; + } + } + + arma::vec receiver_time_offset_ref_channel_s; + receiver_time_offset_ref_channel_s = true_obs_vec.at(min_pr_ch_id).col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + std::cout << "Ref channel initial Receiver time offset " << receiver_time_offset_ref_channel_s(0) * 1e3 << " [ms]" << std::endl; + + for (unsigned int n = 0; n < measured_obs_vec.size(); n++) + { + //debug save to .mat + std::vector tmp_vector_x(true_obs_vec.at(n).col(0).colptr(0), + true_obs_vec.at(n).col(0).colptr(0) + true_obs_vec.at(n).col(0).n_rows); + std::vector tmp_vector_y(true_obs_vec.at(n).col(1).colptr(0), + true_obs_vec.at(n).col(1).colptr(0) + true_obs_vec.at(n).col(1).n_rows); + save_mat_xy(tmp_vector_x, tmp_vector_y, std::string("true_pr_ch_" + std::to_string(n))); + + std::vector tmp_vector_x2(measured_obs_vec.at(n).col(0).colptr(0), + measured_obs_vec.at(n).col(0).colptr(0) + measured_obs_vec.at(n).col(0).n_rows); + std::vector tmp_vector_y2(measured_obs_vec.at(n).col(4).colptr(0), + measured_obs_vec.at(n).col(4).colptr(0) + measured_obs_vec.at(n).col(4).n_rows); + save_mat_xy(tmp_vector_x2, tmp_vector_y2, std::string("measured_pr_ch_" + std::to_string(n))); + + std::vector tmp_vector_x3(true_obs_vec.at(n).col(0).colptr(0), + true_obs_vec.at(n).col(0).colptr(0) + true_obs_vec.at(n).col(0).n_rows); + std::vector tmp_vector_y3(true_obs_vec.at(n).col(2).colptr(0), + true_obs_vec.at(n).col(2).colptr(0) + true_obs_vec.at(n).col(2).n_rows); + save_mat_xy(tmp_vector_x3, tmp_vector_y3, std::string("true_doppler_ch_" + std::to_string(n))); + + std::vector tmp_vector_x4(measured_obs_vec.at(n).col(0).colptr(0), + measured_obs_vec.at(n).col(0).colptr(0) + measured_obs_vec.at(n).col(0).n_rows); + std::vector tmp_vector_y4(measured_obs_vec.at(n).col(2).colptr(0), + measured_obs_vec.at(n).col(2).colptr(0) + measured_obs_vec.at(n).col(2).n_rows); + save_mat_xy(tmp_vector_x4, tmp_vector_y4, std::string("measured_doppler_ch_" + std::to_string(n))); + + + if (epoch_counters_vec.at(n) > 10) //discard non-valid channels + { + arma::vec true_TOW_ref_ch_s = true_obs_vec.at(min_pr_ch_id).col(0) - receiver_time_offset_ref_channel_s(0); + arma::vec true_TOW_ch_s = true_obs_vec.at(n).col(0) - receiver_time_offset_ref_channel_s(0); + //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), + true_TOW_ch_s, + true_TOW_ref_ch_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) + " "); + check_results_carrier_phase_double_diff(true_obs_vec.at(n), + true_obs_vec.at(min_pr_ch_id), + true_TOW_ch_s, + true_TOW_ref_ch_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) + " "); + + check_results_carrier_doppler_double_diff(true_obs_vec.at(n), + true_obs_vec.at(min_pr_ch_id), + true_TOW_ch_s, + true_TOW_ref_ch_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; + } + if (FLAGS_compute_single_diffs) + { + check_results_carrier_phase(true_obs_vec.at(n), + true_TOW_ch_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), + true_TOW_ch_s, + measured_obs_vec.at(n), + "[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " "); + } + } + else + { + std::cout << "PRN " << gnss_synchro_vec.at(n).PRN << " has NO observations!\n"; } - 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) + " "); } std::cout << "Test completed in " << elapsed_seconds.count() << " [s]" << std::endl; } 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 2ae61e5fb..87e2b2dc3 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 @@ -411,7 +411,8 @@ bool TrackingPullInTest::acquire_signal(int SV_ID) tmp_gnss_synchro.PRN = SV_ID; System_and_Signal = "GPS L1 CA"; config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); - acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + //acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); + acquisition = std::make_shared(config.get(), "Acquisition", 1, 0); } else if (implementation.compare("Galileo_E1_DLL_PLL_VEML_Tracking") == 0) { @@ -809,6 +810,7 @@ TEST_F(TrackingPullInTest, ValidationOfResults) std::vector promptI; std::vector promptQ; std::vector CN0_dBHz; + std::vector Doppler; long int epoch_counter = 0; while (trk_dump.read_binary_obs()) { @@ -828,7 +830,7 @@ TEST_F(TrackingPullInTest, ValidationOfResults) promptI.push_back(trk_dump.prompt_I); promptQ.push_back(trk_dump.prompt_Q); CN0_dBHz.push_back(trk_dump.CN0_SNV_dB_Hz); - + Doppler.push_back(trk_dump.carrier_doppler_hz); epoch_counter++; } @@ -917,6 +919,28 @@ TEST_F(TrackingPullInTest, ValidationOfResults) g3.savetops("CN0_output"); g3.showonscreen(); // window output + + Gnuplot g4("linespoints"); + if (!FLAGS_enable_external_signal_file) + { + g4.set_title(std::to_string(generator_CN0_values.at(current_cn0_idx)) + " dB-Hz, GPS L1 C/A tracking CN0 output (PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")"); + } + else + { + g4.set_title("D_e=" + std::to_string(acq_doppler_error_hz_values.at(current_acq_doppler_error_idx)) + " [Hz] " + "T_e= " + std::to_string(acq_delay_error_chips_values.at(current_acq_doppler_error_idx).at(current_acq_code_error_idx)) + " [Chips] PLL/DLL BW: " + std::to_string(FLAGS_PLL_bw_hz_start) + "," + std::to_string(FLAGS_DLL_bw_hz_start) + " [Hz], (PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")"); + } + g4.set_grid(); + g4.set_xlabel("Time [s]"); + g4.set_ylabel("Estimated Doppler [Hz]"); + g4.cmd("set key box opaque"); + + g4.plot_xy(trk_timestamp_s, Doppler, + std::to_string(static_cast(round(generator_CN0_values.at(current_cn0_idx)))) + "[dB-Hz]", decimate); + + g4.set_legend(); + g4.savetops("Doppler"); + + g4.showonscreen(); // window output } } catch (const GnuplotException& ge)