From 1a559f238d86f649aadf6801dc35c65012e5856f Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 12 Feb 2018 17:17:39 +0100 Subject: [PATCH 01/14] Add dll_pll_veml_tracking files --- .../tracking/gnuradio_blocks/CMakeLists.txt | 1 + .../gnuradio_blocks/dll_pll_veml_tracking.cc | 1289 +++++++++++++++++ .../gnuradio_blocks/dll_pll_veml_tracking.h | 242 ++++ 3 files changed, 1532 insertions(+) create mode 100755 src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc create mode 100755 src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 6ad6bf2db..f1461950e 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -39,6 +39,7 @@ set(TRACKING_GR_BLOCKS_SOURCES glonass_l1_ca_dll_pll_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc + dll_pll_veml_tracking.cc ${OPT_TRACKING_BLOCKS} ) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc new file mode 100755 index 000000000..773ea51cb --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -0,0 +1,1289 @@ +/*! + * \file dll_pll_veml_tracking.cc + * \brief Implementation of a code DLL + carrier PLL VEML (Very Early + * Minus Late) tracking block for Galileo E1 signals + * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com + * + * Code DLL + carrier PLL according to the algorithms described in: + * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency + * Approach, Birkhauser, 2007 + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "dll_pll_veml_tracking.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "control_message_factory.h" + +#include "Galileo_E1.h" +#include "galileo_e1_signal_processing.h" +#include "Galileo_E5a.h" +#include "GPS_L1_CA.h" +#include "GPS_L2C.h" +#include "GPS_L5.h" + +using google::LogMessage; + +dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking( + double fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + float early_late_space_chips, + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot, + char system, char signal[3]) +{ + return dll_pll_veml_tracking_sptr(new dll_pll_veml_tracking( + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, system, signal)); +} + + +void dll_pll_veml_tracking::forecast (int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) { ninput_items_required[0] = static_cast(d_vector_length) * 2; } +} + + +dll_pll_veml_tracking::dll_pll_veml_tracking( + double fs_in, unsigned int vector_length, bool dump, + std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, float dll_bw_narrow_hz, + float early_late_space_chips, float very_early_late_space_chips, + float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, + int extend_correlation_symbols, bool track_pilot, char system, char signal[3]): + gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry bit synchronization message port input + this->message_port_register_in(pmt::mp("preamble_timestamp_s")); + this->message_port_register_out(pmt::mp("events")); + this->set_relative_rate(1.0 / static_cast(vector_length)); + + // initialize internal vars + d_dump = dump; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; + d_code_period = 0.0; + d_code_chip_rate = 0.0; + d_signal_carrier_freq = 0.0; + d_code_length_chips = 0; + + + + if(system == "G") + { + systemName["G"] = std::string("GPS"); + sys = "G"; + if(signal == "1C") + { + d_signal_carrier_freq = GPS_L1_FREQ_HZ; + d_code_period = GPS_L1_CA_CODE_PERIOD; + d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; + d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); + } + else if(signal == "2S") + { + d_signal_carrier_freq = GPS_L2_FREQ_HZ; + d_code_period = GPS_L2_M_PERIOD; + d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; + d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + } + else if(signal == "L5") + { + d_signal_carrier_freq = GPS_L5_FREQ_HZ; + d_code_period = GPS_L5i_PERIOD; + d_code_chip_rate = GPS_L5i_CODE_RATE_HZ; + d_code_length_chips = static_cast(GPS_L5i_CODE_LENGTH_CHIPS); + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks" <(Galileo_E1_B_CODE_LENGTH_CHIPS); + } + else if(signal == "5X") + { + d_signal_carrier_freq = Galileo_E5a_FREQ_HZ; + d_code_period = GALILEO_E5a_CODE_PERIOD; + d_code_chip_rate = Galileo_E5a_CODE_CHIP_RATE_HZ; + d_code_length_chips = static_cast(Galileo_E5a_CODE_LENGTH_CHIPS); + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks" <(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + + // correlator outputs (scalar) + d_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + // map memory pointers of correlator outputs + d_Very_Early = &d_correlator_outs[0]; + d_Early = &d_correlator_outs[1]; + d_Prompt = &d_correlator_outs[2]; + d_Late = &d_correlator_outs[3]; + d_Very_Late = &d_correlator_outs[4]; + + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_very_early_late_spc_chips; + d_local_code_shift_chips[1] = - d_early_late_spc_chips; + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_early_late_spc_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_chips; + + d_correlation_length_samples = d_vector_length; + multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); + + d_extend_correlation_symbols = extend_correlation_symbols; + // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal + d_track_pilot = track_pilot; + if (d_track_pilot) + { + // extended integration control + if (d_extend_correlation_symbols > 1) + { + d_enable_extended_integration = true; + } + else + { + d_enable_extended_integration = false; + } + // Extra correlator for the data component + d_local_code_data_shift_chips = static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); + d_local_code_data_shift_chips[0] = 0.0; + correlator_data_cpu.init(2 * d_correlation_length_samples, 1); + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_Prompt_Data[0] = gr_complex(0,0); + d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + } + else + { + // Disable extended integration if data component tracking is selected + d_enable_extended_integration = false; + } + + //--- Initializations ------------------------------ + // Initial code frequency basis of NCO + d_code_freq_chips = static_cast(d_code_chip_rate); + // Residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // Residual carrier phase + d_rem_carr_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0; + //d_sample_counter_seconds = 0; + d_acq_sample_stamp = 0; + + d_current_prn_length_samples = static_cast(d_vector_length); + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1; + d_CN0_SNV_dB_Hz = 0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; + + clear_tracking_vars(); + + d_acquisition_gnss_synchro = 0; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_doppler_hz = 0.0; + d_acc_carrier_phase_rad = 0.0; + + d_extend_correlation_symbols_count = 0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_K_blk_samples = 0.0; + d_code_phase_samples = 0.0; + + d_state = 0; // initial state: standby +} + + +void dll_pll_veml_tracking::start_tracking() +{ + /* + * correct the code phase according to the delay between acq and trk + */ + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + + long int acq_trk_diff_samples; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + // Doppler effect + // Fd=(C/(C+Vr))*F + double radial_velocity = (Galileo_E1_FREQ_HZ + d_acq_carrier_doppler_hz) / Galileo_E1_FREQ_HZ; + // new chip and prn sequence periods based on acq Doppler + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; + d_code_freq_chips = radial_velocity * Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + T_chip_mod_seconds = 1/d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + d_current_prn_length_samples = round(T_prn_mod_samples); + + double T_prn_true_seconds = Galileo_E1_B_CODE_LENGTH_CHIPS / Galileo_E1_CODE_CHIP_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; + corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); + if (corrected_acq_phase_samples < 0) + { + corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + } + delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; + + d_acq_code_phase_samples = corrected_acq_phase_samples; + + d_carrier_doppler_hz = d_acq_carrier_doppler_hz; + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + + // DLL/PLL filter initialization + d_carrier_loop_filter.initialize(); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter + + if (d_track_pilot) + { + char pilot_signal[3] = "1C"; + galileo_e1_code_gen_float_sampled(d_tracking_code, + pilot_signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + galileo_e1_code_gen_float_sampled(d_data_code, + d_acquisition_gnss_synchro->Signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + d_Prompt_Data[0] = gr_complex(0,0); // clean data correlator output + correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), + d_data_code, + d_local_code_shift_chips); + } + else + { + galileo_e1_code_gen_float_sampled(d_tracking_code, + d_acquisition_gnss_synchro->Signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + } + + multicorrelator_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), d_tracking_code, d_local_code_shift_chips); + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + d_carrier_lock_fail_counter = 0; + d_rem_code_phase_samples = 0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_rad = 0.0; + + d_code_phase_samples = d_acq_code_phase_samples; + + // DEBUG OUTPUT + std::cout << "Tracking of Galileo E1 signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking pull-in + d_state = 1; + + LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; +} + + +dll_pll_veml_tracking::~dll_pll_veml_tracking() +{ + if (d_dump_file.is_open()) + { + try + { + d_dump_file.close(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } + } + if(d_dump) + { + if(d_channel == 0) + { + std::cout << "Writing .mat files ..."; + } + save_matfile(); + if(d_channel == 0) + { + std::cout << " done." << std::endl; + } + } + try + { + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_tracking_code); + if (d_track_pilot) + { + volk_gnsssdr_free(d_Prompt_Data); + volk_gnsssdr_free(d_data_code); + volk_gnsssdr_free(d_local_code_data_shift_chips); + correlator_data_cpu.free(); + } + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } +} + + +bool dll_pll_veml_tracking::acquire_secondary() +{ + //******* preamble correlation ******** + int corr_value = 0; + for (unsigned int i = 0; i < Galileo_E1_C_SECONDARY_CODE_LENGTH; i++) + { + if (d_Prompt_buffer_deque.at(i).real() < 0) // symbols clipping + { + if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + { + corr_value++; + } + else + { + corr_value--; + } + } + else + { + if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + { + corr_value--; + } + else + { + corr_value++; + } + } + } + + if (abs(corr_value) == Galileo_E1_C_SECONDARY_CODE_LENGTH) + { + return true; + } + else + { + return false; + } +} + + +bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() +{ + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; + d_cn0_estimation_counter++; + return true; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + return false; + } + else + { + return true; + } + } +} + + +// correlation requires: +// - updated remnant carrier phase in radians (rem_carr_phase_rad) +// - updated remnant code phase in samples (d_rem_code_phase_samples) +// - d_code_freq_chips +// - d_carrier_doppler_hz +void dll_pll_veml_tracking::do_correlation_step(const gr_complex* input_samples) +{ + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,input_samples); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); + + // DATA CORRELATOR (if tracking tracks the pilot signal) + if (d_track_pilot) + { + correlator_data_cpu.set_input_output_vectors(d_Prompt_Data,input_samples); + correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); + } +} + + +void dll_pll_veml_tracking::run_dll_pll(bool disable_costas_loop) +{ + // ################## PLL ########################################################## + // PLL discriminator + if (disable_costas_loop == true) + { + // Secondary code acquired. No symbols transition should be present in the signal + d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + } + else + { + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + } + + // Carrier discriminator filter + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz); + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; + // New code Doppler frequency estimation + d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); + + // ################## DLL ########################################################## + // DLL discriminator + d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] + // Code discriminator filter + d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] +} + + +void dll_pll_veml_tracking::clear_tracking_vars() +{ + *d_Very_Early = gr_complex(0,0); + *d_Early = gr_complex(0,0); + *d_Prompt = gr_complex(0,0); + *d_Late = gr_complex(0,0); + *d_Very_Late= gr_complex(0,0); + d_carr_error_hz = 0.0; + d_carr_error_filt_hz = 0.0; + d_code_error_chips = 0.0; + d_code_error_filt_chips = 0.0; + d_current_symbol = 0; +} + + +void dll_pll_veml_tracking::log_data() +{ + if(d_dump) + { + // Dump results to file + float prompt_I; + float prompt_Q; + float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; + float tmp_float; + double tmp_double; + + prompt_I = static_cast(d_P_accu.real()); + prompt_Q = static_cast(d_P_accu.imag()); + + tmp_VE = std::abs(d_VE_accu); + tmp_E = std::abs(d_E_accu); + tmp_P = std::abs(d_P_accu); + tmp_L = std::abs(d_L_accu); + tmp_VL = std::abs(d_VL_accu); + + try + { + // Dump correlators output + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = d_carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } +} + + +int dll_pll_veml_tracking::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // Block input data and block output stream pointers + const gr_complex* in = reinterpret_cast(input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + switch(d_state) + { + case 0: // standby - bypass + { + current_synchro_data.Tracking_sample_counter = d_sample_counter; + break; + } + case 1: // pull-in + { + /* + * Signal alignment (skip samples until the incoming signal is aligned with local replica) + */ + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + int samples_offset; + double acq_trk_shif_correction_samples; + int acq_to_trk_delay_samples; + acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.fs = d_fs_in; + *out[0] = current_synchro_data; + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples + consume_each(samples_offset); // shift input to perform alignment with local replica + d_state = 2; // next state is the symbol synchronization + return 0; + } + case 2: // wide tracking and symbol synchronization + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Current NCO and code generator parameters + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; + // perform a correlation step + do_correlation_step(in); + // save single correlation step variables + d_VE_accu = *d_Very_Early; + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + d_VL_accu = *d_Very_Late; + // check lock status + if (cn0_and_tracking_lock_status() == false) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + // perform DLL/PLL tracking loop computations + run_dll_pll(false); + + // ################## PLL COMMANDS ################################################# + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## DLL COMMANDS ################################################# + // Code error from DLL + double code_error_filt_secs; + code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; // [seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples + + // ########### Output the tracking results to Telemetry block ########## + if (d_track_pilot) + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + + //std::cout<<(d_Prompt->real()>0); + if (d_enable_extended_integration) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_buffer_deque.push_back(*d_Prompt); + if (d_Prompt_buffer_deque.size() == Galileo_E1_C_SECONDARY_CODE_LENGTH) + { + if (acquire_secondary() == true) + { + d_extend_correlation_symbols_count = 0; + // reset extended correlator + d_VE_accu = gr_complex(0,0); + d_E_accu = gr_complex(0,0); + d_P_accu = gr_complex(0,0); + d_L_accu = gr_complex(0,0); + d_VL_accu = gr_complex(0,0); + d_Prompt_buffer_deque.clear(); + d_current_symbol = 0; + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_very_early_late_spc_narrow_chips; + d_local_code_shift_chips[1] = - d_early_late_spc_narrow_chips; + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; + + LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + //std::cout << " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl; + //std::cout << " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; + + // UPDATE INTEGRATION TIME + double new_correlation_time_s = static_cast(d_extend_correlation_symbols) * Galileo_E1_CODE_PERIOD; + d_carrier_loop_filter.set_pdi(new_correlation_time_s); + d_code_loop_filter.set_pdi(new_correlation_time_s); + + d_state = 3; // next state is the extended correlator integrator + } + + d_Prompt_buffer_deque.pop_front(); + } + } + } + break; + } + case 3: // coherent integration (correlation time extension) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Current NCO and code generator parameters + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; + // perform a correlation step + do_correlation_step(in); + // correct the integration sign using the current symbol of the secondary code + if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') + { + d_VE_accu += *d_Very_Early; + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + d_VL_accu += *d_Very_Late; + } + else + { + d_VE_accu -= *d_Very_Early; + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + d_VL_accu -= *d_Very_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; + + // PLL/DLL not enabled, we are in the middle of a coherent integration + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + + // ################## PLL ########################################################## + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples; + d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples + + // ########### Output the tracking results to Telemetry block ########## + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count >= (d_extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + break; + } + case 4: // narrow tracking + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // perform a correlation step + do_correlation_step(in); + + // correct the integration using the current symbol + if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') + { + d_VE_accu += *d_Very_Early; + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + d_VL_accu += *d_Very_Late; + } + else + { + d_VE_accu -= *d_Very_Early; + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + d_VL_accu -= *d_Very_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; + + // check lock status + if (cn0_and_tracking_lock_status() == false) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + run_dll_pll(true); // Costas loop disabled, use four quadrant atan + + // ################## PLL ########################################################## + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## DLL ########################################################## + // Code phase accumulator + double code_error_filt_secs; + code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples + + // ########### Output the tracking results to Telemetry block ########## + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // reset extended correlator + d_VE_accu = gr_complex(0,0); + d_E_accu = gr_complex(0,0); + d_P_accu = gr_complex(0,0); + d_L_accu = gr_complex(0,0); + d_VL_accu = gr_complex(0,0); + d_state = 3; //new coherent integration (correlation time extension) cycle + } + } + } + + //assign the GNURadio block output data + // current_synchro_data.System = {'E'}; + // std::string str_aux = "1B"; + // const char * str = str_aux.c_str(); // get a C style null terminated string + // std::memcpy(static_cast(current_synchro_data.Signal), str, 3); + + current_synchro_data.fs = d_fs_in; + *out[0] = current_synchro_data; + + consume_each(d_current_prn_length_samples); // this is required for gr_block derivates + d_sample_counter += d_current_prn_length_samples; // count for the processed samples + + if (current_synchro_data.Flag_valid_symbol_output) + { + return 1; + } + else + { + return 0; + } +} + + +int dll_pll_veml_tracking::save_matfile() +{ + // READ DUMP FILE + std::ifstream::pos_type size; + int number_of_double_vars = 1; + int number_of_float_vars = 17; + int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + + sizeof(float) * number_of_float_vars + sizeof(unsigned int); + std::ifstream dump_file; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + } + catch(const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; + } + // count number of epochs and rewind + long int num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + float * abs_VE = new float [num_epoch]; + float * abs_E = new float [num_epoch]; + float * abs_P = new float [num_epoch]; + float * abs_L = new float [num_epoch]; + float * abs_VL = new float [num_epoch]; + float * Prompt_I = new float [num_epoch]; + float * Prompt_Q = new float [num_epoch]; + unsigned long int * PRN_start_sample_count = new unsigned long int [num_epoch]; + float * acc_carrier_phase_rad = new float [num_epoch]; + float * carrier_doppler_hz = new float [num_epoch]; + float * code_freq_chips = new float [num_epoch]; + float * carr_error_hz = new float [num_epoch]; + float * carr_error_filt_hz = new float [num_epoch]; + float * code_error_chips = new float [num_epoch]; + float * code_error_filt_chips = new float [num_epoch]; + float * CN0_SNV_dB_Hz = new float [num_epoch]; + float * carrier_lock_test = new float [num_epoch]; + float * aux1 = new float [num_epoch]; + double * aux2 = new double [num_epoch]; + unsigned int * PRN = new unsigned int [num_epoch]; + + try + { + if (dump_file.is_open()) + { + for(long int i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); + dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = d_dump_filename; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); + if(reinterpret_cast(matfp) != NULL) + { + size_t dims[2] = {1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 0; +} + + +void dll_pll_veml_tracking::set_channel(unsigned int channel) +{ + d_channel = channel; + LOG(INFO) << "Tracking Channel set to " << d_channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} + + +void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + d_acquisition_gnss_synchro = p_gnss_synchro; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h new file mode 100755 index 000000000..8ee493af6 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -0,0 +1,242 @@ +/*! + * \file dll_pll_veml_tracking.h + * \brief Implementation of a code DLL + carrier PLL VEML (Very Early + * Minus Late) tracking block for Galileo E1 signals + * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_DLL_PLL_VEML_TRACKING_H +#define GNSS_SDR_DLL_PLL_VEML_TRACKING_H + +#define CN0_ESTIMATION_SAMPLES 20 +#define MINIMUM_VALID_CN0 25 +#define MAXIMUM_LOCK_FAIL_COUNTER 50 +#define CARRIER_LOCK_THRESHOLD 0.85 + +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "tracking_2nd_DLL_filter.h" +#include "tracking_2nd_PLL_filter.h" +#include "cpu_multicorrelator_real_codes.h" + +class dll_pll_veml_tracking; + +typedef boost::shared_ptr dll_pll_veml_tracking_sptr; + +dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int vector_length, + bool dump, std::string dump_filename, + float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, float dll_bw_narrow_hz, + float early_late_space_chips, float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, bool track_pilot, + char system, char signal[3]); + +/*! + * \brief This class implements a code DLL + carrier PLL VEML (Very Early + * Minus Late) tracking block for Galileo E1 signals + */ +class dll_pll_veml_tracking: public gr::block +{ +public: + ~dll_pll_veml_tracking(); + + void set_channel(unsigned int channel); + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void start_tracking(); + + /*! + * \brief Code DLL + carrier PLL according to the algorithms described in: + * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, + * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, + * Birkhauser, 2007 + */ + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast (int noutput_items, gr_vector_int &ninput_items_required); +private: + friend dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int vector_length, + bool dump, std::string dump_filename, + float pll_bw_hz, float dll_bw_hz, float pll_bw_narrow_hz, + float dll_bw_narrow_hz, float early_late_space_chips, + float very_early_late_space_chips, float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, bool track_pilot, + char system, char signal[3]); + + dll_pll_veml_tracking(double fs_in, unsigned + int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + float early_late_space_chips, + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot, + char system, char signal[3]); + + bool cn0_and_tracking_lock_status(); + void do_correlation_step(const gr_complex* input_samples); + void run_dll_pll(bool disable_costas_loop); + void update_local_code(); + void update_local_carrier(); + bool acquire_secondary(); + + void clear_tracking_vars(); + + void log_data(); + + // tracking configuration vars + unsigned int d_vector_length; + bool d_dump; + + Gnss_Synchro* d_acquisition_gnss_synchro; + unsigned int d_channel; + // long d_fs_in; + double d_fs_in; + + + //Signal parameters + double d_signal_carrier_freq; + double d_code_period; + double d_code_chip_rate; + unsigned int d_code_length_chips; + + //tracking state machine + int d_state; + + //Integration period in samples + int d_correlation_length_samples; + int d_n_correlator_taps; + double d_early_late_spc_chips; + double d_very_early_late_spc_chips; + + double d_early_late_spc_narrow_chips; + double d_very_early_late_spc_narrow_chips; + + float* d_tracking_code; + float* d_data_code; + float* d_local_code_shift_chips; + gr_complex* d_correlator_outs; + cpu_multicorrelator_real_codes multicorrelator_cpu; + //todo: currently the multicorrelator does not support adding extra correlator + //with different local code, thus we need extra multicorrelator instance. + //Implement this functionality inside multicorrelator class + //as an enhancement to increase the performance + float* d_local_code_data_shift_chips; + cpu_multicorrelator_real_codes correlator_data_cpu; //for data channel + + gr_complex *d_Very_Early; + gr_complex *d_Early; + gr_complex *d_Prompt; + gr_complex *d_Late; + gr_complex *d_Very_Late; + + int d_extend_correlation_symbols; + int d_extend_correlation_symbols_count; + bool d_enable_extended_integration; + int d_current_symbol; + + gr_complex d_VE_accu; + gr_complex d_E_accu; + gr_complex d_P_accu; + gr_complex d_L_accu; + gr_complex d_VL_accu; + + bool d_track_pilot; + gr_complex *d_Prompt_Data; + + double d_code_phase_step_chips; + double d_carrier_phase_step_rad; + // remaining code phase and carrier phase between tracking loops + double d_rem_code_phase_samples; + double d_rem_carr_phase_rad; + + // PLL and DLL filter library + Tracking_2nd_DLL_filter d_code_loop_filter; + Tracking_2nd_PLL_filter d_carrier_loop_filter; + + // acquisition + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + + // tracking parameters + float d_dll_bw_hz; + float d_pll_bw_hz; + float d_dll_bw_narrow_hz; + float d_pll_bw_narrow_hz; + // tracking vars + double d_carr_error_hz; + double d_carr_error_filt_hz; + double d_code_error_chips; + double d_code_error_filt_chips; + + double d_K_blk_samples; + + double d_code_freq_chips; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_rem_code_phase_chips; + double d_code_phase_samples; + + //PRN period in samples + int d_current_prn_length_samples; + + //processing samples counters + unsigned long int d_sample_counter; + unsigned long int d_acq_sample_stamp; + + // CN0 estimation and lock detector + int d_cn0_estimation_counter; + std::deque d_Prompt_buffer_deque; + gr_complex* d_Prompt_buffer; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + int d_carrier_lock_fail_counter; + + // file dump + std::string d_dump_filename; + std::ofstream d_dump_file; + + std::map systemName; + std::string sys; + + int save_matfile(); +}; + +#endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_H From 4ff65b77964fc278da03b3853ecc3d3f4db8a6de Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 13 Feb 2018 15:20:30 +0100 Subject: [PATCH 02/14] Minor changes --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 158 +++++++++--------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 16 +- 2 files changed, 89 insertions(+), 85 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 773ea51cb..b31691f09 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -72,7 +72,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking( float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3]) + char system, char signal[3], bool veml) { return dll_pll_veml_tracking_sptr(new dll_pll_veml_tracking( fs_in, @@ -88,7 +88,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking( early_late_space_narrow_chips, very_early_late_space_narrow_chips, extend_correlation_symbols, - track_pilot, system, signal)); + track_pilot, system, signal, veml)); } @@ -105,7 +105,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( float pll_bw_narrow_hz, float dll_bw_narrow_hz, float early_late_space_chips, float very_early_late_space_chips, float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, - int extend_correlation_symbols, bool track_pilot, char system, char signal[3]): + int extend_correlation_symbols, bool track_pilot, char system, char signal[3], bool veml): gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { @@ -116,6 +116,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // initialize internal vars d_dump = dump; + d_veml = veml; d_fs_in = fs_in; d_vector_length = vector_length; d_dump_filename = dump_filename; @@ -126,25 +127,25 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( - if(system == "G") + if((system - 'G') == 0) { systemName["G"] = std::string("GPS"); sys = "G"; - if(signal == "1C") + if(std::string(signal).compare("1C") == 0) { d_signal_carrier_freq = GPS_L1_FREQ_HZ; d_code_period = GPS_L1_CA_CODE_PERIOD; d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); } - else if(signal == "2S") + else if(std::string(signal).compare("2S") == 0) { d_signal_carrier_freq = GPS_L2_FREQ_HZ; d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); } - else if(signal == "L5") + else if(std::string(signal).compare("L5") == 0) { d_signal_carrier_freq = GPS_L5_FREQ_HZ; d_code_period = GPS_L5i_PERIOD; @@ -157,18 +158,18 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( std::cout << "Invalid Signal argument when instantiating tracking blocks" <(Galileo_E1_B_CODE_LENGTH_CHIPS); } - else if(signal == "5X") + else if(std::string(signal).compare("5X") == 0) { d_signal_carrier_freq = Galileo_E5a_FREQ_HZ; d_code_period = GALILEO_E5a_CODE_PERIOD; @@ -214,26 +215,42 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_tracking_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); // correlator outputs (scalar) - d_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late + if(d_veml) { d_n_correlator_taps = 5; } // Very-Early, Early, Prompt, Late, Very-Late + else { d_n_correlator_taps = 3; } + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + for (int n = 0; n < d_n_correlator_taps; n++) { d_correlator_outs[n] = gr_complex(0,0); } // map memory pointers of correlator outputs - d_Very_Early = &d_correlator_outs[0]; - d_Early = &d_correlator_outs[1]; - d_Prompt = &d_correlator_outs[2]; - d_Late = &d_correlator_outs[3]; - d_Very_Late = &d_correlator_outs[4]; + if(d_veml) + { + d_Very_Early = &d_correlator_outs[0]; + d_Early = &d_correlator_outs[1]; + d_Prompt = &d_correlator_outs[2]; + d_Late = &d_correlator_outs[3]; + d_Very_Late = &d_correlator_outs[4]; + d_local_code_shift_chips[0] = - d_very_early_late_spc_chips; + d_local_code_shift_chips[1] = - d_early_late_spc_chips; + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_early_late_spc_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_chips; - d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = - d_very_early_late_spc_chips; - d_local_code_shift_chips[1] = - d_early_late_spc_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_chips; + } + else + { + d_Very_Early = nullptr; + d_Early = &d_correlator_outs[0]; + d_Prompt = &d_correlator_outs[1]; + d_Late = &d_correlator_outs[2]; + d_Very_Late = nullptr; + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + } d_correlation_length_samples = d_vector_length; multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); @@ -268,7 +285,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( //--- Initializations ------------------------------ // Initial code frequency basis of NCO - d_code_freq_chips = static_cast(d_code_chip_rate); + d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) d_rem_code_phase_samples = 0.0; // Residual carrier phase @@ -283,15 +300,15 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; - d_carrier_lock_test = 1; - d_CN0_SNV_dB_Hz = 0; + d_Prompt_buffer = new gr_complex[DLL_PLL_CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; + d_carrier_lock_threshold = DLL_PLL_CARRIER_LOCK_THRESHOLD; clear_tracking_vars(); - d_acquisition_gnss_synchro = 0; + d_acquisition_gnss_synchro = nullptr; d_channel = 0; d_acq_code_phase_samples = 0.0; d_acq_carrier_doppler_hz = 0.0; @@ -311,6 +328,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( void dll_pll_veml_tracking::start_tracking() { + gr::thread::scoped_lock l(d_setlock); /* * correct the code phase according to the delay between acq and trk */ @@ -318,42 +336,36 @@ void dll_pll_veml_tracking::start_tracking() d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; - long int acq_trk_diff_samples; - double acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + long int acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; - acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_fs_in; // Doppler effect // Fd=(C/(C+Vr))*F - double radial_velocity = (Galileo_E1_FREQ_HZ + d_acq_carrier_doppler_hz) / Galileo_E1_FREQ_HZ; + double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; // new chip and prn sequence periods based on acq Doppler - double T_chip_mod_seconds; - double T_prn_mod_seconds; - double T_prn_mod_samples; - d_code_freq_chips = radial_velocity * Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_freq_chips = radial_velocity * d_code_chip_rate; d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); - T_chip_mod_seconds = 1/d_code_freq_chips; - T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + double T_chip_mod_seconds = 1.0 / d_code_freq_chips; + double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); + double T_prn_mod_samples = T_prn_mod_seconds * d_fs_in; d_current_prn_length_samples = round(T_prn_mod_samples); - double T_prn_true_seconds = Galileo_E1_B_CODE_LENGTH_CHIPS / Galileo_E1_CODE_CHIP_RATE_HZ; - double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_true_seconds = static_cast(d_code_length_chips) / d_code_chip_rate; + double T_prn_true_samples = T_prn_true_seconds * d_fs_in; double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; - double corrected_acq_phase_samples, delay_correction_samples; - corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); - if (corrected_acq_phase_samples < 0) + double corrected_acq_phase_samples = fmod(d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in, T_prn_true_samples); + if (corrected_acq_phase_samples < 0.0) { corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; } - delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; + double delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; d_acq_code_phase_samples = corrected_acq_phase_samples; d_carrier_doppler_hz = d_acq_carrier_doppler_hz; - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / d_fs_in; // DLL/PLL filter initialization d_carrier_loop_filter.initialize(); // initialize the carrier filter @@ -362,41 +374,27 @@ void dll_pll_veml_tracking::start_tracking() if (d_track_pilot) { char pilot_signal[3] = "1C"; - galileo_e1_code_gen_float_sampled(d_tracking_code, - pilot_signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - galileo_e1_code_gen_float_sampled(d_data_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - d_Prompt_Data[0] = gr_complex(0,0); // clean data correlator output - correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), - d_data_code, - d_local_code_shift_chips); + galileo_e1_code_gen_float_sampled(d_tracking_code, pilot_signal, false, + d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); + galileo_e1_code_gen_float_sampled(d_data_code, d_acquisition_gnss_synchro->Signal, false, + d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); + d_Prompt_Data[0] = gr_complex(0,0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_local_code_shift_chips); } else { - galileo_e1_code_gen_float_sampled(d_tracking_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); + galileo_e1_code_gen_float_sampled(d_tracking_code, d_acquisition_gnss_synchro->Signal, false, + d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); } - multicorrelator_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), d_tracking_code, d_local_code_shift_chips); + multicorrelator_cpu.set_local_code_and_taps(d_code_length_chips, d_tracking_code, d_local_code_shift_chips); for (int n = 0; n < d_n_correlator_taps; n++) { d_correlator_outs[n] = gr_complex(0,0); } d_carrier_lock_fail_counter = 0; - d_rem_code_phase_samples = 0; + d_rem_code_phase_samples = 0.0; d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; d_acc_carrier_phase_rad = 0.0; @@ -507,7 +505,7 @@ bool dll_pll_veml_tracking::acquire_secondary() bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + if (d_cn0_estimation_counter < DLL_PLL_CN0_ESTIMATION_SAMPLES) { // fill buffer with prompt correlator output values d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; @@ -518,11 +516,11 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES); // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < DLL_PLL_MINIMUM_VALID_CN0) { d_carrier_lock_fail_counter++; } @@ -530,7 +528,7 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; } - if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + if (d_carrier_lock_fail_counter > DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER) { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; @@ -698,6 +696,8 @@ void dll_pll_veml_tracking::log_data() int dll_pll_veml_tracking::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { + gr::thread::scoped_lock l(d_setlock); + // Block input data and block output stream pointers const gr_complex* in = reinterpret_cast(input_items[0]); Gnss_Synchro **out = reinterpret_cast(&output_items[0]); @@ -1259,6 +1259,8 @@ int dll_pll_veml_tracking::save_matfile() void dll_pll_veml_tracking::set_channel(unsigned int channel) { + gr::thread::scoped_lock l(d_setlock); + d_channel = channel; LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# @@ -1285,5 +1287,7 @@ void dll_pll_veml_tracking::set_channel(unsigned int channel) void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { + gr::thread::scoped_lock l(d_setlock); + d_acquisition_gnss_synchro = p_gnss_synchro; } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 8ee493af6..a1a7fbfd3 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -32,10 +32,10 @@ #ifndef GNSS_SDR_DLL_PLL_VEML_TRACKING_H #define GNSS_SDR_DLL_PLL_VEML_TRACKING_H -#define CN0_ESTIMATION_SAMPLES 20 -#define MINIMUM_VALID_CN0 25 -#define MAXIMUM_LOCK_FAIL_COUNTER 50 -#define CARRIER_LOCK_THRESHOLD 0.85 +#define DLL_PLL_CN0_ESTIMATION_SAMPLES 20 +#define DLL_PLL_MINIMUM_VALID_CN0 25 +#define DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER 50 +#define DLL_PLL_CARRIER_LOCK_THRESHOLD 0.85 #include #include @@ -58,7 +58,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3]); + char system, char signal[3], bool veml); /*! * \brief This class implements a code DLL + carrier PLL VEML (Very Early @@ -91,7 +91,7 @@ private: float very_early_late_space_chips, float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3]); + char system, char signal[3], bool veml); dll_pll_veml_tracking(double fs_in, unsigned int vector_length, @@ -107,7 +107,7 @@ private: float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3]); + char system, char signal[3], bool veml); bool cn0_and_tracking_lock_status(); void do_correlation_step(const gr_complex* input_samples); @@ -123,7 +123,7 @@ private: // tracking configuration vars unsigned int d_vector_length; bool d_dump; - + bool d_veml; Gnss_Synchro* d_acquisition_gnss_synchro; unsigned int d_channel; // long d_fs_in; From 74e8af01f9d340b56b178528363c39e8dd9fdf8a Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 12 Mar 2018 15:16:39 +0100 Subject: [PATCH 03/14] Minor changes --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 1262 +++++++++-------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 106 +- .../libs/cpu_multicorrelator_real_codes.cc | 2 +- .../tracking/libs/lock_detectors.cc | 22 +- .../tracking/libs/tracking_2nd_DLL_filter.cc | 9 +- .../tracking/libs/tracking_2nd_DLL_filter.h | 14 +- .../tracking/libs/tracking_2nd_PLL_filter.cc | 8 +- .../tracking/libs/tracking_2nd_PLL_filter.h | 14 +- .../tracking/libs/tracking_discriminators.cc | 6 +- .../tracking/libs/tracking_loop_filter.cc | 73 +- .../tracking/libs/tracking_loop_filter.h | 2 + 11 files changed, 777 insertions(+), 741 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index b31691f09..2b0a0072d 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -37,8 +37,8 @@ #include "dll_pll_veml_tracking.h" #include #include -#include #include +#include #include #include #include @@ -58,56 +58,58 @@ using google::LogMessage; dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking( - double fs_in, - unsigned int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float pll_bw_narrow_hz, - float dll_bw_narrow_hz, - float early_late_space_chips, - float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, - bool track_pilot, - char system, char signal[3], bool veml) + double fs_in, + unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + float early_late_space_chips, + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot, + char system, char signal[3], bool veml) { return dll_pll_veml_tracking_sptr(new dll_pll_veml_tracking( - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - early_late_space_chips, - very_early_late_space_chips, - early_late_space_narrow_chips, - very_early_late_space_narrow_chips, - extend_correlation_symbols, - track_pilot, system, signal, veml)); + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, system, signal, veml)); } -void dll_pll_veml_tracking::forecast (int noutput_items, - gr_vector_int &ninput_items_required) +void dll_pll_veml_tracking::forecast(int noutput_items, + gr_vector_int &ninput_items_required) { - if (noutput_items != 0) { ninput_items_required[0] = static_cast(d_vector_length) * 2; } + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; + } } dll_pll_veml_tracking::dll_pll_veml_tracking( - double fs_in, unsigned int vector_length, bool dump, - std::string dump_filename, float pll_bw_hz, float dll_bw_hz, - float pll_bw_narrow_hz, float dll_bw_narrow_hz, - float early_late_space_chips, float very_early_late_space_chips, - float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, - int extend_correlation_symbols, bool track_pilot, char system, char signal[3], bool veml): - gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + double fs_in, unsigned int vector_length, bool dump, + std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, float dll_bw_narrow_hz, + float early_late_space_chips, float very_early_late_space_chips, + float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, + int extend_correlation_symbols, bool track_pilot, char system, char signal[3], bool veml) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -117,6 +119,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // initialize internal vars d_dump = dump; d_veml = veml; + d_track_pilot = track_pilot; d_fs_in = fs_in; d_vector_length = vector_length; d_dump_filename = dump_filename; @@ -124,70 +127,99 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_code_chip_rate = 0.0; d_signal_carrier_freq = 0.0; d_code_length_chips = 0; - - - - if((system - 'G') == 0) - { - systemName["G"] = std::string("GPS"); - sys = "G"; - if(std::string(signal).compare("1C") == 0) + d_secondary = false; + d_secondary_code_length = 0; + d_secondary_code_string = nullptr; + d_correlation_length_ms = 0; + signal_type = std::string(signal); + if (system == 'G') { - d_signal_carrier_freq = GPS_L1_FREQ_HZ; - d_code_period = GPS_L1_CA_CODE_PERIOD; - d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; - d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); + systemName = "GPS"; + if (signal_type.compare("1C") == 0) + { + d_signal_carrier_freq = GPS_L1_FREQ_HZ; + d_code_period = GPS_L1_CA_CODE_PERIOD; + d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; + d_correlation_length_ms = 1; + d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); + d_secondary = false; + d_track_pilot = false; + } + else if (signal_type.compare("2S") == 0) + { + d_signal_carrier_freq = GPS_L2_FREQ_HZ; + d_code_period = GPS_L2_M_PERIOD; + d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; + d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + d_correlation_length_ms = 20; + d_secondary = false; + d_track_pilot = false; + } + else if (signal_type.compare("L5") == 0) + { + d_signal_carrier_freq = GPS_L5_FREQ_HZ; + d_code_period = GPS_L5i_PERIOD; + d_code_chip_rate = GPS_L5i_CODE_RATE_HZ; + d_correlation_length_ms = 1; + d_code_length_chips = static_cast(GPS_L5i_CODE_LENGTH_CHIPS); + d_secondary = false; + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + } } - else if(std::string(signal).compare("2S") == 0) + else if (system == 'E') { - d_signal_carrier_freq = GPS_L2_FREQ_HZ; - d_code_period = GPS_L2_M_PERIOD; - d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; - d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + systemName = "Galileo"; + if (signal_type.compare("1B") == 0) + { + d_signal_carrier_freq = Galileo_E1_FREQ_HZ; + d_code_period = Galileo_E1_CODE_PERIOD; + d_code_chip_rate = Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_length_chips = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + d_correlation_length_ms = 4; + d_secondary = true; + if (d_track_pilot) + { + d_secondary_code_length = static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E1_C_SECONDARY_CODE); + } + else + { + d_secondary = false; + } + } + else if (signal_type.compare("5X") == 0) + { + d_signal_carrier_freq = Galileo_E5a_FREQ_HZ; + d_code_period = GALILEO_E5a_CODE_PERIOD; + d_code_chip_rate = Galileo_E5a_CODE_CHIP_RATE_HZ; + d_correlation_length_ms = 1; + d_code_length_chips = static_cast(Galileo_E5a_CODE_LENGTH_CHIPS); + d_secondary = true; + if (d_track_pilot) + { + d_secondary_code_length = static_cast(Galileo_E5a_Q_SECONDARY_CODE_LENGTH); + } + else + { + d_secondary_code_length = static_cast(Galileo_E5a_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E5a_I_SECONDARY_CODE); + } + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + } } - else if(std::string(signal).compare("L5") == 0) - { - d_signal_carrier_freq = GPS_L5_FREQ_HZ; - d_code_period = GPS_L5i_PERIOD; - d_code_chip_rate = GPS_L5i_CODE_RATE_HZ; - d_code_length_chips = static_cast(GPS_L5i_CODE_LENGTH_CHIPS); - } - else - { - LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; - std::cout << "Invalid Signal argument when instantiating tracking blocks" <(Galileo_E1_B_CODE_LENGTH_CHIPS); - } - else if(std::string(signal).compare("5X") == 0) - { - d_signal_carrier_freq = Galileo_E5a_FREQ_HZ; - d_code_period = GALILEO_E5a_CODE_PERIOD; - d_code_chip_rate = Galileo_E5a_CODE_CHIP_RATE_HZ; - d_code_length_chips = static_cast(Galileo_E5a_CODE_LENGTH_CHIPS); - } - else - { - LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; - std::cout << "Invalid Signal argument when instantiating tracking blocks" <(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + d_tracking_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); // correlator outputs (scalar) - if(d_veml) { d_n_correlator_taps = 5; } // Very-Early, Early, Prompt, Late, Very-Late - else { d_n_correlator_taps = 3; } - - d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); - - for (int n = 0; n < d_n_correlator_taps; n++) + if (d_veml) { - d_correlator_outs[n] = gr_complex(0,0); + // Very-Early, Early, Prompt, Late, Very-Late + d_n_correlator_taps = 5; } - // map memory pointers of correlator outputs - if(d_veml) - { - d_Very_Early = &d_correlator_outs[0]; - d_Early = &d_correlator_outs[1]; - d_Prompt = &d_correlator_outs[2]; - d_Late = &d_correlator_outs[3]; - d_Very_Late = &d_correlator_outs[4]; - d_local_code_shift_chips[0] = - d_very_early_late_spc_chips; - d_local_code_shift_chips[1] = - d_early_late_spc_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_chips; - - } else - { - d_Very_Early = nullptr; - d_Early = &d_correlator_outs[0]; - d_Prompt = &d_correlator_outs[1]; - d_Late = &d_correlator_outs[2]; - d_Very_Late = nullptr; - d_local_code_shift_chips[0] = - d_early_late_spc_chips; - d_local_code_shift_chips[1] = 0.0; - d_local_code_shift_chips[2] = d_early_late_spc_chips; - } + { + // Early, Prompt, Late + d_n_correlator_taps = 3; + } + + d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment())); + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + + // map memory pointers of correlator outputs + if (d_veml) + { + d_Very_Early = &d_correlator_outs[0]; + d_Early = &d_correlator_outs[1]; + d_Prompt = &d_correlator_outs[2]; + d_Late = &d_correlator_outs[3]; + d_Very_Late = &d_correlator_outs[4]; + d_local_code_shift_chips[0] = -d_very_early_late_spc_chips; + d_local_code_shift_chips[1] = -d_early_late_spc_chips; + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_early_late_spc_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_chips; + d_null_shift = &d_local_code_shift_chips[2]; + } + else + { + d_Very_Early = nullptr; + d_Early = &d_correlator_outs[0]; + d_Prompt = &d_correlator_outs[1]; + d_Late = &d_correlator_outs[2]; + d_Very_Late = nullptr; + d_local_code_shift_chips[0] = -d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; + d_null_shift = &d_local_code_shift_chips[1]; + } d_correlation_length_samples = d_vector_length; multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); d_extend_correlation_symbols = extend_correlation_symbols; // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal - d_track_pilot = track_pilot; if (d_track_pilot) { // extended integration control @@ -270,12 +307,10 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_enable_extended_integration = false; } // Extra correlator for the data component - d_local_code_data_shift_chips = static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); - d_local_code_data_shift_chips[0] = 0.0; correlator_data_cpu.init(2 * d_correlation_length_samples, 1); - d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_Prompt_Data[0] = gr_complex(0,0); - d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); } else { @@ -283,7 +318,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_enable_extended_integration = false; } - //--- Initializations ------------------------------ + //--- Initializations ---// // Initial code frequency basis of NCO d_code_freq_chips = d_code_chip_rate; // Residual code phase (in chips) @@ -293,7 +328,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // sample synchronization d_sample_counter = 0; - //d_sample_counter_seconds = 0; d_acq_sample_stamp = 0; d_current_prn_length_samples = static_cast(d_vector_length); @@ -322,7 +356,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_K_blk_samples = 0.0; d_code_phase_samples = 0.0; - d_state = 0; // initial state: standby + d_state = 0; // initial state: standby } @@ -336,7 +370,7 @@ void dll_pll_veml_tracking::start_tracking() d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; - long int acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + long int acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_fs_in; // Doppler effect @@ -358,7 +392,7 @@ void dll_pll_veml_tracking::start_tracking() double corrected_acq_phase_samples = fmod(d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in, T_prn_true_samples); if (corrected_acq_phase_samples < 0.0) { - corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + corrected_acq_phase_samples += T_prn_mod_samples; } double delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; @@ -368,30 +402,27 @@ void dll_pll_veml_tracking::start_tracking() d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / d_fs_in; // DLL/PLL filter initialization - d_carrier_loop_filter.initialize(); // initialize the carrier filter - d_code_loop_filter.initialize(); // initialize the code filter + d_carrier_loop_filter.initialize(); // initialize the carrier filter + d_code_loop_filter.initialize(); // initialize the code filter if (d_track_pilot) { char pilot_signal[3] = "1C"; galileo_e1_code_gen_float_sampled(d_tracking_code, pilot_signal, false, - d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); + d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); galileo_e1_code_gen_float_sampled(d_data_code, d_acquisition_gnss_synchro->Signal, false, - d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); - d_Prompt_Data[0] = gr_complex(0,0); - correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_local_code_shift_chips); + d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_null_shift); } else { galileo_e1_code_gen_float_sampled(d_tracking_code, d_acquisition_gnss_synchro->Signal, false, - d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); + d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); } multicorrelator_cpu.set_local_code_and_taps(d_code_length_chips, d_tracking_code, d_local_code_shift_chips); - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); d_carrier_lock_fail_counter = 0; d_rem_code_phase_samples = 0.0; @@ -402,8 +433,8 @@ void dll_pll_veml_tracking::start_tracking() d_code_phase_samples = d_acq_code_phase_samples; // DEBUG OUTPUT - std::cout << "Tracking of Galileo E1 signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + std::cout << "Tracking of " << systemName << " " << signal_type << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; // enable tracking pull-in d_state = 1; @@ -419,28 +450,28 @@ dll_pll_veml_tracking::~dll_pll_veml_tracking() if (d_dump_file.is_open()) { try - { + { d_dump_file.close(); - } - catch(const std::exception & ex) - { + } + catch (const std::exception &ex) + { LOG(WARNING) << "Exception in destructor " << ex.what(); - } + } } - if(d_dump) + if (d_dump) { - if(d_channel == 0) + if (d_channel == 0) { std::cout << "Writing .mat files ..."; } save_matfile(); - if(d_channel == 0) + if (d_channel == 0) { std::cout << " done." << std::endl; } } try - { + { volk_gnsssdr_free(d_local_code_shift_chips); volk_gnsssdr_free(d_correlator_outs); volk_gnsssdr_free(d_tracking_code); @@ -448,16 +479,15 @@ dll_pll_veml_tracking::~dll_pll_veml_tracking() { volk_gnsssdr_free(d_Prompt_Data); volk_gnsssdr_free(d_data_code); - volk_gnsssdr_free(d_local_code_data_shift_chips); correlator_data_cpu.free(); } delete[] d_Prompt_buffer; multicorrelator_cpu.free(); - } - catch(const std::exception & ex) - { + } + catch (const std::exception &ex) + { LOG(WARNING) << "Exception in destructor " << ex.what(); - } + } } @@ -465,11 +495,11 @@ bool dll_pll_veml_tracking::acquire_secondary() { //******* preamble correlation ******** int corr_value = 0; - for (unsigned int i = 0; i < Galileo_E1_C_SECONDARY_CODE_LENGTH; i++) + for (unsigned int i = 0; i < d_secondary_code_length; i++) { - if (d_Prompt_buffer_deque.at(i).real() < 0) // symbols clipping + if (d_Prompt_buffer_deque.at(i).real() < 0.0) // symbols clipping { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + if (d_secondary_code_string->at(i) == '0') { corr_value++; } @@ -480,7 +510,7 @@ bool dll_pll_veml_tracking::acquire_secondary() } else { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + if (d_secondary_code_string->at(i) == '0') { corr_value--; } @@ -491,7 +521,7 @@ bool dll_pll_veml_tracking::acquire_secondary() } } - if (abs(corr_value) == Galileo_E1_C_SECONDARY_CODE_LENGTH) + if (abs(corr_value) == d_secondary_code_length) { return true; } @@ -516,7 +546,7 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, d_fs_in, static_cast(d_code_length_chips)); // Carrier lock indicator d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES); // Loss of lock detection @@ -532,13 +562,13 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock d_carrier_lock_fail_counter = 0; return false; } else { - return true; + return true; } } } @@ -549,28 +579,28 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() // - updated remnant code phase in samples (d_rem_code_phase_samples) // - d_code_freq_chips // - d_carrier_doppler_hz -void dll_pll_veml_tracking::do_correlation_step(const gr_complex* input_samples) +void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) { // ################# CARRIER WIPEOFF AND CORRELATORS ############################## // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,input_samples); + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, input_samples); multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); // DATA CORRELATOR (if tracking tracks the pilot signal) if (d_track_pilot) { - correlator_data_cpu.set_input_output_vectors(d_Prompt_Data,input_samples); + correlator_data_cpu.set_input_output_vectors(d_Prompt_Data, input_samples); correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); } } @@ -595,23 +625,26 @@ void dll_pll_veml_tracking::run_dll_pll(bool disable_costas_loop) // New carrier Doppler frequency estimation d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; // New code Doppler frequency estimation - d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); + d_code_freq_chips = (1.0 + d_carrier_doppler_hz / d_signal_carrier_freq) * d_code_chip_rate; // ################## DLL ########################################################## // DLL discriminator - d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] + d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] // Code discriminator filter - d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] + d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] } void dll_pll_veml_tracking::clear_tracking_vars() { - *d_Very_Early = gr_complex(0,0); - *d_Early = gr_complex(0,0); - *d_Prompt = gr_complex(0,0); - *d_Late = gr_complex(0,0); - *d_Very_Late= gr_complex(0,0); + if (d_veml) + { + *d_Very_Early = gr_complex(0.0, 0.0); + *d_Very_Late = gr_complex(0.0, 0.0); + } + *d_Early = gr_complex(0.0, 0.0); + *d_Prompt = gr_complex(0.0, 0.0); + *d_Late = gr_complex(0.0, 0.0); d_carr_error_hz = 0.0; d_carr_error_filt_hz = 0.0; d_code_error_chips = 0.0; @@ -622,7 +655,7 @@ void dll_pll_veml_tracking::clear_tracking_vars() void dll_pll_veml_tracking::log_data() { - if(d_dump) + if (d_dump) { // Dump results to file float prompt_I; @@ -634,382 +667,391 @@ void dll_pll_veml_tracking::log_data() prompt_I = static_cast(d_P_accu.real()); prompt_Q = static_cast(d_P_accu.imag()); - tmp_VE = std::abs(d_VE_accu); + if (d_veml) + { + tmp_VE = std::abs(d_VE_accu); + tmp_VL = std::abs(d_VL_accu); + } + else + { + tmp_VE = 0.0; + tmp_VL = 0.0; + } tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); - tmp_VL = std::abs(d_VL_accu); try - { + { // Dump correlators output - d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase tmp_float = d_acc_carrier_phase_rad; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency tmp_float = d_carrier_doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // PLL commands tmp_float = d_carr_error_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_carr_error_filt_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // DLL commands tmp_float = d_code_error_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_code_error_filt_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test tmp_float = d_CN0_SNV_dB_Hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_carrier_lock_test; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); - } + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } catch (const std::ifstream::failure &e) - { + { LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } + } } } -int dll_pll_veml_tracking::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { gr::thread::scoped_lock l(d_setlock); - - // Block input data and block output stream pointers - const gr_complex* in = reinterpret_cast(input_items[0]); + const gr_complex *in = reinterpret_cast(input_items[0]); Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro current_synchro_data = Gnss_Synchro(); - switch(d_state) - { - case 0: // standby - bypass + switch (d_state) { - current_synchro_data.Tracking_sample_counter = d_sample_counter; - break; + case 0: // Standby - Pass Through + { + break; + } + case 1: // Pull-in + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + // Fill the acquisition data + int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + double acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + int samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + d_sample_counter += samples_offset; // count for the processed samples + consume_each(samples_offset); // shift input to perform alignment with local replica + d_state = 2; // next state is the symbol synchronization + return 0; + } + case 2: // Wide tracking and symbol synchronization + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Current NCO and code generator parameters + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / d_fs_in; + d_code_phase_step_chips = d_code_freq_chips / d_fs_in; + d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_phase_step_chips; + // Perform a correlation step + do_correlation_step(in); + // Save single correlation step variables + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + + // Check lock status + if (!cn0_and_tracking_lock_status()) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + // Perform DLL/PLL tracking loop computations + run_dll_pll(false); + + // ################## PLL COMMANDS ################################################# + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; + // remanent carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## DLL COMMANDS ################################################# + // Code error from DLL + double code_error_filt_secs = d_code_period * d_code_error_filt_chips / d_code_chip_rate; // [seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + double T_prn_samples = T_prn_seconds * d_fs_in; + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * d_fs_in; + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples + + // ########### Output the tracking results to Telemetry block ########## + if (d_track_pilot) + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + + if (d_enable_extended_integration) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_buffer_deque.push_back(*d_Prompt); + if (d_Prompt_buffer_deque.size() == d_secondary_code_length) + { + if (acquire_secondary()) + { + d_extend_correlation_symbols_count = 0; + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_Prompt_buffer_deque.clear(); + d_current_symbol = 0; + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + + // Set narrow taps delay values [chips] + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips; + d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips; + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips; + d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips; + } + LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + + // UPDATE INTEGRATION TIME + float new_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); + d_carrier_loop_filter.set_pdi(new_correlation_time_s); + d_code_loop_filter.set_pdi(new_correlation_time_s); + + d_state = 3; // next state is the extended correlator integrator + } + + d_Prompt_buffer_deque.pop_front(); + } + } + } + break; + } + case 3: // coherent integration (correlation time extension) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Current NCO and code generator parameters + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / d_fs_in; + d_code_phase_step_chips = d_code_freq_chips / d_fs_in; + d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; + // perform a correlation step + do_correlation_step(in); + // correct the integration sign using the current symbol of the secondary code + if (d_secondary_code_string->at(d_current_symbol) == '0') + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } + else + { + if (d_veml) + { + d_VE_accu -= *d_Very_Early; + d_VL_accu -= *d_Very_Late; + } + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol %= d_secondary_code_length; + + // PLL/DLL not enabled, we are in the middle of a coherent integration + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + + // ################## PLL ########################################################## + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + double T_prn_samples = T_prn_seconds * d_fs_in; + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples; + d_current_prn_length_samples = static_cast(round(K_blk_samples)); //round to a discrete samples + + // ########### Output the tracking results to Telemetry block ########## + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); //rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count >= (d_extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + break; + } + case 4: // narrow tracking + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // perform a correlation step + do_correlation_step(in); + + // correct the integration using the current symbol + if (d_secondary_code_string->at(d_current_symbol) == '0') + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } + else + { + if (d_veml) + { + d_VE_accu -= *d_Very_Early; + d_VL_accu -= *d_Very_Late; + } + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol %= d_secondary_code_length; + + // check lock status + if (!cn0_and_tracking_lock_status()) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + run_dll_pll(true); // Costas loop disabled, use four quadrant atan + + // ################## PLL ########################################################## + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## DLL ########################################################## + // Code phase accumulator + double code_error_filt_secs = d_code_period * d_code_error_filt_chips / d_code_chip_rate; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + double T_prn_samples = T_prn_seconds * d_fs_in; + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * d_fs_in; + d_current_prn_length_samples = static_cast(round(K_blk_samples)); // round to a discrete number of samples + + // ########### Output the tracking results to Telemetry block ########## + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); //rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_state = 3; //new coherent integration (correlation time extension) cycle + } + } } - case 1: // pull-in - { - /* - * Signal alignment (skip samples until the incoming signal is aligned with local replica) - */ - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples - consume_each(samples_offset); // shift input to perform alignment with local replica - d_state = 2; // next state is the symbol synchronization - return 0; - } - case 2: // wide tracking and symbol synchronization - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; - // perform a correlation step - do_correlation_step(in); - // save single correlation step variables - d_VE_accu = *d_Very_Early; - d_E_accu = *d_Early; - d_P_accu = *d_Prompt; - d_L_accu = *d_Late; - d_VL_accu = *d_Very_Late; - // check lock status - if (cn0_and_tracking_lock_status() == false) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - // perform DLL/PLL tracking loop computations - run_dll_pll(false); - - // ################## PLL COMMANDS ################################################# - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL COMMANDS ################################################# - // Code error from DLL - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; // [seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - // ########### Output the tracking results to Telemetry block ########## - if (d_track_pilot) - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - - //std::cout<<(d_Prompt->real()>0); - if (d_enable_extended_integration) - { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_buffer_deque.push_back(*d_Prompt); - if (d_Prompt_buffer_deque.size() == Galileo_E1_C_SECONDARY_CODE_LENGTH) - { - if (acquire_secondary() == true) - { - d_extend_correlation_symbols_count = 0; - // reset extended correlator - d_VE_accu = gr_complex(0,0); - d_E_accu = gr_complex(0,0); - d_P_accu = gr_complex(0,0); - d_L_accu = gr_complex(0,0); - d_VL_accu = gr_complex(0,0); - d_Prompt_buffer_deque.clear(); - d_current_symbol = 0; - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = - d_very_early_late_spc_narrow_chips; - d_local_code_shift_chips[1] = - d_early_late_spc_narrow_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; - - LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - //std::cout << " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl; - //std::cout << " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; - - // UPDATE INTEGRATION TIME - double new_correlation_time_s = static_cast(d_extend_correlation_symbols) * Galileo_E1_CODE_PERIOD; - d_carrier_loop_filter.set_pdi(new_correlation_time_s); - d_code_loop_filter.set_pdi(new_correlation_time_s); - - d_state = 3; // next state is the extended correlator integrator - } - - d_Prompt_buffer_deque.pop_front(); - } - } - } - break; - } - case 3: // coherent integration (correlation time extension) - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; - // perform a correlation step - do_correlation_step(in); - // correct the integration sign using the current symbol of the secondary code - if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu += *d_Very_Early; - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - d_VL_accu += *d_Very_Late; - } - else - { - d_VE_accu -= *d_Very_Early; - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; - d_VL_accu -= *d_Very_Late; - } - d_current_symbol++; - // secondary code roll-up - d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; - - // PLL/DLL not enabled, we are in the middle of a coherent integration - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples; - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count >= (d_extend_correlation_symbols - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 4; - } - break; - } - case 4: // narrow tracking - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // perform a correlation step - do_correlation_step(in); - - // correct the integration using the current symbol - if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu += *d_Very_Early; - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - d_VL_accu += *d_Very_Late; - } - else - { - d_VE_accu -= *d_Very_Early; - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; - d_VL_accu -= *d_Very_Late; - } - d_current_symbol++; - // secondary code roll-up - d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; - - // check lock status - if (cn0_and_tracking_lock_status() == false) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - run_dll_pll(true); // Costas loop disabled, use four quadrant atan - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL ########################################################## - // Code phase accumulator - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - // reset extended correlator - d_VE_accu = gr_complex(0,0); - d_E_accu = gr_complex(0,0); - d_P_accu = gr_complex(0,0); - d_L_accu = gr_complex(0,0); - d_VL_accu = gr_complex(0,0); - d_state = 3; //new coherent integration (correlation time extension) cycle - } - } - } //assign the GNURadio block output data // current_synchro_data.System = {'E'}; @@ -1017,14 +1059,14 @@ int dll_pll_veml_tracking::general_work (int noutput_items __attribute__((unused // const char * str = str_aux.c_str(); // get a C style null terminated string // std::memcpy(static_cast(current_synchro_data.Signal), str, 3); - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - - consume_each(d_current_prn_length_samples); // this is required for gr_block derivates - d_sample_counter += d_current_prn_length_samples; // count for the processed samples + consume_each(d_current_prn_length_samples); + d_sample_counter += d_current_prn_length_samples; if (current_synchro_data.Flag_valid_symbol_output) { + current_synchro_data.fs = static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + *out[0] = current_synchro_data; return 1; } else @@ -1041,18 +1083,18 @@ int dll_pll_veml_tracking::save_matfile() int number_of_double_vars = 1; int number_of_float_vars = 17; int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + - sizeof(float) * number_of_float_vars + sizeof(unsigned int); + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream dump_file; dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try - { + { dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); - } - catch(const std::ifstream::failure &e) - { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << std::endl; return 1; - } + } // count number of epochs and rewind long int num_epoch = 0; if (dump_file.is_open()) @@ -1065,32 +1107,32 @@ int dll_pll_veml_tracking::save_matfile() { return 1; } - float * abs_VE = new float [num_epoch]; - float * abs_E = new float [num_epoch]; - float * abs_P = new float [num_epoch]; - float * abs_L = new float [num_epoch]; - float * abs_VL = new float [num_epoch]; - float * Prompt_I = new float [num_epoch]; - float * Prompt_Q = new float [num_epoch]; - unsigned long int * PRN_start_sample_count = new unsigned long int [num_epoch]; - float * acc_carrier_phase_rad = new float [num_epoch]; - float * carrier_doppler_hz = new float [num_epoch]; - float * code_freq_chips = new float [num_epoch]; - float * carr_error_hz = new float [num_epoch]; - float * carr_error_filt_hz = new float [num_epoch]; - float * code_error_chips = new float [num_epoch]; - float * code_error_filt_chips = new float [num_epoch]; - float * CN0_SNV_dB_Hz = new float [num_epoch]; - float * carrier_lock_test = new float [num_epoch]; - float * aux1 = new float [num_epoch]; - double * aux2 = new double [num_epoch]; - unsigned int * PRN = new unsigned int [num_epoch]; + float *abs_VE = new float[num_epoch]; + float *abs_E = new float[num_epoch]; + float *abs_P = new float[num_epoch]; + float *abs_L = new float[num_epoch]; + float *abs_VL = new float[num_epoch]; + float *Prompt_I = new float[num_epoch]; + float *Prompt_Q = new float[num_epoch]; + unsigned long int *PRN_start_sample_count = new unsigned long int[num_epoch]; + float *acc_carrier_phase_rad = new float[num_epoch]; + float *carrier_doppler_hz = new float[num_epoch]; + float *code_freq_chips = new float[num_epoch]; + float *carr_error_hz = new float[num_epoch]; + float *carr_error_filt_hz = new float[num_epoch]; + float *code_error_chips = new float[num_epoch]; + float *code_error_filt_chips = new float[num_epoch]; + float *CN0_SNV_dB_Hz = new float[num_epoch]; + float *carrier_lock_test = new float[num_epoch]; + float *aux1 = new float[num_epoch]; + double *aux2 = new double[num_epoch]; + unsigned int *PRN = new unsigned int[num_epoch]; try - { + { if (dump_file.is_open()) { - for(long int i = 0; i < num_epoch; i++) + for (long int i = 0; i < num_epoch; i++) { dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); @@ -1115,10 +1157,10 @@ int dll_pll_veml_tracking::save_matfile() } } dump_file.close(); - } + } catch (const std::ifstream::failure &e) - { - std::cerr << "Problem reading dump file:" << e.what() << std::endl; + { + std::cerr << "Problem reading dump file:" << e.what() << std::endl; delete[] abs_VE; delete[] abs_E; delete[] abs_P; @@ -1140,7 +1182,7 @@ int dll_pll_veml_tracking::save_matfile() delete[] aux2; delete[] PRN; return 1; - } + } // WRITE MAT FILE mat_t *matfp; @@ -1149,87 +1191,87 @@ int dll_pll_veml_tracking::save_matfile() filename.erase(filename.length() - 4, 4); filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); - if(reinterpret_cast(matfp) != NULL) + if (reinterpret_cast(matfp) != NULL) { size_t dims[2] = {1, static_cast(num_epoch)}; matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); } Mat_Close(matfp); @@ -1260,34 +1302,32 @@ int dll_pll_veml_tracking::save_matfile() void dll_pll_veml_tracking::set_channel(unsigned int channel) { gr::thread::scoped_lock l(d_setlock); - d_channel = channel; LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) + if (d_dump) { - if (d_dump_file.is_open() == false) + if (!d_dump_file.is_open()) { try - { + { d_dump_filename.append(boost::lexical_cast(d_channel)); d_dump_filename.append(".dat"); - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } + } catch (const std::ifstream::failure &e) - { + { LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } + } } } } -void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) { gr::thread::scoped_lock l(d_setlock); - d_acquisition_gnss_synchro = p_gnss_synchro; } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index a1a7fbfd3..6538de571 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -39,7 +39,6 @@ #include #include -#include #include #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" @@ -51,26 +50,26 @@ class dll_pll_veml_tracking; typedef boost::shared_ptr dll_pll_veml_tracking_sptr; dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int vector_length, - bool dump, std::string dump_filename, - float pll_bw_hz, float dll_bw_hz, - float pll_bw_narrow_hz, float dll_bw_narrow_hz, - float early_late_space_chips, float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, bool track_pilot, - char system, char signal[3], bool veml); + bool dump, std::string dump_filename, + float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, float dll_bw_narrow_hz, + float early_late_space_chips, float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, bool track_pilot, + char system, char signal[3], bool veml); /*! * \brief This class implements a code DLL + carrier PLL VEML (Very Early * Minus Late) tracking block for Galileo E1 signals */ -class dll_pll_veml_tracking: public gr::block +class dll_pll_veml_tracking : public gr::block { public: ~dll_pll_veml_tracking(); void set_channel(unsigned int channel); - void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); + void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); void start_tracking(); /*! @@ -79,38 +78,38 @@ public: * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, * Birkhauser, 2007 */ - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + int general_work(int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast(int noutput_items, gr_vector_int &ninput_items_required); - void forecast (int noutput_items, gr_vector_int &ninput_items_required); private: friend dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int vector_length, - bool dump, std::string dump_filename, - float pll_bw_hz, float dll_bw_hz, float pll_bw_narrow_hz, - float dll_bw_narrow_hz, float early_late_space_chips, - float very_early_late_space_chips, float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, bool track_pilot, - char system, char signal[3], bool veml); + bool dump, std::string dump_filename, + float pll_bw_hz, float dll_bw_hz, float pll_bw_narrow_hz, + float dll_bw_narrow_hz, float early_late_space_chips, + float very_early_late_space_chips, float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, bool track_pilot, + char system, char signal[3], bool veml); - dll_pll_veml_tracking(double fs_in, unsigned - int vector_length, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float pll_bw_narrow_hz, - float dll_bw_narrow_hz, - float early_late_space_chips, - float very_early_late_space_chips, - float early_late_space_narrow_chips, - float very_early_late_space_narrow_chips, - int extend_correlation_symbols, - bool track_pilot, - char system, char signal[3], bool veml); + dll_pll_veml_tracking(double fs_in, unsigned int vector_length, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, + float early_late_space_chips, + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot, + char system, char signal[3], bool veml); bool cn0_and_tracking_lock_status(); - void do_correlation_step(const gr_complex* input_samples); + void do_correlation_step(const gr_complex *input_samples); void run_dll_pll(bool disable_costas_loop); void update_local_code(); void update_local_carrier(); @@ -121,10 +120,13 @@ private: void log_data(); // tracking configuration vars - unsigned int d_vector_length; bool d_dump; bool d_veml; - Gnss_Synchro* d_acquisition_gnss_synchro; + bool d_secondary; + unsigned int d_secondary_code_length; + std::string *d_secondary_code_string; + Gnss_Synchro *d_acquisition_gnss_synchro; + unsigned int d_vector_length; unsigned int d_channel; // long d_fs_in; double d_fs_in; @@ -141,24 +143,24 @@ private: //Integration period in samples int d_correlation_length_samples; + int d_correlation_length_ms; int d_n_correlator_taps; double d_early_late_spc_chips; double d_very_early_late_spc_chips; - double d_early_late_spc_narrow_chips; double d_very_early_late_spc_narrow_chips; - float* d_tracking_code; - float* d_data_code; - float* d_local_code_shift_chips; - gr_complex* d_correlator_outs; + float *d_tracking_code; + float *d_data_code; + float *d_local_code_shift_chips; + float *d_null_shift; + gr_complex *d_correlator_outs; cpu_multicorrelator_real_codes multicorrelator_cpu; - //todo: currently the multicorrelator does not support adding extra correlator + //TODO: currently the multicorrelator does not support adding extra correlator //with different local code, thus we need extra multicorrelator instance. //Implement this functionality inside multicorrelator class //as an enhancement to increase the performance - float* d_local_code_data_shift_chips; - cpu_multicorrelator_real_codes correlator_data_cpu; //for data channel + cpu_multicorrelator_real_codes correlator_data_cpu; //for data channel gr_complex *d_Very_Early; gr_complex *d_Early; @@ -166,9 +168,9 @@ private: gr_complex *d_Late; gr_complex *d_Very_Late; + bool d_enable_extended_integration; int d_extend_correlation_symbols; int d_extend_correlation_symbols_count; - bool d_enable_extended_integration; int d_current_symbol; gr_complex d_VE_accu; @@ -223,7 +225,7 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; std::deque d_Prompt_buffer_deque; - gr_complex* d_Prompt_buffer; + gr_complex *d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; @@ -233,10 +235,10 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; - std::map systemName; - std::string sys; + std::string systemName; + std::string signal_type; int save_matfile(); }; -#endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_H +#endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_H diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index 5de891064..4c7735aa2 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -122,7 +122,7 @@ bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( lv_32fc_t phase_offset_as_complex[1]; phase_offset_as_complex[0] = lv_cmake(std::cos(rem_carrier_phase_in_rad), -std::sin(rem_carrier_phase_in_rad)); // call VOLK_GNSSSDR kernel - volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0, -phase_step_rad)), phase_offset_as_complex, (const float**)d_local_codes_resampled, d_n_correlators, signal_length_samples); + volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, (const float**)d_local_codes_resampled, d_n_correlators, signal_length_samples); return true; } diff --git a/src/algorithms/tracking/libs/lock_detectors.cc b/src/algorithms/tracking/libs/lock_detectors.cc index 331391978..4457e361f 100644 --- a/src/algorithms/tracking/libs/lock_detectors.cc +++ b/src/algorithms/tracking/libs/lock_detectors.cc @@ -67,20 +67,20 @@ */ float cn0_svn_estimator(gr_complex* Prompt_buffer, int length, long fs_in, double code_length) { - double SNR = 0; - double SNR_dB_Hz = 0; - double Psig = 0; - double Ptot = 0; + double SNR = 0.0; + double SNR_dB_Hz = 0.0; + double Psig = 0.0; + double Ptot = 0.0; for (int i = 0; i < length; i++) { Psig += std::abs(static_cast(Prompt_buffer[i].real())); Ptot += static_cast(Prompt_buffer[i].imag()) * static_cast(Prompt_buffer[i].imag()) + static_cast(Prompt_buffer[i].real()) * static_cast(Prompt_buffer[i].real()); } - Psig = Psig / static_cast(length); + Psig /= static_cast(length); Psig = Psig * Psig; - Ptot = Ptot / static_cast(length); + Ptot /= static_cast(length); SNR = Psig / (Ptot - Psig); - SNR_dB_Hz = 10 * log10(SNR) + 10 * log10(static_cast(fs_in) / 2) - 10 * log10(code_length); + SNR_dB_Hz = 10.0 * log10(SNR) + 10.0 * log10(static_cast(fs_in) / 2.0) - 10.0 * log10(code_length); return static_cast(SNR_dB_Hz); } @@ -96,10 +96,10 @@ float cn0_svn_estimator(gr_complex* Prompt_buffer, int length, long fs_in, doubl */ float carrier_lock_detector(gr_complex* Prompt_buffer, int length) { - float tmp_sum_I = 0; - float tmp_sum_Q = 0; - float NBD = 0; - float NBP = 0; + float tmp_sum_I = 0.0; + float tmp_sum_Q = 0.0; + float NBD = 0.0; + float NBP = 0.0; for (int i = 0; i < length; i++) { tmp_sum_I += Prompt_buffer[i].real(); diff --git a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc index fe234322e..3c9fdb117 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc +++ b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc @@ -41,11 +41,10 @@ void Tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k) { // Solve natural frequency - float Wn; - Wn = lbw * 8 * zeta / (4 * zeta * zeta + 1); + float Wn = lbw * 8.0 * zeta / (4.0 * zeta * zeta + 1.0); // solve for t1 & t2 *tau1 = k / (Wn * Wn); - *tau2 = (2.0 * zeta) / Wn; + *tau2 = 2.0 * zeta / Wn; } @@ -67,9 +66,7 @@ void Tracking_2nd_DLL_filter::initialize() float Tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator) { - float code_nco; - code_nco = d_old_code_nco + (d_tau2_code / d_tau1_code) * (DLL_discriminator - d_old_code_error) + (DLL_discriminator + d_old_code_error) * (d_pdi_code / (2 * d_tau1_code)); - //code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(DLL_discriminator - d_old_code_error) + DLL_discriminator * (d_pdi_code/d_tau1_code); + float code_nco = d_old_code_nco + (d_tau2_code / d_tau1_code) * (DLL_discriminator - d_old_code_error) + (DLL_discriminator + d_old_code_error) * (d_pdi_code / (2.0 * d_tau1_code)); d_old_code_nco = code_nco; d_old_code_error = DLL_discriminator; //[chips] return code_nco; diff --git a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h index 57660c682..62c570a21 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h +++ b/src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h @@ -49,13 +49,13 @@ class Tracking_2nd_DLL_filter { private: // PLL filter parameters - float d_tau1_code = 0; - float d_tau2_code = 0; - float d_pdi_code = 0; - float d_dllnoisebandwidth = 0; - float d_dlldampingratio = 0; - float d_old_code_error = 0; - float d_old_code_nco = 0; + float d_tau1_code = 0.0; + float d_tau2_code = 0.0; + float d_pdi_code = 0.0; + float d_dllnoisebandwidth = 0.0; + float d_dlldampingratio = 0.0; + float d_old_code_error = 0.0; + float d_old_code_nco = 0.0; void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k); public: diff --git a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc index 90ec3331f..b455ce546 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc +++ b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc @@ -40,11 +40,10 @@ void Tracking_2nd_PLL_filter::calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k) { // Solve natural frequency - float Wn; - Wn = lbw * 8 * zeta / (4 * zeta * zeta + 1); + float Wn = lbw * 8.0 * zeta / (4.0 * zeta * zeta + 1.0); // solve for t1 & t2 *tau1 = k / (Wn * Wn); - *tau2 = (2.0 * zeta) / Wn; + *tau2 = 2.0 * zeta / Wn; } @@ -71,8 +70,7 @@ void Tracking_2nd_PLL_filter::initialize() */ float Tracking_2nd_PLL_filter::get_carrier_nco(float PLL_discriminator) { - float carr_nco; - carr_nco = d_old_carr_nco + (d_tau2_carr / d_tau1_carr) * (PLL_discriminator - d_old_carr_error) + (PLL_discriminator + d_old_carr_error) * (d_pdi_carr / (2 * d_tau1_carr)); + float carr_nco = d_old_carr_nco + (d_tau2_carr / d_tau1_carr) * (PLL_discriminator - d_old_carr_error) + (PLL_discriminator + d_old_carr_error) * (d_pdi_carr / (2.0 * d_tau1_carr)); //carr_nco = d_old_carr_nco + (d_tau2_carr/d_tau1_carr)*(PLL_discriminator - d_old_carr_error) + PLL_discriminator * (d_pdi_carr/d_tau1_carr); d_old_carr_nco = carr_nco; d_old_carr_error = PLL_discriminator; diff --git a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h index d02cc17df..833e8a2c6 100644 --- a/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h +++ b/src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h @@ -48,15 +48,15 @@ class Tracking_2nd_PLL_filter { private: // PLL filter parameters - float d_tau1_carr = 0; - float d_tau2_carr = 0; - float d_pdi_carr = 0; + float d_tau1_carr = 0.0; + float d_tau2_carr = 0.0; + float d_pdi_carr = 0.0; - float d_pllnoisebandwidth = 0; - float d_plldampingratio = 0; + float d_pllnoisebandwidth = 0.0; + float d_plldampingratio = 0.0; - float d_old_carr_error = 0; - float d_old_carr_nco = 0; + float d_old_carr_error = 0.0; + float d_old_carr_nco = 0.0; void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k); diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc index d4c9b2a48..0ab876591 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.cc +++ b/src/algorithms/tracking/libs/tracking_discriminators.cc @@ -83,7 +83,7 @@ double pll_cloop_two_quadrant_atan(gr_complex prompt_s1) } else { - return 0; + return 0.0; } } @@ -107,7 +107,7 @@ double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) } else { - return 0.5 * (P_early - P_late) / ((P_early + P_late)); + return 0.5 * (P_early - P_late) / (P_early + P_late); } } @@ -131,6 +131,6 @@ double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr } else { - return (P_early - P_late) / ((P_early + P_late)); + return (P_early - P_late) / (P_early + P_late); } } diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.cc b/src/algorithms/tracking/libs/tracking_loop_filter.cc index 2571202c3..156a60d92 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.cc +++ b/src/algorithms/tracking/libs/tracking_loop_filter.cc @@ -37,9 +37,6 @@ #include -#define MAX_LOOP_ORDER 3 -#define MAX_HISTORY_LENGTH 4 - Tracking_loop_filter::Tracking_loop_filter(float update_interval, float noise_bandwidth, int loop_order, @@ -50,8 +47,8 @@ Tracking_loop_filter::Tracking_loop_filter(float update_interval, d_noise_bandwidth(noise_bandwidth), d_update_interval(update_interval) { - d_inputs.resize(MAX_HISTORY_LENGTH, 0.0); - d_outputs.resize(MAX_HISTORY_LENGTH, 0.0); + d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); + d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); update_coefficients(); } @@ -62,8 +59,8 @@ Tracking_loop_filter::Tracking_loop_filter() d_noise_bandwidth(15.0), d_update_interval(0.001) { - d_inputs.resize(MAX_HISTORY_LENGTH, 0.0); - d_outputs.resize(MAX_HISTORY_LENGTH, 0.0); + d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); + d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0); update_coefficients(); } @@ -75,12 +72,12 @@ Tracking_loop_filter::~Tracking_loop_filter() float Tracking_loop_filter::apply(float current_input) { // Now apply the filter coefficients: - float result = 0; + float result = 0.0; // Hanlde the old outputs first: for (unsigned int ii = 0; ii < d_output_coefficients.size(); ++ii) { - result += d_output_coefficients[ii] * d_outputs[(d_current_index + ii) % MAX_HISTORY_LENGTH]; + result += d_output_coefficients[ii] * d_outputs[(d_current_index + ii) % MAX_LOOP_HISTORY_LENGTH]; } // Now update the index to handle the inputs. @@ -93,7 +90,7 @@ float Tracking_loop_filter::apply(float current_input) d_current_index--; if (d_current_index < 0) { - d_current_index += MAX_HISTORY_LENGTH; + d_current_index += MAX_LOOP_HISTORY_LENGTH; } d_inputs[d_current_index] = current_input; @@ -101,7 +98,7 @@ float Tracking_loop_filter::apply(float current_input) for (unsigned int ii = 0; ii < d_input_coefficients.size(); ++ii) { - result += d_input_coefficients[ii] * d_inputs[(d_current_index + ii) % MAX_HISTORY_LENGTH]; + result += d_input_coefficients[ii] * d_inputs[(d_current_index + ii) % MAX_LOOP_HISTORY_LENGTH]; } @@ -122,7 +119,7 @@ void Tracking_loop_filter::update_coefficients(void) float wn; float T = d_update_interval; - float zeta = 1 / std::sqrt(2); + float zeta = 1.0 / std::sqrt(2.0); // The following is based on the bilinear transform approximation of // the analog integrator. The loop format is from Kaplan & Hegarty @@ -146,7 +143,7 @@ void Tracking_loop_filter::update_coefficients(void) d_input_coefficients[1] = g1 * T / 2.0; d_output_coefficients.resize(1); - d_output_coefficients[0] = 1; + d_output_coefficients[0] = 1.0; } else { @@ -157,28 +154,28 @@ void Tracking_loop_filter::update_coefficients(void) } break; case 2: - wn = d_noise_bandwidth * (8 * zeta) / (4 * zeta * zeta + 1); + wn = d_noise_bandwidth * (8.0 * zeta) / (4.0 * zeta * zeta + 1.0); g1 = wn * wn; - g2 = wn * 2 * zeta; + g2 = wn * 2.0 * zeta; if (d_include_last_integrator) { d_input_coefficients.resize(3); - d_input_coefficients[0] = T / 2 * (g1 * T / 2 + g2); - d_input_coefficients[1] = T * T / 2 * g1; - d_input_coefficients[2] = T / 2 * (g1 * T / 2 - g2); + d_input_coefficients[0] = T / 2.0 * (g1 * T / 2.0 + g2); + d_input_coefficients[1] = T * T / 2.0 * g1; + d_input_coefficients[2] = T / 2.0 * (g1 * T / 2.0 - g2); d_output_coefficients.resize(2); - d_output_coefficients[0] = 2; - d_output_coefficients[1] = -1; + d_output_coefficients[0] = 2.0; + d_output_coefficients[1] = -1.0; } else { d_input_coefficients.resize(2); d_input_coefficients[0] = (g1 * T / 2.0 + g2); - d_input_coefficients[1] = g1 * T / 2 - g2; + d_input_coefficients[1] = g1 * T / 2.0 - g2; d_output_coefficients.resize(1); - d_output_coefficients[0] = 1; + d_output_coefficients[0] = 1.0; } break; @@ -193,27 +190,27 @@ void Tracking_loop_filter::update_coefficients(void) if (d_include_last_integrator) { d_input_coefficients.resize(4); - d_input_coefficients[0] = T / 2 * (g3 + T / 2 * (g2 + T / 2 * g1)); - d_input_coefficients[1] = T / 2 * (-g3 + T / 2 * (g2 + 3 * T / 2 * g1)); - d_input_coefficients[2] = T / 2 * (-g3 - T / 2 * (g2 - 3 * T / 2 * g1)); - d_input_coefficients[3] = T / 2 * (g3 - T / 2 * (g2 - T / 2 * g1)); + d_input_coefficients[0] = T / 2.0 * (g3 + T / 2.0 * (g2 + T / 2.0 * g1)); + d_input_coefficients[1] = T / 2.0 * (-g3 + T / 2.0 * (g2 + 3.0 * T / 2.0 * g1)); + d_input_coefficients[2] = T / 2.0 * (-g3 - T / 2.0 * (g2 - 3.0 * T / 2.0 * g1)); + d_input_coefficients[3] = T / 2.0 * (g3 - T / 2.0 * (g2 - T / 2.0 * g1)); d_output_coefficients.resize(3); - d_output_coefficients[0] = 3; - d_output_coefficients[1] = -3; - d_output_coefficients[2] = 1; + d_output_coefficients[0] = 3.0; + d_output_coefficients[1] = -3.0; + d_output_coefficients[2] = 1.0; } else { d_input_coefficients.resize(3); - d_input_coefficients[0] = g3 + T / 2 * (g2 + T / 2 * g1); - d_input_coefficients[1] = g1 * T * T / 2 - 2 * g3; - d_input_coefficients[2] = g3 + T / 2 * (-g2 + T / 2 * g1); + d_input_coefficients[0] = g3 + T / 2.0 * (g2 + T / 2.0 * g1); + d_input_coefficients[1] = g1 * T * T / 2.0 - 2.0 * g3; + d_input_coefficients[2] = g3 + T / 2.0 * (-g2 + T / 2.0 * g1); d_output_coefficients.resize(2); - d_output_coefficients[0] = 2; - d_output_coefficients[1] = -1; + d_output_coefficients[0] = 2.0; + d_output_coefficients[1] = -1.0; } break; }; @@ -254,7 +251,7 @@ bool Tracking_loop_filter::get_include_last_integrator(void) const void Tracking_loop_filter::set_order(int loop_order) { - if (loop_order < 1 || loop_order > MAX_LOOP_ORDER) + if (loop_order < 1 or loop_order > MAX_LOOP_ORDER) { LOG(ERROR) << "Ignoring attempt to set loop order to " << loop_order << ". Maximum allowed order is: " << MAX_LOOP_ORDER @@ -274,7 +271,7 @@ int Tracking_loop_filter::get_order(void) const void Tracking_loop_filter::initialize(float initial_output) { - d_inputs.assign(MAX_HISTORY_LENGTH, 0.0); - d_outputs.assign(MAX_HISTORY_LENGTH, initial_output); - d_current_index = MAX_HISTORY_LENGTH - 1; + d_inputs.assign(MAX_LOOP_HISTORY_LENGTH, 0.0); + d_outputs.assign(MAX_LOOP_HISTORY_LENGTH, initial_output); + d_current_index = MAX_LOOP_HISTORY_LENGTH - 1; } diff --git a/src/algorithms/tracking/libs/tracking_loop_filter.h b/src/algorithms/tracking/libs/tracking_loop_filter.h index 7fa486960..9a852ba68 100644 --- a/src/algorithms/tracking/libs/tracking_loop_filter.h +++ b/src/algorithms/tracking/libs/tracking_loop_filter.h @@ -33,6 +33,8 @@ #ifndef GNSS_SDR_TRACKING_LOOP_FILTER_H_ #define GNSS_SDR_TRACKING_LOOP_FILTER_H_ +#define MAX_LOOP_ORDER 3 +#define MAX_LOOP_HISTORY_LENGTH 4 #include From e4bada817660d4c4f96ec47ef449875ee2a21858 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 13 Mar 2018 11:16:30 +0100 Subject: [PATCH 04/14] Handle RTCM printer exceptions --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 786 +++++++++--------- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 14 +- 2 files changed, 407 insertions(+), 393 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index dc3afaf72..2661ae937 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -34,12 +34,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include using google::LogMessage; @@ -544,34 +546,46 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); } - - if (d_ls_pvt->gps_ephemeris_map.size() > 0) + try { - if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + if (d_ls_pvt->gps_ephemeris_map.size() > 0) { - d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + if (tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->galileo_ephemeris_map.size() > 0) + { + if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) + { + if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } } } - if (d_ls_pvt->galileo_ephemeris_map.size() > 0) + catch (const boost::exception& ex) { - if (tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } + std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << std::endl; + LOG(ERROR) << "RTCM boost exception: " << boost::diagnostic_information(ex); } - if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + catch (const std::exception& ex) { - if (tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } - } - if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) - { - if (tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time - } + std::cout << "RTCM std exception: " << ex.what() << std::endl; + LOG(ERROR) << "RTCM std exception: " << ex.what(); } } } @@ -590,10 +604,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // compute on the fly PVT solution if (flag_compute_pvt_output == true) { - bool pvt_result; - pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false); - - if (pvt_result == true) + if (d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false)) { if (std::fabs(current_RX_time - last_pvt_display_T_rx_s) * 1000.0 >= static_cast(d_display_rate_ms)) { @@ -1177,88 +1188,350 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } // ####################### RTCM MESSAGES ################# - if (b_rtcm_writing_started) + try { - if (type_of_rx == 1) // GPS L1 C/A + if (b_rtcm_writing_started) { - if (flag_write_RTCM_1019_output == true) + if (type_of_rx == 1) // GPS L1 C/A + { + if (flag_write_RTCM_1019_output == true) + { + for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gps_ephemeris_iter; + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + { + if (flag_write_RTCM_1045_output == true) + { + for (std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gal_ephemeris_iter; + gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if (type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if (flag_write_RTCM_1019_output == true) + { + for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + std::map::const_iterator gps_ephemeris_iter; + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_ephemeris_iter; + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if (flag_write_RTCM_1019_output == true) + { + for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_1045_output == true) + { + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) + { + if (system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + gps_channel = i; + } + } + } + if (gal_channel == 0) + { + if (system.compare("E") == 0) + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + gal_channel = i; + } + } + } + i++; + } + if (flag_write_RTCM_MSM_output == true) + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if (flag_write_RTCM_MSM_output == true) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + { + if (flag_write_RTCM_1020_output == true) + { + for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + + std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A + { + if (flag_write_RTCM_1019_output == true) + { + for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if (flag_write_RTCM_1020_output == true) + { + for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if (flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gps_channel == 0) + { + if (system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + gps_channel = i; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + glo_channel = i; + } + } + } + i++; + } + if (flag_write_RTCM_MSM_output == true) + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if (flag_write_RTCM_MSM_output == true) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B + { + if (flag_write_RTCM_1020_output == true) + { + for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + if (flag_write_RTCM_1045_output == true) + { + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if (flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if (gal_channel == 0) + { + if (system.compare("E") == 0) + { + // This is a channel with valid GPS signal + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + { + gal_channel = i; + } + } + } + if (glo_channel == 0) + { + if (system.compare("R") == 0) + { + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + glo_channel = i; + } + } + } + i++; + } + if (flag_write_RTCM_MSM_output == true) + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if (flag_write_RTCM_MSM_output == true) + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + } + + if (!b_rtcm_writing_started) // the first time + { + if (type_of_rx == 1) // GPS L1 C/A { for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) { d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); } - } - if (flag_write_RTCM_MSM_output == true) - { - std::map::const_iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + + std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo - { - if (flag_write_RTCM_1045_output == true) + if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo { for (std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_ephemeris_iter++) { d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); } - } - if (flag_write_RTCM_MSM_output == true) - { - std::map::const_iterator gal_ephemeris_iter; - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + + std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - } - if (type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - if (flag_write_RTCM_1019_output == true) + if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) { d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); } - } - if (flag_write_RTCM_MSM_output == true) - { - std::map::const_iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - std::map::const_iterator gps_cnav_ephemeris_iter; - gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + + std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); + std::map::const_iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - } - if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if (flag_write_RTCM_1019_output == true) + if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++) + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } } - } - if (flag_write_RTCM_1045_output == true) - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + if (d_rtcm_MT1045_rate_ms != 0) { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } } - } - if (flag_write_RTCM_MSM_output == true) - { - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { @@ -1269,7 +1542,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { // This is a channel with valid GPS signal gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { gps_channel = i; } @@ -1280,7 +1553,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (system.compare("E") == 0) { galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { gal_channel = i; } @@ -1288,62 +1561,54 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } i++; } - if (flag_write_RTCM_MSM_output == true) + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0)) { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS - { - if (flag_write_RTCM_1020_output == true) + if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS { for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) { d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); } - } - std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); + std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); - if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A - { - if (flag_write_RTCM_1019_output == true) - { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } + b_rtcm_writing_started = true; } - if (flag_write_RTCM_1020_output == true) + if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } } - } - if (flag_write_RTCM_MSM_output == true) - { + if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) + { + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); + } + } + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { std::string system(&gnss_observables_iter->second.System, 1); if (gps_channel == 0) @@ -1371,42 +1636,35 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } i++; } - if (flag_write_RTCM_MSM_output == true) + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + + b_rtcm_writing_started = true; + } + if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B + { + if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); } } - if (flag_write_RTCM_MSM_output == true) + if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) + for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); } } - } - } - if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B - { - if (flag_write_RTCM_1020_output == true) - { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - } - if (flag_write_RTCM_1045_output == true) - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - if (flag_write_RTCM_MSM_output == true) - { - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) { @@ -1436,266 +1694,32 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } i++; } - if (flag_write_RTCM_MSM_output == true) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (flag_write_RTCM_MSM_output == true) + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) { - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } } - - if (!b_rtcm_writing_started) // the first time + catch (const boost::exception& ex) { - if (type_of_rx == 1) // GPS L1 C/A - { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - - std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo - { - for (std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); - } - - std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); - - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - - std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); - std::map::const_iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if (d_rtcm_MT1045_rate_ms != 0) - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if (gps_channel == 0) - { - if (system.compare("G") == 0) - { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - gps_channel = i; - } - } - } - if (gal_channel == 0) - { - if (system.compare("E") == 0) - { - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - gal_channel = i; - } - } - } - i++; - } - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0)) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS - { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - - std::map::const_iterator glo_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); - - if (glo_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glo_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A - { - if (d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend(); gps_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - } - - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if (gps_channel == 0) - { - if (system.compare("G") == 0) - { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - gps_channel = i; - } - } - } - if (glo_channel == 0) - { - if (system.compare("R") == 0) - { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - glo_channel = i; - } - } - } - i++; - } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - b_rtcm_writing_started = true; - } - if (type_of_rx == 27) // GLONASS L1 C/A + Galileo E1B - { - if (d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1020(glonass_gnav_ephemeris_iter->second, d_ls_pvt->glonass_gnav_utc_model); - } - } - if (d_rtcm_MT1045_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for (galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); galileo_ephemeris_iter++) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if (gal_channel == 0) - { - if (system.compare("E") == 0) - { - // This is a channel with valid GPS signal - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) - { - gal_channel = i; - } - } - } - if (glo_channel == 0) - { - if (system.compare("R") == 0) - { - glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - glo_channel = i; - } - } - } - i++; - } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, {}, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } + std::cout << "RTCM boost exception: " << boost::diagnostic_information(ex) << std::endl; + LOG(ERROR) << "RTCM boost exception: " << boost::diagnostic_information(ex); + } + catch (const std::exception& ex) + { + std::cout << "RTCM std exception: " << ex.what() << std::endl; + LOG(ERROR) << "RTCM std exception: " << ex.what(); } } } // DEBUG MESSAGE: Display position in console output - if ((d_ls_pvt->is_valid_position() == true) && (flag_display_pvt == true)) + if (d_ls_pvt->is_valid_position() and flag_display_pvt) { std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 2b0a0072d..7456bf3c1 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -637,14 +637,7 @@ void dll_pll_veml_tracking::run_dll_pll(bool disable_costas_loop) void dll_pll_veml_tracking::clear_tracking_vars() { - if (d_veml) - { - *d_Very_Early = gr_complex(0.0, 0.0); - *d_Very_Late = gr_complex(0.0, 0.0); - } - *d_Early = gr_complex(0.0, 0.0); - *d_Prompt = gr_complex(0.0, 0.0); - *d_Late = gr_complex(0.0, 0.0); + std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); d_carr_error_hz = 0.0; d_carr_error_filt_hz = 0.0; d_code_error_chips = 0.0; @@ -1069,10 +1062,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) *out[0] = current_synchro_data; return 1; } - else - { - return 0; - } + return 0; } From bdce8b6a6628f928310b44801ab017880e34d1fa Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 13 Mar 2018 12:38:33 +0100 Subject: [PATCH 05/14] Add float code generation in some signals --- src/algorithms/libs/gps_l2c_signal.cc | 17 + src/algorithms/libs/gps_l2c_signal.h | 2 +- src/algorithms/libs/gps_l5_signal.cc | 31 + src/algorithms/libs/gps_l5_signal.h | 2 + .../gps_l5_telemetry_decoder_cc.cc | 10 +- .../gps_l5_telemetry_decoder_cc.h | 5 +- .../galileo_e1_dll_pll_veml_tracking.cc | 85 +- .../galileo_e1_dll_pll_veml_tracking.h | 3 + .../adapters/gps_l1_ca_dll_pll_tracking.cc | 74 +- .../adapters/gps_l1_ca_dll_pll_tracking.h | 3 + .../gnuradio_blocks/dll_pll_veml_tracking.cc | 732 ++++++++++-------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 80 +- .../libs/cpu_multicorrelator_real_codes.cc | 2 +- src/core/system_parameters/GPS_L5.h | 9 +- src/core/system_parameters/gnss_synchro.h | 14 +- 15 files changed, 654 insertions(+), 415 deletions(-) diff --git a/src/algorithms/libs/gps_l2c_signal.cc b/src/algorithms/libs/gps_l2c_signal.cc index 68fb7c024..a0d266ab7 100644 --- a/src/algorithms/libs/gps_l2c_signal.cc +++ b/src/algorithms/libs/gps_l2c_signal.cc @@ -71,6 +71,23 @@ void gps_l2c_m_code_gen_complex(std::complex* _dest, unsigned int _prn) delete[] _code; } +void gps_l2c_m_code_gen_float(float* _dest, unsigned int _prn) +{ + int32_t* _code = new int32_t[GPS_L2_M_CODE_LENGTH_CHIPS]; + + if (_prn > 0 and _prn < 51) + { + gps_l2c_m_code(_code, _prn); + } + + for (signed int i = 0; i < GPS_L2_M_CODE_LENGTH_CHIPS; i++) + { + _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + } + + delete[] _code; +} + /* * Generates complex GPS L2C M code for the desired SV ID and sampled to specific sampling frequency diff --git a/src/algorithms/libs/gps_l2c_signal.h b/src/algorithms/libs/gps_l2c_signal.h index a5f48a230..627588355 100644 --- a/src/algorithms/libs/gps_l2c_signal.h +++ b/src/algorithms/libs/gps_l2c_signal.h @@ -38,7 +38,7 @@ //!Generates complex GPS L2C M code for the desired SV ID void gps_l2c_m_code_gen_complex(std::complex* _dest, unsigned int _prn); - +void gps_l2c_m_code_gen_float(float* _dest, unsigned int _prn); //! Generates complex GPS L2C M code for the desired SV ID, and sampled to specific sampling frequency void gps_l2c_m_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, signed int _fs); diff --git a/src/algorithms/libs/gps_l5_signal.cc b/src/algorithms/libs/gps_l5_signal.cc index 7a443d20f..0bacbea7b 100644 --- a/src/algorithms/libs/gps_l5_signal.cc +++ b/src/algorithms/libs/gps_l5_signal.cc @@ -197,6 +197,22 @@ void gps_l5i_code_gen_complex(std::complex* _dest, unsigned int _prn) delete[] _code; } +void gps_l5i_code_gen_float(float* _dest, unsigned int _prn) +{ + int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS]; + + if (_prn > 0 and _prn < 51) + { + make_l5i(_code, _prn - 1); + } + + for (signed int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++) + { + _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + } + + delete[] _code; +} /* * Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency @@ -264,7 +280,22 @@ void gps_l5q_code_gen_complex(std::complex* _dest, unsigned int _prn) delete[] _code; } +void gps_l5q_code_gen_float(float* _dest, unsigned int _prn) +{ + int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS]; + if (_prn > 0 and _prn < 51) + { + make_l5q(_code, _prn - 1); + } + + for (signed int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++) + { + _dest[i] = 1.0 - 2.0 * static_cast(_code[i]); + } + + delete[] _code; +} /* * Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency */ diff --git a/src/algorithms/libs/gps_l5_signal.h b/src/algorithms/libs/gps_l5_signal.h index e9fee4e4b..928180e9a 100644 --- a/src/algorithms/libs/gps_l5_signal.h +++ b/src/algorithms/libs/gps_l5_signal.h @@ -38,9 +38,11 @@ //!Generates complex GPS L5i M code for the desired SV ID void gps_l5i_code_gen_complex(std::complex* _dest, unsigned int _prn); +void gps_l5i_code_gen_float(float* _dest, unsigned int _prn); //!Generates complex GPS L5q M code for the desired SV ID void gps_l5q_code_gen_complex(std::complex* _dest, unsigned int _prn); +void gps_l5q_code_gen_float(float* _dest, unsigned int _prn); //! Generates complex GPS L5i M code for the desired SV ID, and sampled to specific sampling frequency void gps_l5i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, signed int _fs); 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 79b0fdb07..af9666c03 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 @@ -70,9 +70,9 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( d_TOW_at_Preamble = 0.0; //initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); - for (int aux = 0; aux < GPS_L5_NH_CODE_LENGTH; aux++) + for (int aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++) { - if (GPS_L5_NH_CODE[aux] == 0) + if (GPS_L5i_NH_CODE[aux] == 0) { bits_NH[aux] = -1.0; } @@ -119,9 +119,9 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u int symbol_value = 0; //Search correlation with Neuman-Hofman Code (see IS-GPS-705D) - if (sym_hist.size() == GPS_L5_NH_CODE_LENGTH) + if (sym_hist.size() == GPS_L5i_NH_CODE_LENGTH) { - for (int i = 0; i < GPS_L5_NH_CODE_LENGTH; i++) + for (int i = 0; i < GPS_L5i_NH_CODE_LENGTH; i++) { if ((bits_NH[i] * sym_hist.at(i)) > 0.0) { @@ -132,7 +132,7 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u corr_NH -= 1; } } - if (abs(corr_NH) == GPS_L5_NH_CODE_LENGTH) + if (abs(corr_NH) == GPS_L5i_NH_CODE_LENGTH) { sync_NH = true; if (corr_NH > 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 c13c2832b..59f0bf108 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 @@ -42,8 +42,7 @@ #include #include -extern "C" -{ +extern "C" { #include "cnav_msg.h" #include "edc.h" #include "bits.h" @@ -91,7 +90,7 @@ private: bool d_flag_valid_word; Gps_CNAV_Navigation_Message d_CNAV_Message; - double bits_NH[GPS_L5_NH_CODE_LENGTH]; + double bits_NH[GPS_L5i_NH_CODE_LENGTH]; std::deque sym_hist; bool sync_NH; bool new_sym; diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index acae5564a..6ad7b940d 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -51,7 +51,6 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( //################# CONFIGURATION PARAMETERS ######################## int fs_in; int vector_length; - int f_if; bool dump; std::string dump_filename; std::string item_type; @@ -64,11 +63,10 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( float very_early_late_space_chips; float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; - + unified_ = configuration->property(role + ".unified", true); item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); dump = configuration->property(role + ".dump", false); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); @@ -93,23 +91,45 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { - item_size_ = sizeof(gr_complex); - tracking_ = galileo_e1_dll_pll_veml_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - early_late_space_chips, - very_early_late_space_chips, - early_late_space_narrow_chips, - very_early_late_space_narrow_chips, - extend_correlation_symbols, - track_pilot); + if (unified_) + { + char sig_[3] = "1B"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, 'E', sig_); + } + else + { + tracking_ = galileo_e1_dll_pll_veml_make_tracking_cc( + 0, + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot); + } } else { @@ -130,7 +150,10 @@ GalileoE1DllPllVemlTracking::~GalileoE1DllPllVemlTracking() void GalileoE1DllPllVemlTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } @@ -140,13 +163,19 @@ void GalileoE1DllPllVemlTracking::start_tracking() void GalileoE1DllPllVemlTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GalileoE1DllPllVemlTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } @@ -170,11 +199,17 @@ void GalileoE1DllPllVemlTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h index e702ff14d..2d3dfd221 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.h @@ -39,6 +39,7 @@ #include "tracking_interface.h" #include "galileo_e1_dll_pll_veml_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include @@ -95,11 +96,13 @@ public: private: galileo_e1_dll_pll_veml_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; #endif // GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_H_ diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index d58329575..4f3fc3e53 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -52,7 +52,6 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( //################# CONFIGURATION PARAMETERS ######################## int fs_in; int vector_length; - int f_if; bool dump; std::string dump_filename; std::string item_type; @@ -63,8 +62,8 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); dump = configuration->property(role + ".dump", false); + unified_ = configuration->property(role + ".unified", true); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); @@ -73,20 +72,44 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( std::string default_dump_filename = "./track_ch"; dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); - + int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); + if (symbols_extended_correlator < 1) symbols_extended_correlator = 1; //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { - item_size_ = sizeof(gr_complex); - tracking_ = gps_l1_ca_dll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); + if (unified_) + { + char sig_[3] = "1C"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_hz, + dll_bw_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_chips, + early_late_space_chips, + symbols_extended_correlator, + false, + 'G', sig_); + } + else + { + tracking_ = gps_l1_ca_dll_pll_make_tracking_cc( + 0, + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + early_late_space_chips); + } } else { @@ -105,7 +128,10 @@ GpsL1CaDllPllTracking::~GpsL1CaDllPllTracking() void GpsL1CaDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } @@ -115,13 +141,19 @@ void GpsL1CaDllPllTracking::start_tracking() void GpsL1CaDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GpsL1CaDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } @@ -145,11 +177,17 @@ void GpsL1CaDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GpsL1CaDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GpsL1CaDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h index ff5a91899..5bf34abfc 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h @@ -40,6 +40,7 @@ #include "tracking_interface.h" #include "gps_l1_ca_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -93,11 +94,13 @@ public: private: gps_l1_ca_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; #endif // GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_H_ diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 7456bf3c1..2e469bb6d 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -35,6 +35,22 @@ */ #include "dll_pll_veml_tracking.h" +#include "tracking_discriminators.h" +#include "lock_detectors.h" +#include "control_message_factory.h" +#include "MATH_CONSTANTS.h" + +#include "Galileo_E1.h" +#include "galileo_e1_signal_processing.h" +#include "Galileo_E5a.h" +#include "galileo_e5_signal_processing.h" +#include "GPS_L1_CA.h" +#include "gps_sdr_signal_processing.h" +#include "GPS_L2C.h" +#include "gps_l2c_signal.h" +#include "GPS_L5.h" +#include "gps_l5_signal.h" + #include #include #include @@ -44,16 +60,7 @@ #include #include #include -#include "tracking_discriminators.h" -#include "lock_detectors.h" -#include "control_message_factory.h" -#include "Galileo_E1.h" -#include "galileo_e1_signal_processing.h" -#include "Galileo_E5a.h" -#include "GPS_L1_CA.h" -#include "GPS_L2C.h" -#include "GPS_L5.h" using google::LogMessage; @@ -72,7 +79,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking( float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3], bool veml) + char system, char signal[3]) { return dll_pll_veml_tracking_sptr(new dll_pll_veml_tracking( fs_in, @@ -88,7 +95,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking( early_late_space_narrow_chips, very_early_late_space_narrow_chips, extend_correlation_symbols, - track_pilot, system, signal, veml)); + track_pilot, system, signal)); } @@ -108,8 +115,8 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( float pll_bw_narrow_hz, float dll_bw_narrow_hz, float early_late_space_chips, float very_early_late_space_chips, float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, - int extend_correlation_symbols, bool track_pilot, char system, char signal[3], bool veml) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + int extend_correlation_symbols, bool track_pilot, char system, char signal[3]) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -118,19 +125,14 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // initialize internal vars d_dump = dump; - d_veml = veml; + d_veml = false; d_track_pilot = track_pilot; d_fs_in = fs_in; d_vector_length = vector_length; d_dump_filename = dump_filename; - d_code_period = 0.0; d_code_chip_rate = 0.0; - d_signal_carrier_freq = 0.0; - d_code_length_chips = 0; - d_secondary = false; d_secondary_code_length = 0; d_secondary_code_string = nullptr; - d_correlation_length_ms = 0; signal_type = std::string(signal); if (system == 'G') { @@ -140,10 +142,14 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_signal_carrier_freq = GPS_L1_FREQ_HZ; d_code_period = GPS_L1_CA_CODE_PERIOD; d_code_chip_rate = GPS_L1_CA_CODE_RATE_HZ; + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); + //GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; d_track_pilot = false; + interchange_iq = false; } else if (signal_type.compare("2S") == 0) { @@ -151,18 +157,37 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_code_period = GPS_L2_M_PERIOD; d_code_chip_rate = GPS_L2_M_CODE_RATE_HZ; d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 20; + d_code_samples_per_chip = 1; + //GPS L2 does not have pilot component nor secondary code d_secondary = false; d_track_pilot = false; + interchange_iq = false; } else if (signal_type.compare("L5") == 0) { d_signal_carrier_freq = GPS_L5_FREQ_HZ; d_code_period = GPS_L5i_PERIOD; d_code_chip_rate = GPS_L5i_CODE_RATE_HZ; + d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GPS_L5i_CODE_LENGTH_CHIPS); - d_secondary = false; + //GPS L5 does not have pilot secondary code + d_secondary = true; + if (d_track_pilot) + { + d_secondary_code_length = static_cast(GPS_L5q_NH_CODE_LENGTH); + d_secondary_code_string = const_cast(&GPS_L5q_NH_CODE_STR); + interchange_iq = true; + } + else + { + d_secondary_code_length = static_cast(GPS_L5i_NH_CODE_LENGTH); + d_secondary_code_string = const_cast(&GPS_L5i_NH_CODE_STR); + interchange_iq = false; + } } else { @@ -179,32 +204,33 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_code_period = Galileo_E1_CODE_PERIOD; d_code_chip_rate = Galileo_E1_CODE_CHIP_RATE_HZ; d_code_length_chips = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + d_symbols_per_bit = 1; d_correlation_length_ms = 4; + d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip + d_veml = true; d_secondary = true; - if (d_track_pilot) - { - d_secondary_code_length = static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); - d_secondary_code_string = const_cast(&Galileo_E1_C_SECONDARY_CODE); - } - else - { - d_secondary = false; - } + d_secondary_code_length = static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E1_C_SECONDARY_CODE); + interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. } else if (signal_type.compare("5X") == 0) { d_signal_carrier_freq = Galileo_E5a_FREQ_HZ; d_code_period = GALILEO_E5a_CODE_PERIOD; d_code_chip_rate = Galileo_E5a_CODE_CHIP_RATE_HZ; + d_symbols_per_bit = 20; d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; d_code_length_chips = static_cast(Galileo_E5a_CODE_LENGTH_CHIPS); d_secondary = true; if (d_track_pilot) { + interchange_iq = true; d_secondary_code_length = static_cast(Galileo_E5a_Q_SECONDARY_CODE_LENGTH); } else { + interchange_iq = false; d_secondary_code_length = static_cast(Galileo_E5a_I_SECONDARY_CODE_LENGTH); d_secondary_code_string = const_cast(&Galileo_E5a_I_SECONDARY_CODE); } @@ -220,10 +246,10 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( LOG(WARNING) << "Invalid System argument when instantiating tracking blocks"; std::cout << "Invalid System argument when instantiating tracking blocks" << std::endl; } - - - d_code_loop_filter = Tracking_2nd_DLL_filter(d_code_period); - d_carrier_loop_filter = Tracking_2nd_PLL_filter(d_code_period); + T_chip_seconds = 0.0; + T_prn_seconds = 0.0; + T_prn_samples = 0.0; + K_blk_samples = 0.0; // Initialize tracking ========================================== @@ -235,6 +261,8 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); + d_code_loop_filter = Tracking_2nd_DLL_filter(static_cast(d_code_period)); + d_carrier_loop_filter = Tracking_2nd_PLL_filter(static_cast(d_code_period)); // Correlator spacing d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) @@ -245,7 +273,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // Initialization of local code replica // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip d_tracking_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); - + std::fill_n(d_tracking_code, 2 * d_code_length_chips, 0.0); // correlator outputs (scalar) if (d_veml) { @@ -270,12 +298,12 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_Prompt = &d_correlator_outs[2]; d_Late = &d_correlator_outs[3]; d_Very_Late = &d_correlator_outs[4]; - d_local_code_shift_chips[0] = -d_very_early_late_spc_chips; - d_local_code_shift_chips[1] = -d_early_late_spc_chips; + d_local_code_shift_chips[0] = -d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_chips; - d_null_shift = &d_local_code_shift_chips[2]; + d_local_code_shift_chips[3] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[2]; } else { @@ -284,38 +312,34 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_Prompt = &d_correlator_outs[1]; d_Late = &d_correlator_outs[2]; d_Very_Late = nullptr; - d_local_code_shift_chips[0] = -d_early_late_spc_chips; + d_local_code_shift_chips[0] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); d_local_code_shift_chips[1] = 0.0; - d_local_code_shift_chips[2] = d_early_late_spc_chips; - d_null_shift = &d_local_code_shift_chips[1]; + d_local_code_shift_chips[2] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[1]; } - d_correlation_length_samples = d_vector_length; - multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); + multicorrelator_cpu.init(2 * d_vector_length, d_n_correlator_taps); - d_extend_correlation_symbols = extend_correlation_symbols; - // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal - if (d_track_pilot) + if (extend_correlation_symbols > 1) { - // extended integration control - if (d_extend_correlation_symbols > 1) - { - d_enable_extended_integration = true; - } - else - { - d_enable_extended_integration = false; - } - // Extra correlator for the data component - correlator_data_cpu.init(2 * d_correlation_length_samples, 1); - d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_Prompt_Data[0] = gr_complex(0.0, 0.0); - d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + d_enable_extended_integration = true; + d_extend_correlation_symbols = extend_correlation_symbols; } else { - // Disable extended integration if data component tracking is selected d_enable_extended_integration = false; + d_extend_correlation_symbols = 1; + } + + // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal + if (d_track_pilot) + { + // Extra correlator for the data component + correlator_data_cpu.init(2 * d_vector_length, 1); + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); + std::fill_n(d_data_code, 2 * d_code_length_chips, 0.0); } //--- Initializations ---// @@ -355,7 +379,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_rem_code_phase_chips = 0.0; d_K_blk_samples = 0.0; d_code_phase_samples = 0.0; - + d_last_prompt = gr_complex(0.0, 0.0); d_state = 0; // initial state: standby } @@ -371,14 +395,15 @@ void dll_pll_veml_tracking::start_tracking() d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; long int acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; - DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_fs_in; + DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; + DLOG(INFO) << "Number of seconds between Acquisition and Tracking = " << acq_trk_diff_seconds; // Doppler effect // Fd=(C/(C+Vr))*F double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; // new chip and prn sequence periods based on acq Doppler d_code_freq_chips = radial_velocity * d_code_chip_rate; - d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + d_code_phase_step_chips = d_code_freq_chips / d_fs_in; double T_chip_mod_seconds = 1.0 / d_code_freq_chips; double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); double T_prn_mod_samples = T_prn_mod_seconds * d_fs_in; @@ -399,29 +424,89 @@ void dll_pll_veml_tracking::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; d_carrier_doppler_hz = d_acq_carrier_doppler_hz; - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / d_fs_in; + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_fs_in; // DLL/PLL filter initialization d_carrier_loop_filter.initialize(); // initialize the carrier filter d_code_loop_filter.initialize(); // initialize the code filter - if (d_track_pilot) + if (systemName.compare("GPS") == 0 and signal_type.compare("1C") == 0) { - char pilot_signal[3] = "1C"; - galileo_e1_code_gen_float_sampled(d_tracking_code, pilot_signal, false, - d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); - galileo_e1_code_gen_float_sampled(d_data_code, d_acquisition_gnss_synchro->Signal, false, - d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); - d_Prompt_Data[0] = gr_complex(0.0, 0.0); - correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_null_shift); + gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); } - else + else if (systemName.compare("GPS") == 0 and signal_type.compare("2S") == 0) { - galileo_e1_code_gen_float_sampled(d_tracking_code, d_acquisition_gnss_synchro->Signal, false, - d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); + gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + } + else if (systemName.compare("GPS") == 0 and signal_type.compare("L5") == 0) + { + if (d_track_pilot) + { + gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + } + } + else if (systemName.compare("Galileo") == 0 and signal_type.compare("1B") == 0) + { + if (d_track_pilot) + { + char pilot_signal[3] = "1C"; + galileo_e1_code_gen_float_sampled(d_tracking_code, + pilot_signal, + false, //CBOC disabled + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + galileo_e1_code_gen_float_sampled(d_data_code, + d_acquisition_gnss_synchro->Signal, + false, //CBOC disabled + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + galileo_e1_code_gen_float_sampled(d_tracking_code, + d_acquisition_gnss_synchro->Signal, + false, //CBOC disabled + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + } + } + else if (systemName.compare("Galileo") == 0 and signal_type.compare("5X") == 0) + { + gr_complex aux_code[d_code_length_chips]; + galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); + if (d_track_pilot) + { + d_secondary_code_string = const_cast(&Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]); + for (unsigned int i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].imag(); + d_data_code[i] = aux_code[i].real(); + } + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift); + } + else + { + for (unsigned int i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].real(); + } + } } - multicorrelator_cpu.set_local_code_and_taps(d_code_length_chips, d_tracking_code, d_local_code_shift_chips); + multicorrelator_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_tracking_code, d_local_code_shift_chips); std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); d_carrier_lock_fail_counter = 0; @@ -429,8 +514,28 @@ void dll_pll_veml_tracking::start_tracking() d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; d_acc_carrier_phase_rad = 0.0; + d_cn0_estimation_counter = 0; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_chips * static_cast(d_code_samples_per_chip); + } d_code_phase_samples = d_acq_code_phase_samples; + d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); + d_carrier_loop_filter.set_pdi(static_cast(d_code_period)); + d_code_loop_filter.set_pdi(static_cast(d_code_period)); // DEBUG OUTPUT std::cout << "Tracking of " << systemName << " " << signal_type << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; @@ -438,10 +543,11 @@ void dll_pll_veml_tracking::start_tracking() // enable tracking pull-in d_state = 1; - - LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz - << " Code Phase correction [samples]=" << delay_correction_samples - << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; + d_Prompt_buffer_deque.clear(); + d_last_prompt = gr_complex(0.0, 0.0); + LOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz + << ". Code Phase correction [samples] = " << delay_correction_samples + << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; } @@ -546,7 +652,7 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, d_fs_in, static_cast(d_code_length_chips)); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, static_cast(d_fs_in), static_cast(d_code_length_chips)); // Carrier lock indicator d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES); // Loss of lock detection @@ -587,9 +693,9 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); + d_rem_code_phase_chips * static_cast(d_code_samples_per_chip), + d_code_phase_step_chips * static_cast(d_code_samples_per_chip), + d_vector_length); // DATA CORRELATOR (if tracking tracks the pilot signal) if (d_track_pilot) @@ -598,26 +704,26 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); + d_rem_code_phase_chips * static_cast(d_code_samples_per_chip), + d_code_phase_step_chips * static_cast(d_code_samples_per_chip), + d_vector_length); } } -void dll_pll_veml_tracking::run_dll_pll(bool disable_costas_loop) +void dll_pll_veml_tracking::run_dll_pll(bool enable_costas_loop) { // ################## PLL ########################################################## // PLL discriminator - if (disable_costas_loop == true) + if (enable_costas_loop) { - // Secondary code acquired. No symbols transition should be present in the signal - d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; } else { - // Costas loop discriminator, insensitive to 180 deg phase transitions - d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + // Secondary code acquired. No symbols transition should be present in the signal + d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2; } // Carrier discriminator filter @@ -625,11 +731,18 @@ void dll_pll_veml_tracking::run_dll_pll(bool disable_costas_loop) // New carrier Doppler frequency estimation d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; // New code Doppler frequency estimation - d_code_freq_chips = (1.0 + d_carrier_doppler_hz / d_signal_carrier_freq) * d_code_chip_rate; + d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate; // ################## DLL ########################################################## // DLL discriminator - d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] + if (d_veml) + { + d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] + } + else + { + d_code_error_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu); // [chips/Ti] + } // Code discriminator filter d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] } @@ -638,11 +751,87 @@ void dll_pll_veml_tracking::run_dll_pll(bool disable_costas_loop) void dll_pll_veml_tracking::clear_tracking_vars() { std::fill_n(d_correlator_outs, d_n_correlator_taps, gr_complex(0.0, 0.0)); + if (d_track_pilot) *d_Prompt_Data = gr_complex(0.0, 0.0); d_carr_error_hz = 0.0; d_carr_error_filt_hz = 0.0; d_code_error_chips = 0.0; d_code_error_filt_chips = 0.0; d_current_symbol = 0; + d_Prompt_buffer_deque.clear(); + d_last_prompt = gr_complex(0.0, 0.0); +} + +void dll_pll_veml_tracking::update_tracking_vars() +{ + T_chip_seconds = 1.0 / d_code_freq_chips; + T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); + double code_error_filt_secs = T_prn_seconds * d_code_error_filt_chips * T_chip_seconds; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + T_prn_samples = T_prn_seconds * d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * d_fs_in; + d_current_prn_length_samples = static_cast(round(K_blk_samples)); // round to a discrete number of samples + + //################### PLL COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_fs_in; + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad += d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / d_fs_in; + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / d_fs_in; +} + +void dll_pll_veml_tracking::save_correlation_results() +{ + if (d_secondary) + { + if (d_secondary_code_string->at(d_current_symbol) == '0') + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } + else + { + if (d_veml) + { + d_VE_accu -= *d_Very_Early; + d_VL_accu -= *d_Very_Late; + } + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol %= d_secondary_code_length; + } + else + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } } @@ -656,10 +845,16 @@ void dll_pll_veml_tracking::log_data() float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; float tmp_float; double tmp_double; - - prompt_I = static_cast(d_P_accu.real()); - prompt_Q = static_cast(d_P_accu.imag()); - + if (d_track_pilot) + { + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); + } + else + { + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); + } if (d_veml) { tmp_VE = std::abs(d_VE_accu); @@ -727,7 +922,7 @@ void dll_pll_veml_tracking::log_data() } -int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), +int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { gr::thread::scoped_lock l(d_setlock); @@ -737,8 +932,11 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) switch (d_state) { - case 0: // Standby - Pass Through + case 0: // Standby - Pass Through. Full throttle { + d_sample_counter += ninput_items[0]; + consume_each(ninput_items[0]); + return 0; break; } case 1: // Pull-in @@ -746,22 +944,20 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) // Signal alignment (skip samples until the incoming signal is aligned with local replica) // Fill the acquisition data int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - double acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + double acq_trk_shif_correction_samples = static_cast(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); int samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + if (samples_offset < 0) + { + samples_offset = 0; + } + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_state = 2; d_sample_counter += samples_offset; // count for the processed samples consume_each(samples_offset); // shift input to perform alignment with local replica - d_state = 2; // next state is the symbol synchronization return 0; } case 2: // Wide tracking and symbol synchronization { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / d_fs_in; - d_code_phase_step_chips = d_code_freq_chips / d_fs_in; - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_phase_step_chips; - // Perform a correlation step do_correlation_step(in); // Save single correlation step variables if (d_veml) @@ -781,103 +977,89 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } else { - // Perform DLL/PLL tracking loop computations - run_dll_pll(false); - - // ################## PLL COMMANDS ################################################# - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; - // remanent carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL COMMANDS ################################################# - // Code error from DLL - double code_error_filt_secs = d_code_period * d_code_error_filt_chips / d_code_chip_rate; // [seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); - double T_prn_samples = T_prn_seconds * d_fs_in; - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * d_fs_in; - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - - // ########### Output the tracking results to Telemetry block ########## - if (d_track_pilot) - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; + bool next_state = false; + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_dll_pll(true); + update_tracking_vars(); // enable write dump file this cycle (valid DLL/PLL cycle) log_data(); - - if (d_enable_extended_integration) + if (d_secondary) { // ####### SECONDARY CODE LOCK ##### d_Prompt_buffer_deque.push_back(*d_Prompt); if (d_Prompt_buffer_deque.size() == d_secondary_code_length) { - if (acquire_secondary()) - { - d_extend_correlation_symbols_count = 0; - // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - d_Prompt_buffer_deque.clear(); - d_current_symbol = 0; - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - - // Set narrow taps delay values [chips] - if (d_veml) - { - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips; - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips; - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; - } - else - { - d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips; - d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips; - } - LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - - // UPDATE INTEGRATION TIME - float new_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); - d_carrier_loop_filter.set_pdi(new_correlation_time_s); - d_code_loop_filter.set_pdi(new_correlation_time_s); - - d_state = 3; // next state is the extended correlator integrator - } - + next_state = acquire_secondary(); d_Prompt_buffer_deque.pop_front(); } } + else //Signal does not have secondary code. Search a bit transition by sign change + { + if (d_current_symbol == (d_symbols_per_bit - 1)) + { + next_state = true; + } + else if (d_current_symbol > 0) + { + d_current_symbol++; + } + else if (d_last_prompt.real() != 0.0) + { + if (d_Prompt->real() * d_last_prompt.real() < 0.0) + { + d_current_symbol = 1; + } + } + d_last_prompt = *d_Prompt; + } + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_last_prompt = gr_complex(0.0, 0.0); + d_Prompt_buffer_deque.clear(); + d_current_symbol = 0; + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + + // Set narrow taps delay values [chips] + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + + // UPDATE INTEGRATION TIME + if (d_enable_extended_integration) + { + d_extend_correlation_symbols_count = 0; + float new_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); + d_carrier_loop_filter.set_pdi(new_correlation_time_s); + d_code_loop_filter.set_pdi(new_correlation_time_s); + d_state = 3; // next state is the extended correlator integrator + LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + else + { + d_state = 4; + } + } } break; } @@ -885,73 +1067,31 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; - // Current NCO and code generator parameters - d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / d_fs_in; - d_code_phase_step_chips = d_code_freq_chips / d_fs_in; - d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; // perform a correlation step do_correlation_step(in); - // correct the integration sign using the current symbol of the secondary code - if (d_secondary_code_string->at(d_current_symbol) == '0') + update_tracking_vars(); + save_correlation_results(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) { - if (d_veml) - { - d_VE_accu += *d_Very_Early; - d_VL_accu += *d_Very_Late; - } - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); } else { - if (d_veml) - { - d_VE_accu -= *d_Very_Early; - d_VL_accu -= *d_Very_Late; - } - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); } - d_current_symbol++; - // secondary code roll-up - d_current_symbol %= d_secondary_code_length; - - // PLL/DLL not enabled, we are in the middle of a coherent integration - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); - double T_prn_samples = T_prn_seconds * d_fs_in; - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples; - d_current_prn_length_samples = static_cast(round(K_blk_samples)); //round to a discrete samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); //rounding error < 1 sample current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = d_correlation_length_ms; - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count >= (d_extend_correlation_symbols - 1)) + if (d_extend_correlation_symbols_count == (d_extend_correlation_symbols - 1)) { d_extend_correlation_symbols_count = 0; d_state = 4; @@ -962,35 +1102,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; + // perform a correlation step do_correlation_step(in); - - // correct the integration using the current symbol - if (d_secondary_code_string->at(d_current_symbol) == '0') - { - if (d_veml) - { - d_VE_accu += *d_Very_Early; - d_VL_accu += *d_Very_Late; - } - d_E_accu += *d_Early; - d_P_accu += *d_Prompt; - d_L_accu += *d_Late; - } - else - { - if (d_veml) - { - d_VE_accu -= *d_Very_Early; - d_VL_accu -= *d_Very_Late; - } - d_E_accu -= *d_Early; - d_P_accu -= *d_Prompt; - d_L_accu -= *d_Late; - } - d_current_symbol++; - // secondary code roll-up - d_current_symbol %= d_secondary_code_length; + save_correlation_results(); // check lock status if (!cn0_and_tracking_lock_status()) @@ -1000,34 +1115,32 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } else { - run_dll_pll(true); // Costas loop disabled, use four quadrant atan - - // ################## PLL ########################################################## - // carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / d_fs_in; - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL ########################################################## - // Code phase accumulator - double code_error_filt_secs = d_code_period * d_code_error_filt_chips / d_code_chip_rate; //[seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); - double T_prn_samples = T_prn_seconds * d_fs_in; - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * d_fs_in; - d_current_prn_length_samples = static_cast(round(K_blk_samples)); // round to a discrete number of samples + run_dll_pll(!d_secondary); + //EQUIVALENT TO THE LINE ABOVE if (d_secondary) + //{ + // //If secondary code is locked, disable Costas Loop + // run_dll_pll(false); + //} + //else + //{ + // //The signal does not have secondary code, enable Costas Loop + // run_dll_pll(true); + //} + update_tracking_vars(); // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + if (interchange_iq) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - // compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); //rounding error < 1 sample current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -1041,20 +1154,15 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_P_accu = gr_complex(0.0, 0.0); d_L_accu = gr_complex(0.0, 0.0); d_VL_accu = gr_complex(0.0, 0.0); - d_state = 3; //new coherent integration (correlation time extension) cycle + if (d_enable_extended_integration) + { + d_state = 3; //new coherent integration (correlation time extension) cycle + } } } } - - //assign the GNURadio block output data - // current_synchro_data.System = {'E'}; - // std::string str_aux = "1B"; - // const char * str = str_aux.c_str(); // get a C style null terminated string - // std::memcpy(static_cast(current_synchro_data.Signal), str, 3); - consume_each(d_current_prn_length_samples); d_sample_counter += d_current_prn_length_samples; - if (current_synchro_data.Flag_valid_symbol_output) { current_synchro_data.fs = static_cast(d_fs_in); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 6538de571..a91d041f8 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -37,13 +37,14 @@ #define DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER 50 #define DLL_PLL_CARRIER_LOCK_THRESHOLD 0.85 -#include -#include -#include #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_PLL_filter.h" #include "cpu_multicorrelator_real_codes.h" +#include +#include +#include + class dll_pll_veml_tracking; @@ -57,7 +58,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3], bool veml); + char system, char signal[3]); /*! * \brief This class implements a code DLL + carrier PLL VEML (Very Early @@ -91,7 +92,7 @@ private: float very_early_late_space_chips, float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3], bool veml); + char system, char signal[3]); dll_pll_veml_tracking(double fs_in, unsigned int vector_length, bool dump, @@ -106,62 +107,63 @@ private: float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot, - char system, char signal[3], bool veml); + char system, char signal[3]); bool cn0_and_tracking_lock_status(); + bool acquire_secondary(); void do_correlation_step(const gr_complex *input_samples); void run_dll_pll(bool disable_costas_loop); - void update_local_code(); - void update_local_carrier(); - bool acquire_secondary(); - + void update_tracking_vars(); void clear_tracking_vars(); - + void save_correlation_results(); void log_data(); + int save_matfile(); // tracking configuration vars bool d_dump; bool d_veml; - bool d_secondary; - unsigned int d_secondary_code_length; - std::string *d_secondary_code_string; - Gnss_Synchro *d_acquisition_gnss_synchro; unsigned int d_vector_length; unsigned int d_channel; - // long d_fs_in; double d_fs_in; - + Gnss_Synchro *d_acquisition_gnss_synchro; //Signal parameters + bool d_secondary; + bool interchange_iq; double d_signal_carrier_freq; double d_code_period; double d_code_chip_rate; + unsigned int d_secondary_code_length; unsigned int d_code_length_chips; + unsigned int d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) + int d_symbols_per_bit; + std::string systemName; + std::string signal_type; + std::string *d_secondary_code_string; //tracking state machine int d_state; //Integration period in samples - int d_correlation_length_samples; int d_correlation_length_ms; int d_n_correlator_taps; - double d_early_late_spc_chips; - double d_very_early_late_spc_chips; - double d_early_late_spc_narrow_chips; - double d_very_early_late_spc_narrow_chips; + float d_early_late_spc_chips; + float d_very_early_late_spc_chips; + float d_early_late_spc_narrow_chips; + float d_very_early_late_spc_narrow_chips; float *d_tracking_code; float *d_data_code; float *d_local_code_shift_chips; - float *d_null_shift; - gr_complex *d_correlator_outs; + float *d_prompt_data_shift; cpu_multicorrelator_real_codes multicorrelator_cpu; - //TODO: currently the multicorrelator does not support adding extra correlator - //with different local code, thus we need extra multicorrelator instance. - //Implement this functionality inside multicorrelator class - //as an enhancement to increase the performance cpu_multicorrelator_real_codes correlator_data_cpu; //for data channel - + /* TODO: currently the multicorrelator does not support adding extra correlator + with different local code, thus we need extra multicorrelator instance. + Implement this functionality inside multicorrelator class + as an enhancement to increase the performance + */ + gr_complex *d_correlator_outs; gr_complex *d_Very_Early; gr_complex *d_Early; gr_complex *d_Prompt; @@ -178,6 +180,7 @@ private: gr_complex d_P_accu; gr_complex d_L_accu; gr_complex d_VL_accu; + gr_complex d_last_prompt; bool d_track_pilot; gr_complex *d_Prompt_Data; @@ -206,39 +209,34 @@ private: double d_carr_error_filt_hz; double d_code_error_chips; double d_code_error_filt_chips; - double d_K_blk_samples; - double d_code_freq_chips; double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_rem_code_phase_chips; double d_code_phase_samples; - + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; //PRN period in samples int d_current_prn_length_samples; - //processing samples counters unsigned long int d_sample_counter; unsigned long int d_acq_sample_stamp; // CN0 estimation and lock detector int d_cn0_estimation_counter; - std::deque d_Prompt_buffer_deque; - gr_complex *d_Prompt_buffer; + int d_carrier_lock_fail_counter; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; - int d_carrier_lock_fail_counter; + std::deque d_Prompt_buffer_deque; + gr_complex *d_Prompt_buffer; // file dump std::string d_dump_filename; std::ofstream d_dump_file; - - std::string systemName; - std::string signal_type; - - int save_matfile(); }; #endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_H diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index 4c7735aa2..6fa82351c 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -122,7 +122,7 @@ bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler( lv_32fc_t phase_offset_as_complex[1]; phase_offset_as_complex[0] = lv_cmake(std::cos(rem_carrier_phase_in_rad), -std::sin(rem_carrier_phase_in_rad)); // call VOLK_GNSSSDR kernel - volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, (const float**)d_local_codes_resampled, d_n_correlators, signal_length_samples); + volk_gnsssdr_32fc_32f_rotator_dot_prod_32fc_xn(d_corr_out, d_sig_in, std::exp(lv_32fc_t(0.0, -phase_step_rad)), phase_offset_as_complex, const_cast(d_local_codes_resampled), d_n_correlators, signal_length_samples); return true; } diff --git a/src/core/system_parameters/GPS_L5.h b/src/core/system_parameters/GPS_L5.h index af317cfc3..ff7ded0d4 100644 --- a/src/core/system_parameters/GPS_L5.h +++ b/src/core/system_parameters/GPS_L5.h @@ -36,6 +36,7 @@ #include "GPS_CNAV.h" #include "MATH_CONSTANTS.h" #include +#include // Physical constants @@ -181,7 +182,11 @@ const int GPS_L5_SYMBOLS_PER_BIT = 2; const int GPS_L5_SAMPLES_PER_SYMBOL = 10; const int GPS_L5_CNAV_DATA_PAGE_SYMBOLS = 600; const int GPS_L5_CNAV_DATA_PAGE_DURATION_S = 6; -const int GPS_L5_NH_CODE_LENGTH = 10; -const int GPS_L5_NH_CODE[10] = {0, 0, 0, 0, 1, 1, 0, 1, 0, 1}; +const int GPS_L5i_NH_CODE_LENGTH = 10; +const int GPS_L5i_NH_CODE[10] = {0, 0, 0, 0, 1, 1, 0, 1, 0, 1}; +const std::string GPS_L5i_NH_CODE_STR = "0000110101"; +const int GPS_L5q_NH_CODE_LENGTH = 20; +const int GPS_L5q_NH_CODE[20] = {0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 1, 0, 1, 0, 0, 1, 1, 1, 0}; +const std::string GPS_L5q_NH_CODE_STR = "00000100110101001110"; #endif /* GNSS_SDR_GPS_L5_H_ */ diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 267df4557..8f4c4d880 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -50,7 +50,7 @@ public: double Acq_delay_samples; //!< Set by Acquisition processing block double Acq_doppler_hz; //!< Set by Acquisition processing block unsigned long int Acq_samplestamp_samples; //!< Set by Acquisition processing block - bool Flag_valid_acquisition; //!< Set by Acquisition processing block + bool Flag_valid_acquisition = false; //!< Set by Acquisition processing block //Tracking long int fs; //!< Set by Tracking processing block double Prompt_I; //!< Set by Tracking processing block @@ -61,17 +61,17 @@ public: double Code_phase_samples; //!< Set by Tracking processing block unsigned long int Tracking_sample_counter; //!< Set by Tracking processing block - bool Flag_valid_symbol_output; //!< Set by Tracking processing block - int correlation_length_ms; //!< Set by Tracking processing block + bool Flag_valid_symbol_output = false; //!< Set by Tracking processing block + int correlation_length_ms; //!< Set by Tracking processing block //Telemetry Decoder - bool Flag_valid_word; //!< Set by Telemetry Decoder processing block + bool Flag_valid_word = false; //!< Set by Telemetry Decoder processing block double TOW_at_current_symbol_s; //!< Set by Telemetry Decoder processing block // Observables - double Pseudorange_m; //!< Set by Observables processing block - double RX_time; //!< Set by Observables processing block - bool Flag_valid_pseudorange; //!< Set by Observables processing block + double Pseudorange_m; //!< Set by Observables processing block + double RX_time; //!< Set by Observables processing block + bool Flag_valid_pseudorange = false; //!< Set by Observables processing block }; #endif From a7737d55da92556dd593a5fa16a5ab5c84d5e7c6 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 16 Mar 2018 12:21:13 +0100 Subject: [PATCH 06/14] Fix Gal E1 primary code generation --- .../libs/galileo_e1_signal_processing.cc | 22 ++- .../libs/galileo_e1_signal_processing.h | 5 + .../galileo_e1_dll_pll_veml_tracking.cc | 2 +- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 2 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 155 ++++++++++++------ 5 files changed, 123 insertions(+), 63 deletions(-) diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index ebeefcccf..beea0297c 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -53,7 +53,7 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], signed int _prn) for (size_t i = 0; i < Galileo_E1_B_PRIMARY_CODE[prn].length(); i++) { hex_to_binary_converter(&_dest[index], Galileo_E1_B_PRIMARY_CODE[prn].at(i)); - index = index + 4; + index += 4; } } else if (_galileo_signal.rfind("1C") != std::string::npos && _galileo_signal.length() >= 2) @@ -61,13 +61,9 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], signed int _prn) for (size_t i = 0; i < Galileo_E1_C_PRIMARY_CODE[prn].length(); i++) { hex_to_binary_converter(&_dest[index], Galileo_E1_C_PRIMARY_CODE[prn].at(i)); - index = index + 4; + index += 4; } } - else - { - return; - } } @@ -107,6 +103,18 @@ void galileo_e1_sinboc_61_gen_int(int* _dest, int* _prn, unsigned int _length_ou } } +void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], unsigned int _prn) +{ + std::string _galileo_signal = _Signal; + unsigned int _codeLength = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + int primary_code_E1_chips[4092]; // _codeLength not accepted by Clang + galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip + for (unsigned int i = 0; i < _codeLength; i++) + { + _dest[2 * i] = static_cast(primary_code_E1_chips[i]); + _dest[2 * i + 1] = -_dest[2 * i]; + } +} void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3]) { @@ -137,8 +145,6 @@ void galileo_e1_gen_float(float* _dest, int* _prn, char _Signal[3]) beta * static_cast(sinboc_61[i]); } } - else - return; } diff --git a/src/algorithms/libs/galileo_e1_signal_processing.h b/src/algorithms/libs/galileo_e1_signal_processing.h index 96f0fd175..5ce893638 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.h +++ b/src/algorithms/libs/galileo_e1_signal_processing.h @@ -34,6 +34,11 @@ #include +/*! + * \brief This function generates Galileo E1 code (can select E1B or E1C sinboc). + * + */ +void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], unsigned int _prn); /*! * \brief This function generates Galileo E1 code (can select E1B or E1C, cboc or sinboc diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index 6ad7b940d..daf6e898a 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -63,7 +63,7 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( float very_early_late_space_chips; float early_late_space_narrow_chips; float very_early_late_space_narrow_chips; - unified_ = configuration->property(role + ".unified", true); + unified_ = configuration->property(role + ".unified", false); item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index 4f3fc3e53..b14c0dd51 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -63,7 +63,7 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); dump = configuration->property(role + ".dump", false); - unified_ = configuration->property(role + ".unified", true); + unified_ = configuration->property(role + ".unified", false); pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 2e469bb6d..e24fea7fb 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -208,9 +208,16 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_correlation_length_ms = 4; d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip d_veml = true; - d_secondary = true; - d_secondary_code_length = static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); - d_secondary_code_string = const_cast(&Galileo_E1_C_SECONDARY_CODE); + if (d_track_pilot) + { + d_secondary = true; + d_secondary_code_length = static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); + d_secondary_code_string = const_cast(&Galileo_E1_C_SECONDARY_CODE); + } + else + { + d_secondary = false; + } interchange_iq = false; // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. } else if (signal_type.compare("5X") == 0) @@ -273,7 +280,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // Initialization of local code replica // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip d_tracking_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); - std::fill_n(d_tracking_code, 2 * d_code_length_chips, 0.0); // correlator outputs (scalar) if (d_veml) { @@ -339,7 +345,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_Prompt_Data[0] = gr_complex(0.0, 0.0); d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); - std::fill_n(d_data_code, 2 * d_code_length_chips, 0.0); } //--- Initializations ---// @@ -457,29 +462,14 @@ void dll_pll_veml_tracking::start_tracking() if (d_track_pilot) { char pilot_signal[3] = "1C"; - galileo_e1_code_gen_float_sampled(d_tracking_code, - pilot_signal, - false, //CBOC disabled - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - galileo_e1_code_gen_float_sampled(d_data_code, - d_acquisition_gnss_synchro->Signal, - false, //CBOC disabled - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); + galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(d_data_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); d_Prompt_Data[0] = gr_complex(0.0, 0.0); correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code, d_prompt_data_shift); } else { - galileo_e1_code_gen_float_sampled(d_tracking_code, - d_acquisition_gnss_synchro->Signal, - false, //CBOC disabled - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); + galileo_e1_code_gen_sinboc11_float(d_tracking_code, d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->PRN); } } else if (systemName.compare("Galileo") == 0 and signal_type.compare("5X") == 0) @@ -693,8 +683,8 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, d_carrier_phase_step_rad, - d_rem_code_phase_chips * static_cast(d_code_samples_per_chip), - d_code_phase_step_chips * static_cast(d_code_samples_per_chip), + static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), d_vector_length); // DATA CORRELATOR (if tracking tracks the pilot signal) @@ -704,8 +694,8 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( d_rem_carr_phase_rad, d_carrier_phase_step_rad, - d_rem_code_phase_chips * static_cast(d_code_samples_per_chip), - d_code_phase_step_chips * static_cast(d_code_samples_per_chip), + static_cast(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), d_vector_length); } } @@ -847,13 +837,29 @@ void dll_pll_veml_tracking::log_data() double tmp_double; if (d_track_pilot) { - prompt_I = d_Prompt_Data->real(); - prompt_Q = d_Prompt_Data->imag(); + if (interchange_iq) + { + prompt_I = d_Prompt_Data->imag(); + prompt_Q = d_Prompt_Data->real(); + } + else + { + prompt_I = d_Prompt_Data->real(); + prompt_Q = d_Prompt_Data->imag(); + } } else { - prompt_I = d_Prompt->real(); - prompt_Q = d_Prompt->imag(); + if (interchange_iq) + { + prompt_I = d_Prompt->imag(); + prompt_Q = d_Prompt->real(); + } + else + { + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); + } } if (d_veml) { @@ -991,6 +997,12 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) if (d_Prompt_buffer_deque.size() == d_secondary_code_length) { next_state = acquire_secondary(); + if (next_state) + { + std::cout << "Secondary code locked for CH " << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + d_Prompt_buffer_deque.pop_front(); } } @@ -1023,21 +1035,24 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_last_prompt = gr_complex(0.0, 0.0); d_Prompt_buffer_deque.clear(); d_current_symbol = 0; - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); // Set narrow taps delay values [chips] - if (d_veml) + if (d_secondary) { - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - else - { - d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } } // UPDATE INTEGRATION TIME @@ -1075,14 +1090,31 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) // ########### Output the tracking results to Telemetry block ########## if (interchange_iq) { - //Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } } else { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } } current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; @@ -1131,14 +1163,31 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) // ########### Output the tracking results to Telemetry block ########## if (interchange_iq) { - //Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } } else { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + if (d_track_pilot) + { + //Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } } current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; From f2fe8e9d0d492d6d8e99282727f6b187d842582d Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 16 Mar 2018 17:40:24 +0100 Subject: [PATCH 07/14] Extended correlator GPS L1 --- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 20 +++--- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 65 +++++++++++-------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 2 +- 3 files changed, 49 insertions(+), 38 deletions(-) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index b14c0dd51..e642b29a1 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -56,19 +56,19 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( std::string dump_filename; std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; - float early_late_space_chips; item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); dump = configuration->property(role + ".dump", false); unified_ = configuration->property(role + ".unified", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 20.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); std::string default_dump_filename = "./track_ch"; dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); @@ -88,12 +88,12 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( dump_filename, pll_bw_hz, dll_bw_hz, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips, - early_late_space_chips, + pll_bw_narrow_hz, + dll_bw_narrow_hz, early_late_space_chips, early_late_space_chips, + early_late_space_narrow_chips, + early_late_space_narrow_chips, symbols_extended_correlator, false, 'G', sig_); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index e24fea7fb..00c01ff95 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -126,6 +126,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // initialize internal vars d_dump = dump; d_veml = false; + d_synchonizing = false; d_track_pilot = track_pilot; d_fs_in = fs_in; d_vector_length = vector_length; @@ -533,6 +534,7 @@ void dll_pll_veml_tracking::start_tracking() // enable tracking pull-in d_state = 1; + d_synchonizing = false; d_Prompt_buffer_deque.clear(); d_last_prompt = gr_complex(0.0, 0.0); LOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz @@ -1008,22 +1010,34 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } else //Signal does not have secondary code. Search a bit transition by sign change { - if (d_current_symbol == (d_symbols_per_bit - 1)) + if (d_synchonizing) { - next_state = true; - } - else if (d_current_symbol > 0) - { - d_current_symbol++; + if (d_Prompt->real() * d_last_prompt.real() > 0.0) + { + d_current_symbol++; + } + else if (d_current_symbol > d_symbols_per_bit) + { + d_synchonizing = false; + d_current_symbol = 1; + } + else + { + d_current_symbol = 1; + d_last_prompt = *d_Prompt; + } } else if (d_last_prompt.real() != 0.0) { - if (d_Prompt->real() * d_last_prompt.real() < 0.0) - { - d_current_symbol = 1; - } + d_current_symbol++; + if (d_current_symbol == d_symbols_per_bit) next_state = true; + } + else + { + d_last_prompt = *d_Prompt; + d_synchonizing = true; + d_current_symbol = 1; } - d_last_prompt = *d_Prompt; } if (next_state) { // reset extended correlator @@ -1035,24 +1049,21 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_last_prompt = gr_complex(0.0, 0.0); d_Prompt_buffer_deque.clear(); d_current_symbol = 0; - + d_synchonizing = false; // Set narrow taps delay values [chips] - if (d_secondary) + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + if (d_veml) { - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - if (d_veml) - { - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - else - { - d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } + d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); } // UPDATE INTEGRATION TIME diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index a91d041f8..cf17cdf56 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -143,7 +143,7 @@ private: //tracking state machine int d_state; - + bool d_synchonizing; //Integration period in samples int d_correlation_length_ms; int d_n_correlator_taps; From 59a991b884d2fb79731c1e966a5e4ad7de62d2fe Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 19 Mar 2018 11:19:55 +0100 Subject: [PATCH 08/14] Add adaptive Cloop-4quadrant discriminator --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 44 +++-- .../gnuradio_blocks/dll_pll_veml_tracking.h | 5 +- src/utils/matlab/dll_pll_veml_plot_sample.m | 84 ++++++++++ .../libs/dll_pll_veml_read_tracking_dump.m | 153 ++++++++++++++++++ src/utils/matlab/libs/plotVEMLTracking.m | 24 +-- 5 files changed, 282 insertions(+), 28 deletions(-) create mode 100644 src/utils/matlab/dll_pll_veml_plot_sample.m create mode 100644 src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 00c01ff95..9a0c97c18 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -126,6 +126,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // initialize internal vars d_dump = dump; d_veml = false; + d_cloop = true; d_synchonizing = false; d_track_pilot = track_pilot; d_fs_in = fs_in; @@ -535,6 +536,7 @@ void dll_pll_veml_tracking::start_tracking() // enable tracking pull-in d_state = 1; d_synchonizing = false; + d_cloop = true; d_Prompt_buffer_deque.clear(); d_last_prompt = gr_complex(0.0, 0.0); LOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_hz @@ -703,11 +705,11 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples) } -void dll_pll_veml_tracking::run_dll_pll(bool enable_costas_loop) +void dll_pll_veml_tracking::run_dll_pll() { // ################## PLL ########################################################## // PLL discriminator - if (enable_costas_loop) + if (d_cloop) { // Costas loop discriminator, insensitive to 180 deg phase transitions d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2; @@ -823,11 +825,18 @@ void dll_pll_veml_tracking::save_correlation_results() d_E_accu += *d_Early; d_P_accu += *d_Prompt; d_L_accu += *d_Late; + d_current_symbol++; + d_current_symbol %= d_symbols_per_bit; } + // If tracking pilot, disable Costas loop + if (d_track_pilot) + d_cloop = false; + else + d_cloop = true; } -void dll_pll_veml_tracking::log_data() +void dll_pll_veml_tracking::log_data(bool integrating) { if (d_dump) { @@ -876,6 +885,16 @@ void dll_pll_veml_tracking::log_data() tmp_E = std::abs(d_E_accu); tmp_P = std::abs(d_P_accu); tmp_L = std::abs(d_L_accu); + if (integrating) + { + // It compensates the amplitude difference while integrating + float scale_factor = static_cast(d_extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); + tmp_VE *= scale_factor; + tmp_E *= scale_factor; + tmp_P *= scale_factor; + tmp_L *= scale_factor; + tmp_VL *= scale_factor; + } try { @@ -987,11 +1006,11 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { bool next_state = false; // Perform DLL/PLL tracking loop computations. Costas Loop enabled - run_dll_pll(true); + run_dll_pll(); update_tracking_vars(); // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); + log_data(false); if (d_secondary) { // ####### SECONDARY CODE LOCK ##### @@ -1139,6 +1158,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_extend_correlation_symbols_count = 0; d_state = 4; } + log_data(true); break; } case 4: // narrow tracking @@ -1158,17 +1178,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } else { - run_dll_pll(!d_secondary); - //EQUIVALENT TO THE LINE ABOVE if (d_secondary) - //{ - // //If secondary code is locked, disable Costas Loop - // run_dll_pll(false); - //} - //else - //{ - // //The signal does not have secondary code, enable Costas Loop - // run_dll_pll(true); - //} + run_dll_pll(); update_tracking_vars(); // ########### Output the tracking results to Telemetry block ########## @@ -1207,7 +1217,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = d_correlation_length_ms; // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); + log_data(false); // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); d_E_accu = gr_complex(0.0, 0.0); diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index cf17cdf56..6d3ddef62 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -112,16 +112,17 @@ private: bool cn0_and_tracking_lock_status(); bool acquire_secondary(); void do_correlation_step(const gr_complex *input_samples); - void run_dll_pll(bool disable_costas_loop); + void run_dll_pll(); void update_tracking_vars(); void clear_tracking_vars(); void save_correlation_results(); - void log_data(); + void log_data(bool integrating); int save_matfile(); // tracking configuration vars bool d_dump; bool d_veml; + bool d_cloop; unsigned int d_vector_length; unsigned int d_channel; double d_fs_in; diff --git a/src/utils/matlab/dll_pll_veml_plot_sample.m b/src/utils/matlab/dll_pll_veml_plot_sample.m new file mode 100644 index 000000000..15b86f098 --- /dev/null +++ b/src/utils/matlab/dll_pll_veml_plot_sample.m @@ -0,0 +1,84 @@ +% /*! +% * \file dll_pll_vml_plot_sample.m +% * \brief Read GNSS-SDR Tracking dump binary file using the provided +% function and plot some internal variables +% * \author Javier Arribas, 2011. jarribas(at)cttc.es +% * \author Antonio Ramos, 2018. antonio.ramos(at)cttc.es +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) +% * +% * GNSS-SDR is a software defined Global Navigation +% * Satellite Systems receiver +% * +% * This file is part of GNSS-SDR. +% * +% * GNSS-SDR is free software: you can redistribute it and/or modify +% * it under the terms of the GNU General Public License as published by +% * the Free Software Foundation, either version 3 of the License, or +% * at your option) any later version. +% * +% * GNSS-SDR is distributed in the hope that it will be useful, +% * but WITHOUT ANY WARRANTY; without even the implied warranty of +% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% * GNU General Public License for more details. +% * +% * You should have received a copy of the GNU General Public License +% * along with GNSS-SDR. If not, see . +% * +% * ------------------------------------------------------------------------- +% */ +close all; +clear all; + +if ~exist('dll_pll_veml_read_tracking_dump.m','file') + addpath('./libs') +end + +samplingFreq = 5000000; %[Hz] +coherent_integration_time_ms = 20; %[ms] +channels = 5; % Number of channels +first_channel = 0; % Number of the first channel + +path = '/dump_dir/'; %% CHANGE THIS PATH + +for N=1:1:channels + tracking_log_path = [path 'track_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch BY YOUR dump_filename + GNSS_tracking(N)= dll_pll_veml_read_tracking_dump(tracking_log_path); +end + +% GNSS-SDR format conversion to MATLAB GPS receiver + +for N=1:1:channels + trackResults(N).status = 'T'; %fake track + trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.'; + trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.'; + trackResults(N).dllDiscr = GNSS_tracking(N).code_error.'; + trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.'; + trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.'; + trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.'; + + trackResults(N).I_P = GNSS_tracking(N).P.'; + trackResults(N).Q_P = zeros(1,length(GNSS_tracking(N).P)); + + trackResults(N).I_VE = GNSS_tracking(N).VE.'; + trackResults(N).I_E = GNSS_tracking(N).E.'; + trackResults(N).I_L = GNSS_tracking(N).L.'; + trackResults(N).I_VL = GNSS_tracking(N).VL.'; + trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE)); + trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E)); + trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L)); + trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL)); + trackResults(N).data_I = GNSS_tracking(N).prompt_I.'; + trackResults(N).data_Q = GNSS_tracking(N).prompt_Q.'; + trackResults(N).PRN = GNSS_tracking(N).PRN.'; + trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.'; + + % Use original MATLAB tracking plot function + settings.numberOfChannels = channels; + settings.msToProcess = length(GNSS_tracking(N).E)*coherent_integration_time_ms; + plotVEMLTracking(N,trackResults,settings) +end + + + diff --git a/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m new file mode 100644 index 000000000..daa2324dd --- /dev/null +++ b/src/utils/matlab/libs/dll_pll_veml_read_tracking_dump.m @@ -0,0 +1,153 @@ +% /*! +% * \file dll_pll_veml_read_tracking_dump.m +% * \brief Read GNSS-SDR Tracking dump binary file into MATLAB. +% * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com +% * ------------------------------------------------------------------------- +% * +% * Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) +% * +% * GNSS-SDR is a software defined Global Navigation +% * Satellite Systems receiver +% * +% * This file is part of GNSS-SDR. +% * +% * GNSS-SDR is free software: you can redistribute it and/or modify +% * it under the terms of the GNU General Public License as published by +% * the Free Software Foundation, either version 3 of the License, or +% * at your option) any later version. +% * +% * GNSS-SDR is distributed in the hope that it will be useful, +% * but WITHOUT ANY WARRANTY; without even the implied warranty of +% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% * GNU General Public License for more details. +% * +% * You should have received a copy of the GNU General Public License +% * along with GNSS-SDR. If not, see . +% * +% * ------------------------------------------------------------------------- +% */ + +function [GNSS_tracking] = dll_pll_veml_read_tracking_dump (filename, count) + %% usage: dll_pll_veml_read_tracking_dump (filename, [count]) + %% + %% open GNSS-SDR tracking binary log file .dat and return the contents + %% + + m = nargchk (1,2,nargin); + + num_float_vars = 17; + num_unsigned_long_int_vars = 1; + num_double_vars = 1; + num_unsigned_int_vars = 1; + + if(~isempty(strfind(computer('arch'), '64'))) + % 64-bit computer + double_size_bytes = 8; + unsigned_long_int_size_bytes = 8; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; + else + double_size_bytes = 8; + unsigned_long_int_size_bytes = 4; + float_size_bytes = 4; + unsigned_int_size_bytes = 4; + end + + skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ... + double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes; + + bytes_shift = 0; + + if (m) + usage (m); + end + + if (nargin < 2) + count = Inf; + end + %loops_counter = fread (f, count, 'uint32',4*12); + f = fopen (filename, 'rb'); + if (f < 0) + else + v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes); + bytes_shift = bytes_shift + unsigned_long_int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved float + v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next float + v18 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes); + bytes_shift = bytes_shift + float_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next double + v19 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes); + bytes_shift = bytes_shift + double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next unsigned int + v20 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes); + fclose (f); + + GNSS_tracking.VE = v1; + GNSS_tracking.E = v2; + GNSS_tracking.P = v3; + GNSS_tracking.L = v4; + GNSS_tracking.VL = v5; + GNSS_tracking.prompt_I = v6; + GNSS_tracking.prompt_Q = v7; + GNSS_tracking.PRN_start_sample = v8; + GNSS_tracking.acc_carrier_phase_rad = v9; + GNSS_tracking.carrier_doppler_hz = v10; + GNSS_tracking.code_freq_hz = v11; + GNSS_tracking.carr_error = v12; + GNSS_tracking.carr_nco = v13; + GNSS_tracking.code_error = v14; + GNSS_tracking.code_nco = v15; + GNSS_tracking.CN0_SNV_dB_Hz = v16; + GNSS_tracking.carrier_lock_test = v17; + GNSS_tracking.var1 = v18; + GNSS_tracking.var2 = v19; + GNSS_tracking.PRN = v20; + end + diff --git a/src/utils/matlab/libs/plotVEMLTracking.m b/src/utils/matlab/libs/plotVEMLTracking.m index 8af2225f9..f3846e164 100644 --- a/src/utils/matlab/libs/plotVEMLTracking.m +++ b/src/utils/matlab/libs/plotVEMLTracking.m @@ -69,8 +69,8 @@ for channelNr = channelList timeAxisInSeconds = (1:4:settings.msToProcess)/1000; %----- Discrete-Time Scatter Plot --------------------------------- - plot(handles(1, 1), trackResults(channelNr).I_P,... - trackResults(channelNr).Q_P, ... + plot(handles(1, 1), trackResults(channelNr).data_I,... + trackResults(channelNr).data_Q, ... '.'); grid (handles(1, 1)); @@ -80,8 +80,9 @@ for channelNr = channelList ylabel(handles(1, 1), 'Q prompt'); %----- Nav bits --------------------------------------------------- - plot (handles(1, 2), timeAxisInSeconds, ... - trackResults(channelNr).I_P); + t = (1:length(trackResults(channelNr).data_I)); + plot (handles(1, 2), t, ... + trackResults(channelNr).data_I); grid (handles(1, 2)); title (handles(1, 2), 'Bits of the navigation message'); @@ -89,7 +90,8 @@ for channelNr = channelList axis (handles(1, 2), 'tight'); %----- PLL discriminator unfiltered-------------------------------- - plot (handles(2, 1), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).pllDiscr)); + plot (handles(2, 1), t, ... trackResults(channelNr).pllDiscr, 'r'); grid (handles(2, 1)); @@ -99,7 +101,8 @@ for channelNr = channelList title (handles(2, 1), 'Raw PLL discriminator'); %----- Correlation ------------------------------------------------ - plot(handles(2, 2), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).I_VE)); + plot(handles(2, 2), t, ... [sqrt(trackResults(channelNr).I_VE.^2 + ... trackResults(channelNr).Q_VE.^2)', ... sqrt(trackResults(channelNr).I_E.^2 + ... @@ -127,7 +130,8 @@ for channelNr = channelList set(hLegend, 'Interpreter', 'Latex'); %----- PLL discriminator filtered---------------------------------- - plot (handles(3, 1), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).pllDiscrFilt)); + plot (handles(3, 1), t, ... trackResults(channelNr).pllDiscrFilt, 'b'); grid (handles(3, 1)); @@ -137,7 +141,8 @@ for channelNr = channelList title (handles(3, 1), 'Filtered PLL discriminator'); %----- DLL discriminator unfiltered-------------------------------- - plot (handles(3, 2), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).dllDiscr)); + plot (handles(3, 2), t, ... trackResults(channelNr).dllDiscr, 'r'); grid (handles(3, 2)); @@ -147,7 +152,8 @@ for channelNr = channelList title (handles(3, 2), 'Raw DLL discriminator'); %----- DLL discriminator filtered---------------------------------- - plot (handles(3, 3), timeAxisInSeconds, ... + t = (1:length(trackResults(channelNr).dllDiscrFilt)); + plot (handles(3, 3), t, ... trackResults(channelNr).dllDiscrFilt, 'b'); grid (handles(3, 3)); From 163500623e289055be8986e3b59c64d27535419e Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 19 Mar 2018 15:37:20 +0100 Subject: [PATCH 09/14] Fix compiling error --- src/core/system_parameters/gnss_synchro.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 8f4c4d880..267df4557 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -50,7 +50,7 @@ public: double Acq_delay_samples; //!< Set by Acquisition processing block double Acq_doppler_hz; //!< Set by Acquisition processing block unsigned long int Acq_samplestamp_samples; //!< Set by Acquisition processing block - bool Flag_valid_acquisition = false; //!< Set by Acquisition processing block + bool Flag_valid_acquisition; //!< Set by Acquisition processing block //Tracking long int fs; //!< Set by Tracking processing block double Prompt_I; //!< Set by Tracking processing block @@ -61,17 +61,17 @@ public: double Code_phase_samples; //!< Set by Tracking processing block unsigned long int Tracking_sample_counter; //!< Set by Tracking processing block - bool Flag_valid_symbol_output = false; //!< Set by Tracking processing block - int correlation_length_ms; //!< Set by Tracking processing block + bool Flag_valid_symbol_output; //!< Set by Tracking processing block + int correlation_length_ms; //!< Set by Tracking processing block //Telemetry Decoder - bool Flag_valid_word = false; //!< Set by Telemetry Decoder processing block + bool Flag_valid_word; //!< Set by Telemetry Decoder processing block double TOW_at_current_symbol_s; //!< Set by Telemetry Decoder processing block // Observables - double Pseudorange_m; //!< Set by Observables processing block - double RX_time; //!< Set by Observables processing block - bool Flag_valid_pseudorange = false; //!< Set by Observables processing block + double Pseudorange_m; //!< Set by Observables processing block + double RX_time; //!< Set by Observables processing block + bool Flag_valid_pseudorange; //!< Set by Observables processing block }; #endif From f577bf3cea70080b8389add3cfb437ce2bc080d6 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Mar 2018 14:44:34 +0100 Subject: [PATCH 10/14] Fix GPS L1 CA unit test --- .../libs/tracking_dump_reader.cc | 28 +++++++++---------- .../libs/tracking_dump_reader.h | 24 ++++++++-------- .../gps_l1_ca_dll_pll_tracking_test.cc | 4 +++ 3 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc index b8f259501..0f8dd9dc3 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc @@ -35,24 +35,24 @@ bool tracking_dump_reader::read_binary_obs() { try { + d_dump_file.read(reinterpret_cast(&abs_VE), sizeof(float)); d_dump_file.read(reinterpret_cast(&abs_E), sizeof(float)); d_dump_file.read(reinterpret_cast(&abs_P), sizeof(float)); d_dump_file.read(reinterpret_cast(&abs_L), sizeof(float)); + d_dump_file.read(reinterpret_cast(&abs_VL), sizeof(float)); d_dump_file.read(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.read(reinterpret_cast(&prompt_Q), sizeof(float)); - d_dump_file.read(reinterpret_cast(&PRN_start_sample_count), sizeof(unsigned long int)); - - d_dump_file.read(reinterpret_cast(&acc_carrier_phase_rad), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carrier_doppler_hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&code_freq_chips), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.read(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - d_dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.read(reinterpret_cast(&carrier_lock_test), sizeof(double)); - d_dump_file.read(reinterpret_cast(&aux1), sizeof(double)); + d_dump_file.read(reinterpret_cast(&acc_carrier_phase_rad), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carrier_doppler_hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&code_freq_chips), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carr_error_hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&code_error_chips), sizeof(float)); + d_dump_file.read(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + d_dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz), sizeof(float)); + d_dump_file.read(reinterpret_cast(&carrier_lock_test), sizeof(float)); + d_dump_file.read(reinterpret_cast(&aux1), sizeof(float)); d_dump_file.read(reinterpret_cast(&aux2), sizeof(double)); d_dump_file.read(reinterpret_cast(&PRN), sizeof(unsigned int)); } @@ -82,8 +82,8 @@ bool tracking_dump_reader::restart() long int tracking_dump_reader::num_epochs() { std::ifstream::pos_type size; - int number_of_double_vars = 11; - int number_of_float_vars = 5; + int number_of_double_vars = 1; + int number_of_float_vars = 17; int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars + sizeof(float) * number_of_float_vars + sizeof(unsigned int); std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h index 2a56fdd1c..e36dec1c8 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h @@ -45,10 +45,12 @@ public: bool open_obs_file(std::string out_file); //tracking dump variables - // EPR + // VEPLVL + float abs_VE; float abs_E; float abs_P; float abs_L; + float abs_VL; // PROMPT I and Q (to analyze navigation symbols) float prompt_I; float prompt_Q; @@ -56,26 +58,26 @@ public: unsigned long int PRN_start_sample_count; // accumulated carrier phase - double acc_carrier_phase_rad; + float acc_carrier_phase_rad; // carrier and code frequency - double carrier_doppler_hz; - double code_freq_chips; + float carrier_doppler_hz; + float code_freq_chips; // PLL commands - double carr_error_hz; - double carr_error_filt_hz; + float carr_error_hz; + float carr_error_filt_hz; // DLL commands - double code_error_chips; - double code_error_filt_chips; + float code_error_chips; + float code_error_filt_chips; // CN0 and carrier lock test - double CN0_SNV_dB_Hz; - double carrier_lock_test; + float CN0_SNV_dB_Hz; + float carrier_lock_test; // AUX vars (for debug purposes) - double aux1; + float aux1; double aux2; unsigned int PRN; diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc index d0ff41697..7ed7741da 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc @@ -225,6 +225,10 @@ void GpsL1CADllPllTrackingTest::configure_receiver() config->set_property("Tracking_1C.pll_bw_hz", "20.0"); config->set_property("Tracking_1C.dll_bw_hz", "2.0"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); + config->set_property("Tracking_1C.pll_bw_narrow_hz", "20.0"); + config->set_property("Tracking_1C.dll_bw_narrow_hz", "2.0"); + config->set_property("Tracking_1C.early_late_space_narrow_chips", "0.5"); + config->set_property("Tracking_1C.unified", "true"); config->set_property("Tracking_1C.extend_correlation_ms", "1"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); From bd0222f4b70e3bdd34b5124f4f6ff350f6088fbf Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 27 Mar 2018 11:01:23 +0200 Subject: [PATCH 11/14] Fix defects detected by Coverity Scan and fix building in MacOs --- .../galileo_e1_dll_pll_veml_tracking.cc | 2 +- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 2 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 18 ++++++++++++++++-- 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index daf6e898a..437f860ce 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -91,10 +91,10 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { + item_size_ = sizeof(gr_complex); if (unified_) { char sig_[3] = "1B"; - item_size_ = sizeof(gr_complex); tracking_unified_ = dll_pll_veml_make_tracking( fs_in, vector_length, diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index e642b29a1..ec5dbcf98 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -77,10 +77,10 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { + item_size_ = sizeof(gr_complex); if (unified_) { char sig_[3] = "1C"; - item_size_ = sizeof(gr_complex); tracking_unified_ = dll_pll_veml_make_tracking( fs_in, vector_length, diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 9a0c97c18..fd4bb389d 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -254,6 +254,14 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( { LOG(WARNING) << "Invalid System argument when instantiating tracking blocks"; std::cout << "Invalid System argument when instantiating tracking blocks" << std::endl; + d_correlation_length_ms = 1; + d_secondary = false; + interchange_iq = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0; + d_symbols_per_bit = 0; } T_chip_seconds = 0.0; T_prn_seconds = 0.0; @@ -348,6 +356,11 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_Prompt_Data[0] = gr_complex(0.0, 0.0); d_data_code = static_cast(volk_gnsssdr_malloc(2 * d_code_length_chips * sizeof(float), volk_gnsssdr_get_alignment())); } + else + { + d_Prompt_Data = nullptr; + d_data_code = nullptr; + } //--- Initializations ---// // Initial code frequency basis of NCO @@ -476,7 +489,7 @@ void dll_pll_veml_tracking::start_tracking() } else if (systemName.compare("Galileo") == 0 and signal_type.compare("5X") == 0) { - gr_complex aux_code[d_code_length_chips]; + gr_complex *aux_code = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex) * d_code_length_chips, volk_gnsssdr_get_alignment())); galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, const_cast(signal_type.c_str())); if (d_track_pilot) { @@ -496,6 +509,7 @@ void dll_pll_veml_tracking::start_tracking() d_tracking_code[i] = aux_code[i].real(); } } + volk_gnsssdr_free(aux_code); } multicorrelator_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_tracking_code, d_local_code_shift_chips); @@ -761,7 +775,7 @@ void dll_pll_veml_tracking::update_tracking_vars() T_prn_seconds = T_chip_seconds * static_cast(d_code_length_chips); double code_error_filt_secs = T_prn_seconds * d_code_error_filt_chips * T_chip_seconds; //[seconds] - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // ################## CARRIER AND CODE NCO BUFFER ALIGNMENT ####################### // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation T_prn_samples = T_prn_seconds * d_fs_in; From 213adad39246803f7c3dd7d04f8b4f0a2807a82d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 28 Mar 2018 18:29:22 +0200 Subject: [PATCH 12/14] Improve carrier phase initialization --- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index fd4bb389d..b7db0f811 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -428,13 +428,13 @@ void dll_pll_veml_tracking::start_tracking() double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); double T_prn_mod_samples = T_prn_mod_seconds * d_fs_in; - d_current_prn_length_samples = round(T_prn_mod_samples); + d_current_prn_length_samples = std::round(T_prn_mod_samples); double T_prn_true_seconds = static_cast(d_code_length_chips) / d_code_chip_rate; double T_prn_true_samples = T_prn_true_seconds * d_fs_in; double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; - double corrected_acq_phase_samples = fmod(d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in, T_prn_true_samples); + double corrected_acq_phase_samples = std::fmod(d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in, T_prn_true_samples); if (corrected_acq_phase_samples < 0.0) { corrected_acq_phase_samples += T_prn_mod_samples; @@ -984,14 +984,14 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { // Signal alignment (skip samples until the incoming signal is aligned with local replica) // Fill the acquisition data - int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + unsigned long int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; double acq_trk_shif_correction_samples = static_cast(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - int samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + int samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); if (samples_offset < 0) { samples_offset = 0; } - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_acq_code_phase_samples; d_state = 2; d_sample_counter += samples_offset; // count for the processed samples consume_each(samples_offset); // shift input to perform alignment with local replica From b3764f6ed1f595d71531a227d86b3f0103186bb5 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 28 Mar 2018 19:54:24 +0200 Subject: [PATCH 13/14] Make use of flags with default values instead of defines --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 37 +++++++++---------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 12 ++---- 2 files changed, 20 insertions(+), 29 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index b7db0f811..c5ec7036a 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -1,8 +1,7 @@ /*! * \file dll_pll_veml_tracking.cc - * \brief Implementation of a code DLL + carrier PLL VEML (Very Early - * Minus Late) tracking block for Galileo E1 signals - * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com + * \brief Implementation of a code DLL + carrier PLL tracking block. + * \author Antonio Ramos, 2018 antonioramosdet(at)gmail.com * * Code DLL + carrier PLL according to the algorithms described in: * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, @@ -11,7 +10,7 @@ * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -39,7 +38,6 @@ #include "lock_detectors.h" #include "control_message_factory.h" #include "MATH_CONSTANTS.h" - #include "Galileo_E1.h" #include "galileo_e1_signal_processing.h" #include "Galileo_E5a.h" @@ -50,17 +48,16 @@ #include "gps_l2c_signal.h" #include "GPS_L5.h" #include "gps_l5_signal.h" - +#include "gnss_sdr_flags.h" +#include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include - using google::LogMessage; @@ -378,11 +375,11 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[DLL_PLL_CN0_ESTIMATION_SAMPLES]; + d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = DLL_PLL_CARRIER_LOCK_THRESHOLD; + d_carrier_lock_threshold = FLAGS_carrier_lock_th; clear_tracking_vars(); @@ -649,7 +646,7 @@ bool dll_pll_veml_tracking::acquire_secondary() bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < DLL_PLL_CN0_ESTIMATION_SAMPLES) + if (d_cn0_estimation_counter < FLAGS_cn0_samples) { // fill buffer with prompt correlator output values d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; @@ -660,11 +657,11 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, static_cast(d_fs_in), static_cast(d_code_length_chips)); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, static_cast(d_fs_in), static_cast(d_code_length_chips)); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < DLL_PLL_MINIMUM_VALID_CN0) + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { d_carrier_lock_fail_counter++; } @@ -672,7 +669,7 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; } - if (d_carrier_lock_fail_counter > DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER) + if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 6d3ddef62..76d4c5fda 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -1,8 +1,7 @@ /*! * \file dll_pll_veml_tracking.h - * \brief Implementation of a code DLL + carrier PLL VEML (Very Early - * Minus Late) tracking block for Galileo E1 signals - * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com + * \brief Implementation of a code DLL + carrier PLL tracking block. + * \author Antonio Ramos, 2018 antonioramosdet(at)gmail.com * * ------------------------------------------------------------------------- * @@ -32,18 +31,13 @@ #ifndef GNSS_SDR_DLL_PLL_VEML_TRACKING_H #define GNSS_SDR_DLL_PLL_VEML_TRACKING_H -#define DLL_PLL_CN0_ESTIMATION_SAMPLES 20 -#define DLL_PLL_MINIMUM_VALID_CN0 25 -#define DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER 50 -#define DLL_PLL_CARRIER_LOCK_THRESHOLD 0.85 - #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_PLL_filter.h" #include "cpu_multicorrelator_real_codes.h" +#include #include #include -#include class dll_pll_veml_tracking; From 539e24f0ac5c89af86d129cce58b3880c8b42240 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 29 Mar 2018 09:23:23 +0200 Subject: [PATCH 14/14] Minor fixes --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 615 +++++++++--------- .../gnuradio_blocks/dll_pll_veml_tracking.h | 17 +- 2 files changed, 313 insertions(+), 319 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index c5ec7036a..88350e361 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -1,7 +1,7 @@ /*! * \file dll_pll_veml_tracking.cc * \brief Implementation of a code DLL + carrier PLL tracking block. - * \author Antonio Ramos, 2018 antonioramosdet(at)gmail.com + * \author Antonio Ramos, 2018 antonio.ramosdet(at)gmail.com * * Code DLL + carrier PLL according to the algorithms described in: * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, @@ -112,8 +112,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( float pll_bw_narrow_hz, float dll_bw_narrow_hz, float early_late_space_chips, float very_early_late_space_chips, float early_late_space_narrow_chips, float very_early_late_space_narrow_chips, - int extend_correlation_symbols, bool track_pilot, char system, char signal[3]) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + int extend_correlation_symbols, bool track_pilot, char system, + char signal[3]) : gr::block("dll_pll_veml_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -145,7 +146,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); - //GPS L1 C/A does not have pilot component nor secondary code + // GPS L1 C/A does not have pilot component nor secondary code d_secondary = false; d_track_pilot = false; interchange_iq = false; @@ -159,7 +160,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; d_correlation_length_ms = 20; d_code_samples_per_chip = 1; - //GPS L2 does not have pilot component nor secondary code + // GPS L2 does not have pilot component nor secondary code d_secondary = false; d_track_pilot = false; interchange_iq = false; @@ -173,7 +174,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( d_correlation_length_ms = 1; d_code_samples_per_chip = 1; d_code_length_chips = static_cast(GPS_L5i_CODE_LENGTH_CHIPS); - //GPS L5 does not have pilot secondary code + // GPS L5 does not have pilot secondary code d_secondary = true; if (d_track_pilot) { @@ -191,7 +192,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( else { LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; - std::cout << "Invalid Signal argument when instantiating tracking blocks" << std::endl; + std::cerr << "Invalid Signal argument when instantiating tracking blocks" << std::endl; } } else if (system == 'E') @@ -250,7 +251,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( else { LOG(WARNING) << "Invalid System argument when instantiating tracking blocks"; - std::cout << "Invalid System argument when instantiating tracking blocks" << std::endl; + std::cerr << "Invalid System argument when instantiating tracking blocks" << std::endl; d_correlation_length_ms = 1; d_secondary = false; interchange_iq = false; @@ -411,12 +412,11 @@ void dll_pll_veml_tracking::start_tracking() d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; - long int acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + long int acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_fs_in; DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; DLOG(INFO) << "Number of seconds between Acquisition and Tracking = " << acq_trk_diff_seconds; - // Doppler effect - // Fd=(C/(C+Vr))*F + // Doppler effect Fd = (C / (C + Vr)) * F double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; // new chip and prn sequence periods based on acq Doppler d_code_freq_chips = radial_velocity * d_code_chip_rate; @@ -604,7 +604,7 @@ dll_pll_veml_tracking::~dll_pll_veml_tracking() bool dll_pll_veml_tracking::acquire_secondary() { - //******* preamble correlation ******** + // ******* preamble correlation ******** int corr_value = 0; for (unsigned int i = 0; i < d_secondary_code_length; i++) { @@ -766,6 +766,7 @@ void dll_pll_veml_tracking::clear_tracking_vars() d_last_prompt = gr_complex(0.0, 0.0); } + void dll_pll_veml_tracking::update_tracking_vars() { T_chip_seconds = 1.0 / d_code_freq_chips; @@ -796,6 +797,7 @@ void dll_pll_veml_tracking::update_tracking_vars() d_rem_code_phase_chips = d_code_freq_chips * d_rem_code_phase_samples / d_fs_in; } + void dll_pll_veml_tracking::save_correlation_results() { if (d_secondary) @@ -960,301 +962,6 @@ void dll_pll_veml_tracking::log_data(bool integrating) } -int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - gr::thread::scoped_lock l(d_setlock); - const gr_complex *in = reinterpret_cast(input_items[0]); - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); - Gnss_Synchro current_synchro_data = Gnss_Synchro(); - - switch (d_state) - { - case 0: // Standby - Pass Through. Full throttle - { - d_sample_counter += ninput_items[0]; - consume_each(ninput_items[0]); - return 0; - break; - } - case 1: // Pull-in - { - // Signal alignment (skip samples until the incoming signal is aligned with local replica) - // Fill the acquisition data - unsigned long int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - double acq_trk_shif_correction_samples = static_cast(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - int samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - if (samples_offset < 0) - { - samples_offset = 0; - } - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_acq_code_phase_samples; - d_state = 2; - d_sample_counter += samples_offset; // count for the processed samples - consume_each(samples_offset); // shift input to perform alignment with local replica - return 0; - } - case 2: // Wide tracking and symbol synchronization - { - do_correlation_step(in); - // Save single correlation step variables - if (d_veml) - { - d_VE_accu = *d_Very_Early; - d_VL_accu = *d_Very_Late; - } - d_E_accu = *d_Early; - d_P_accu = *d_Prompt; - d_L_accu = *d_Late; - - // Check lock status - if (!cn0_and_tracking_lock_status()) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - bool next_state = false; - // Perform DLL/PLL tracking loop computations. Costas Loop enabled - run_dll_pll(); - update_tracking_vars(); - - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); - if (d_secondary) - { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_buffer_deque.push_back(*d_Prompt); - if (d_Prompt_buffer_deque.size() == d_secondary_code_length) - { - next_state = acquire_secondary(); - if (next_state) - { - std::cout << "Secondary code locked for CH " << d_channel - << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - - d_Prompt_buffer_deque.pop_front(); - } - } - else //Signal does not have secondary code. Search a bit transition by sign change - { - if (d_synchonizing) - { - if (d_Prompt->real() * d_last_prompt.real() > 0.0) - { - d_current_symbol++; - } - else if (d_current_symbol > d_symbols_per_bit) - { - d_synchonizing = false; - d_current_symbol = 1; - } - else - { - d_current_symbol = 1; - d_last_prompt = *d_Prompt; - } - } - else if (d_last_prompt.real() != 0.0) - { - d_current_symbol++; - if (d_current_symbol == d_symbols_per_bit) next_state = true; - } - else - { - d_last_prompt = *d_Prompt; - d_synchonizing = true; - d_current_symbol = 1; - } - } - if (next_state) - { // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - d_last_prompt = gr_complex(0.0, 0.0); - d_Prompt_buffer_deque.clear(); - d_current_symbol = 0; - d_synchonizing = false; - // Set narrow taps delay values [chips] - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - if (d_veml) - { - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - else - { - d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - - // UPDATE INTEGRATION TIME - if (d_enable_extended_integration) - { - d_extend_correlation_symbols_count = 0; - float new_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); - d_carrier_loop_filter.set_pdi(new_correlation_time_s); - d_code_loop_filter.set_pdi(new_correlation_time_s); - d_state = 3; // next state is the extended correlator integrator - LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); - std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; - } - else - { - d_state = 4; - } - } - } - break; - } - case 3: // coherent integration (correlation time extension) - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // perform a correlation step - do_correlation_step(in); - update_tracking_vars(); - save_correlation_results(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (d_track_pilot) - { - //Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (d_track_pilot) - { - //Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count == (d_extend_correlation_symbols - 1)) - { - d_extend_correlation_symbols_count = 0; - d_state = 4; - } - log_data(true); - break; - } - case 4: // narrow tracking - { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - - // perform a correlation step - do_correlation_step(in); - save_correlation_results(); - - // check lock status - if (!cn0_and_tracking_lock_status()) - { - clear_tracking_vars(); - d_state = 0; // loss-of-lock detected - } - else - { - run_dll_pll(); - update_tracking_vars(); - - // ########### Output the tracking results to Telemetry block ########## - if (interchange_iq) - { - if (d_track_pilot) - { - //Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); - } - } - else - { - if (d_track_pilot) - { - //Note that data and pilot components are in quadrature. I and Q are interchanged - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - } - else - { - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - } - } - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = d_correlation_length_ms; - // enable write dump file this cycle (valid DLL/PLL cycle) - log_data(false); - // reset extended correlator - d_VE_accu = gr_complex(0.0, 0.0); - d_E_accu = gr_complex(0.0, 0.0); - d_P_accu = gr_complex(0.0, 0.0); - d_L_accu = gr_complex(0.0, 0.0); - d_VL_accu = gr_complex(0.0, 0.0); - if (d_enable_extended_integration) - { - d_state = 3; //new coherent integration (correlation time extension) cycle - } - } - } - } - consume_each(d_current_prn_length_samples); - d_sample_counter += d_current_prn_length_samples; - if (current_synchro_data.Flag_valid_symbol_output) - { - current_synchro_data.fs = static_cast(d_fs_in); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - *out[0] = current_synchro_data; - return 1; - } - return 0; -} - - int dll_pll_veml_tracking::save_matfile() { // READ DUMP FILE @@ -1510,3 +1217,297 @@ void dll_pll_veml_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) gr::thread::scoped_lock l(d_setlock); d_acquisition_gnss_synchro = p_gnss_synchro; } + + +int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + gr::thread::scoped_lock l(d_setlock); + const gr_complex *in = reinterpret_cast(input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + + switch (d_state) + { + case 0: // Standby - Consume samples at full throttle, do nothing + { + d_sample_counter += ninput_items[0]; + consume_each(ninput_items[0]); + return 0; + break; + } + case 1: // Pull-in + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + unsigned long int acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + double acq_trk_shif_correction_samples = static_cast(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + int samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + if (samples_offset < 0) + { + samples_offset = 0; + } + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_acq_code_phase_samples; + d_state = 2; + d_sample_counter += samples_offset; // count for the processed samples + consume_each(samples_offset); // shift input to perform alignment with local replica + return 0; + } + case 2: // Wide tracking and symbol synchronization + { + do_correlation_step(in); + // Save single correlation step variables + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + + // Check lock status + if (!cn0_and_tracking_lock_status()) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + bool next_state = false; + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_dll_pll(); + update_tracking_vars(); + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(false); + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_buffer_deque.push_back(*d_Prompt); + if (d_Prompt_buffer_deque.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + std::cout << "Secondary code locked for CH " << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + + d_Prompt_buffer_deque.pop_front(); + } + } + else // Signal does not have secondary code. Search a bit transition by sign change + { + if (d_synchonizing) + { + if (d_Prompt->real() * d_last_prompt.real() > 0.0) + { + d_current_symbol++; + } + else if (d_current_symbol > d_symbols_per_bit) + { + d_synchonizing = false; + d_current_symbol = 1; + } + else + { + d_current_symbol = 1; + d_last_prompt = *d_Prompt; + } + } + else if (d_last_prompt.real() != 0.0) + { + d_current_symbol++; + if (d_current_symbol == d_symbols_per_bit) next_state = true; + } + else + { + d_last_prompt = *d_Prompt; + d_synchonizing = true; + d_current_symbol = 1; + } + } + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_last_prompt = gr_complex(0.0, 0.0); + d_Prompt_buffer_deque.clear(); + d_current_symbol = 0; + d_synchonizing = false; + // Set narrow taps delay values [chips] + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + + // UPDATE INTEGRATION TIME + if (d_enable_extended_integration) + { + d_extend_correlation_symbols_count = 0; + float new_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); + d_carrier_loop_filter.set_pdi(new_correlation_time_s); + d_code_loop_filter.set_pdi(new_correlation_time_s); + d_state = 3; // next state is the extended correlator integrator + LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + } + else + { + d_state = 4; + } + } + } + break; + } + case 3: // coherent integration (correlation time extension) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // perform a correlation step + do_correlation_step(in); + update_tracking_vars(); + save_correlation_results(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) + { + if (d_track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } + } + else + { + if (d_track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + } + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (d_extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + log_data(true); + break; + } + case 4: // narrow tracking + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + + // perform a correlation step + do_correlation_step(in); + save_correlation_results(); + + // check lock status + if (!cn0_and_tracking_lock_status()) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + run_dll_pll(); + update_tracking_vars(); + + // ########### Output the tracking results to Telemetry block ########## + if (interchange_iq) + { + if (d_track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).real()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).imag()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).real()); + } + } + else + { + if (d_track_pilot) + { + // Note that data and pilot components are in quadrature. I and Q are interchanged + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + } + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(false); + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + if (d_enable_extended_integration) + { + d_state = 3; // new coherent integration (correlation time extension) cycle + } + } + } + } + consume_each(d_current_prn_length_samples); + d_sample_counter += d_current_prn_length_samples; + if (current_synchro_data.Flag_valid_symbol_output) + { + current_synchro_data.fs = static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + *out[0] = current_synchro_data; + return 1; + } + return 0; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 76d4c5fda..1065f44ef 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -1,7 +1,7 @@ /*! * \file dll_pll_veml_tracking.h * \brief Implementation of a code DLL + carrier PLL tracking block. - * \author Antonio Ramos, 2018 antonioramosdet(at)gmail.com + * \author Antonio Ramos, 2018 antonio.ramosdet(at)gmail.com * * ------------------------------------------------------------------------- * @@ -55,8 +55,7 @@ dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(double fs_in, unsigned int char system, char signal[3]); /*! - * \brief This class implements a code DLL + carrier PLL VEML (Very Early - * Minus Late) tracking block for Galileo E1 signals + * \brief This class implements a code DLL + carrier PLL tracking block. */ class dll_pll_veml_tracking : public gr::block { @@ -67,12 +66,6 @@ public: void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); void start_tracking(); - /*! - * \brief Code DLL + carrier PLL according to the algorithms described in: - * K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, - * A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach, - * Birkhauser, 2007 - */ int general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); @@ -214,9 +207,9 @@ private: double T_prn_seconds; double T_prn_samples; double K_blk_samples; - //PRN period in samples + // PRN period in samples int d_current_prn_length_samples; - //processing samples counters + // processing samples counters unsigned long int d_sample_counter; unsigned long int d_acq_sample_stamp; @@ -234,4 +227,4 @@ private: std::ofstream d_dump_file; }; -#endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_H +#endif // GNSS_SDR_DLL_PLL_VEML_TRACKING_H