diff --git a/conf/gnss-sdr_galileo_E1_extended_correlator_byte.conf b/conf/gnss-sdr_galileo_E1_extended_correlator_byte.conf new file mode 100644 index 000000000..fb6a08fc0 --- /dev/null +++ b/conf/gnss-sdr_galileo_E1_extended_correlator_byte.conf @@ -0,0 +1,269 @@ +; Default configuration file +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second]. +GNSS-SDR.internal_fs_sps=20000000 + + +;######### SIGNAL_SOURCE CONFIG ############ +;#implementation: Use [File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental) +SignalSource.implementation=File_Signal_Source + +;#filename: path to file with the captured GNSS signal samples to be processed +;SignalSource.filename=/home/javier/signals/L125_III1b_210s_L1_2msps.bin ; <- PUT YOUR FILE HERE + +SignalSource.filename=/media/javier/SISTEMA/signals/fraunhofer/L125_III1b_210s_L1.bin + +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +SignalSource.item_type=byte + +;#sampling_frequency: Original Signal sampling frequency in [Hz] +SignalSource.sampling_frequency=20000000 + +;#freq: RF front-end center frequency in [Hz] +SignalSource.freq=1575420000 + +;#samples: Number of samples to be processed. Notice that 0 indicates the entire file. +SignalSource.samples=0 + +;#repeat: Repeat the processing file. Disable this option in this version +SignalSource.repeat=false + +;#dump: Dump the Signal source data to a file. Disable this option in this version +SignalSource.dump=false + +SignalSource.dump_filename=../data/signal_source.dat + +;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing. +; it helps to not overload the CPU, but the processing time will be longer. +SignalSource.enable_throttle_control=false + + +;######### SIGNAL_CONDITIONER CONFIG ############ +;## It holds blocks to change data type, filter and resample input data. + +;#implementation: Use [Pass_Through] or [Signal_Conditioner] +;#[Pass_Through] disables this block and the [DataTypeAdapter], [InputFilter] and [Resampler] blocks +;#[Signal_Conditioner] enables this block. Then you have to configure [DataTypeAdapter], [InputFilter] and [Resampler] blocks +SignalConditioner.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER CONFIG ############ +;## Changes the type of input data. Please disable it in this version. +;#implementation: [Pass_Through] disables this block +DataTypeAdapter.implementation=Ibyte_To_Complex + +;######### INPUT_FILTER CONFIG ############ +;## Filter the input data. Can be combined with frequency translation for IF signals + +InputFilter.implementation=Pass_Through + + +;######### RESAMPLER CONFIG ############ +;## Resamples the input data. + +;#implementation: Use [Pass_Through] or [Direct_Resampler] +;#[Pass_Through] disables this block +;#[Direct_Resampler] enables a resampler that implements a nearest neigbourhood interpolation +;Resampler.implementation=Direct_Resampler +Resampler.implementation=Pass_Through + + +;######### CHANNELS GLOBAL CONFIG ############ +;#count: Number of available GPS satellite channels. +Channels_1C.count=0 +;#count: Number of available Galileo satellite channels. +Channels_1B.count=1 +;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver +Channels.in_acquisition=1 + +;#signal: +;#if the option is disabled by default is assigned "1C" GPS L1 C/A +Channel1.signal=1B +Channel2.signal=1B +Channel3.signal=1B +Channel4.signal=1B +Channel5.signal=1B +Channel6.signal=1B +Channel7.signal=1B +Channel8.signal=1B +Channel9.signal=1B +Channel10.signal=1B +Channel11.signal=1B +Channel12.signal=1B +Channel13.signal=1B +Channel14.signal=1B +Channel15.signal=1B + + +;######### GPS ACQUISITION CONFIG ############ + +;#dump: Enable or disable the acquisition internal data file logging [true] or [false] +Acquisition_1C.dump=false +;#filename: Log path and filename +Acquisition_1C.dump_filename=./acq_dump.dat +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +Acquisition_1C.item_type=gr_complex +;#if: Signal intermediate frequency in [Hz] +Acquisition_1C.if=0 +;#sampled_ms: Signal block duration for the acquisition signal detection [ms] +Acquisition_1C.sampled_ms=1 +;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.use_CFAR_algorithm=false; +;#threshold: Acquisition threshold +Acquisition_1C.threshold=18 +;#doppler_max: Maximum expected Doppler shift [Hz] +Acquisition_1C.doppler_max=5000 +;#doppler_max: Doppler step in the grid search [Hz] +Acquisition_1C.doppler_step=500 + + +;######### GALILEO ACQUISITION CONFIG ############ + +;#dump: Enable or disable the acquisition internal data file logging [true] or [false] +Acquisition_1B.dump=false +;#filename: Log path and filename +Acquisition_1B.dump_filename=../data/acq_dump.dat +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +Acquisition_1B.item_type=gr_complex +;#if: Signal intermediate frequency in [Hz] +Acquisition_1B.if=0 +;#sampled_ms: Signal block duration for the acquisition signal detection [ms] +Acquisition_1B.sampled_ms=4 +;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.acquire_pilot=true +Acquisition_1B.use_CFAR_algorithm=false +;#threshold: Acquisition threshold +Acquisition_1B.threshold=21 +;#doppler_max: Maximum expected Doppler shift [Hz] +Acquisition_1B.doppler_max=5000 +;#doppler_max: Doppler step in the grid search [Hz] +Acquisition_1B.doppler_step=125 +Acquisition_1B.bit_transition_flag=true + +;######### TRACKING GPS CONFIG ############ + +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking +;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. +Tracking_1C.item_type=gr_complex + +;#sampling_frequency: Signal Intermediate Frequency in [Hz] +Tracking_1C.if=0 + +;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] +Tracking_1C.dump=false + +;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. +Tracking_1C.dump_filename=../data/epl_tracking_ch_ + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1C.pll_bw_hz=30.0; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1C.dll_bw_hz=2.0; + +;#order: PLL/DLL loop filter order [2] or [3] +Tracking_1C.order=3; + +;######### TRACKING GALILEO CONFIG ############ + +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +;#item_type: Type and resolution for each of the signal samples. +Tracking_1B.item_type=gr_complex + +;#sampling_frequency: Signal Intermediate Frequency in [Hz] +Tracking_1B.if=0 + +;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] +Tracking_1B.dump=true + +;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. +Tracking_1B.dump_filename=../data/veml_tracking_ch_ + +Tracking_1B.track_pilot=true + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1B.pll_bw_hz=4.0; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1B.dll_bw_hz=0.5; + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1B.pll_bw_narrow_hz=2.0; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1B.dll_bw_narrow_hz=0.25; + +Tracking_1B.extend_correlation_symbols=4; + +;#order: PLL/DLL loop filter order [2] or [3] +Tracking_1B.order=3; + +;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo +Tracking_1B.early_late_space_chips=0.15; + +;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] +Tracking_1B.very_early_late_space_chips=0.6; + +;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo +Tracking_1B.early_late_space_narrow_chips=0.06; + +;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] +Tracking_1B.very_early_late_space_narrow_chips=0.25; + + + +;######### TELEMETRY DECODER GPS CONFIG ############ +;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false +;#decimation factor +TelemetryDecoder_1C.decimation_factor=4; + +;######### TELEMETRY DECODER GALILEO CONFIG ############ +;#implementation: Use [Galileo_E1B_Telemetry_Decoder] for Galileo E1B +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables + +;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] +Observables.dump=false + +;#dump_filename: Log path and filename. +Observables.dump_filename=./observables.dat + + +;######### PVT CONFIG ############ +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT + +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad + +;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] +PVT.output_rate_ms=100; + +;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms. +PVT.display_rate_ms=500; + +;#dump: Enable or disable the PVT internal binary data file logging [true] or [false] +PVT.dump=false + +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 + +;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump. +PVT.dump_filename=./PVT diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc index f9712824c..b0f64a25b 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -70,6 +70,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions + acquire_pilot_= configuration_->property(role + ".acquire_pilot", false); //will be true in future versions max_dwells_ = configuration_->property(role + ".max_dwells", 1); @@ -252,8 +253,18 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() std::complex * code = new std::complex[code_length_]; - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, - cboc, gnss_synchro_->PRN, fs_in_, 0, false); + if (acquire_pilot_==true) + { + //set local signal generator to Galileo E1 pilot component (1C) + char pilot_signal[3]="1C"; + galileo_e1_code_gen_complex_sampled(code, pilot_signal, + cboc, gnss_synchro_->PRN, fs_in_, 0, false); + }else + { + galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + cboc, gnss_synchro_->PRN, fs_in_, 0, false); + } + for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h index da8d2e742..71727cecd 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h @@ -146,6 +146,7 @@ private: unsigned int code_length_; bool bit_transition_flag_; bool use_CFAR_algorithm_flag_; + bool acquire_pilot_; unsigned int channel_; float threshold_; unsigned int doppler_max_; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index 60ad2e82c..897532983 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -127,13 +127,9 @@ pcps_acquisition_cc::pcps_acquisition_cc( // For dumping samples into a file d_dump = dump; d_dump_filename = dump_filename; - d_gnss_synchro = 0; d_grid_doppler_wipeoffs = 0; - - d_done = false; d_blocking = blocking; - d_new_data_available = false; d_worker_active = false; d_data_buffer = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); } @@ -160,19 +156,6 @@ pcps_acquisition_cc::~pcps_acquisition_cc() { d_dump_file.close(); } - - // Let the worker thread know that we are done and then wait to join - if( d_worker_thread.joinable() ) - { - { - std::lock_guard lk( d_mutex ); - d_done = true; - d_cond.notify_one(); - } - - d_worker_thread.join(); - } - volk_gnsssdr_free( d_data_buffer ); } @@ -233,9 +216,6 @@ void pcps_acquisition_cc::init() int doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_freq + doppler); } - - d_new_data_available = false; - d_done = false; d_worker_active = false; } @@ -253,6 +233,7 @@ void pcps_acquisition_cc::set_state(int state) d_mag = 0.0; d_input_power = 0.0; d_test_statistics = 0.0; + d_active = true; } else if (d_state == 0) {} @@ -299,7 +280,7 @@ void pcps_acquisition_cc::send_negative_acquisition() } -int pcps_acquisition_cc::general_work(int noutput_items, +int pcps_acquisition_cc::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 __attribute__((unused))) { @@ -314,192 +295,157 @@ int pcps_acquisition_cc::general_work(int noutput_items, * 6. Declare positive or negative acquisition using a message port */ - switch (d_state) + gr::thread::scoped_lock lk(d_setlock); + if(!d_active || d_worker_active) + { + d_sample_counter += d_fft_size * ninput_items[0]; + consume_each(ninput_items[0]); + return 0; + } + + switch(d_state) { case 0: { - if (d_active) - { - //restart acquisition variables - d_gnss_synchro->Acq_delay_samples = 0.0; - d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; - d_well_count = 0; - d_mag = 0.0; - d_input_power = 0.0; - d_test_statistics = 0.0; - d_state = 1; - } - + //restart acquisition variables + d_gnss_synchro->Acq_delay_samples = 0.0; + d_gnss_synchro->Acq_doppler_hz = 0.0; + d_gnss_synchro->Acq_samplestamp_samples = 0; + d_well_count = 0; + d_mag = 0.0; + d_input_power = 0.0; + d_test_statistics = 0.0; + d_state = 1; d_sample_counter += d_fft_size * ninput_items[0]; // sample counter consume_each(ninput_items[0]); - break; } case 1: { - std::unique_lock lk( d_mutex ); - - int num_items_consumed = 1; - - if( d_worker_active ) + // Copy the data to the core and let it know that new data is available + memcpy(d_data_buffer, input_items[0], d_fft_size * sizeof(gr_complex)); + if(d_blocking) { - if( d_blocking ) - { - // Should never get here: - std::string msg = "pcps_acquisition_cc: Entered general work with worker active in blocking mode, should never happen"; - LOG(WARNING) << msg; - std::cout << msg << std::endl; - d_cond.wait( lk, [&]{ return !this->d_worker_active; } ); - } - else - { - num_items_consumed = ninput_items[0]; - d_sample_counter += d_fft_size * num_items_consumed; - } + lk.unlock(); + acquisition_core(d_sample_counter); } else { - // Copy the data to the core and let it know that new data is available - memcpy( d_data_buffer, input_items[0], d_fft_size * sizeof( gr_complex ) ); - d_new_data_available = true; - d_cond.notify_one(); - - if( d_blocking ) - { - d_cond.wait( lk, [&]{ return !this->d_new_data_available; } ); - } + gr::thread::thread d_worker(&pcps_acquisition_cc::acquisition_core, this, d_sample_counter); + d_worker_active = true; } - - consume_each(num_items_consumed); - + d_sample_counter += d_fft_size; + consume_each(1); break; - } // case 1, switch d_state - - } // switch d_state - - return noutput_items; + } + } + return 0; } -void pcps_acquisition_cc::acquisition_core( void ) +void pcps_acquisition_cc::acquisition_core( unsigned long int samp_count ) { - d_worker_active = false; - while( 1 ) + gr::thread::scoped_lock lk(d_setlock); + + // initialize acquisition algorithm + int doppler; + uint32_t indext = 0; + float magt = 0.0; + const gr_complex *in = d_data_buffer; //Get the input samples pointer + + int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size ); + + float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); + + d_input_power = 0.0; + d_mag = 0.0; + d_well_count++; + + DLOG(INFO) << "Channel: " << d_channel + << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + << " ,sample stamp: " << samp_count << ", threshold: " + << d_threshold << ", doppler_max: " << d_doppler_max + << ", doppler_step: " << d_doppler_step + << ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); + + lk.unlock(); + if (d_use_CFAR_algorithm_flag) { - std::unique_lock lk( d_mutex ); - d_cond.wait( lk, [&]{ return this->d_new_data_available or this->d_done; } ); - d_worker_active = !d_done; - unsigned long int sample_counter = d_sample_counter; // sample counter - lk.unlock(); + // 1- (optional) Compute the input signal power estimation + volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); + volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size); + d_input_power /= static_cast(d_fft_size); + } + // 2- Doppler frequency search loop + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) + { + // doppler search steps + doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; - if( d_done ) + volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); + + // 3- Perform the FFT-based convolution (parallel time search) + // Compute the FFT of the carrier wiped--off incoming signal + d_fft_if->execute(); + + // Multiply carrier wiped--off, Fourier transformed incoming signal + // with the local FFT'd code reference using SIMD operations with VOLK library + volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); + + // compute the inverse FFT + d_ifft->execute(); + + // Search maximum + size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 ); + volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size); + volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); + magt = d_magnitude[indext]; + + if (d_use_CFAR_algorithm_flag) { - break; + // Normalize the maximum value to correct the scale factor introduced by FFTW + magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); } - - // initialize acquisition algorithm - int doppler; - uint32_t indext = 0; - float magt = 0.0; - const gr_complex *in = d_data_buffer; //Get the input samples pointer - - int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size ); - - float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); - - d_input_power = 0.0; - d_mag = 0.0; - d_well_count++; - - DLOG(INFO) << "Channel: " << d_channel - << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN - << " ,sample stamp: " << sample_counter << ", threshold: " - << d_threshold << ", doppler_max: " << d_doppler_max - << ", doppler_step: " << d_doppler_step - << ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); - - if (d_use_CFAR_algorithm_flag == true) + // 4- record the maximum peak and the associated synchronization parameters + if (d_mag < magt) { - // 1- (optional) Compute the input signal power estimation - volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); - volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size); - d_input_power /= static_cast(d_fft_size); - } - // 2- Doppler frequency search loop - for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) - { - // doppler search steps - doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; + d_mag = magt; - volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, - d_grid_doppler_wipeoffs[doppler_index], d_fft_size); - - // 3- Perform the FFT-based convolution (parallel time search) - // Compute the FFT of the carrier wiped--off incoming signal - d_fft_if->execute(); - - // Multiply carrier wiped--off, Fourier transformed incoming signal - // with the local FFT'd code reference using SIMD operations with VOLK library - volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), - d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); - - // compute the inverse FFT - d_ifft->execute(); - - // Search maximum - size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 ); - volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size); - volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); - magt = d_magnitude[indext]; - - if (d_use_CFAR_algorithm_flag == true) + if (!d_use_CFAR_algorithm_flag) { - // Normalize the maximum value to correct the scale factor introduced by FFTW - magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); - } - // 4- record the maximum peak and the associated synchronization parameters - if (d_mag < magt) - { - d_mag = magt; - - if (d_use_CFAR_algorithm_flag == false) - { - // Search grid noise floor approximation for this doppler line - volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size); - d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1); - } - - // In case that d_bit_transition_flag = true, we compare the potentially - // new maximum test statistics (d_mag/d_input_power) with the value in - // d_test_statistics. When the second dwell is being processed, the value - // of d_mag/d_input_power could be lower than d_test_statistics (i.e, - // the maximum test statistics in the previous dwell is greater than - // current d_mag/d_input_power). Note that d_test_statistics is not - // restarted between consecutive dwells in multidwell operation. - - if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) - { - d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); - d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); - d_gnss_synchro->Acq_samplestamp_samples = sample_counter; - - // 5- Compute the test statistics and compare to the threshold - //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; - d_test_statistics = d_mag / d_input_power; - } + // Search grid noise floor approximation for this doppler line + volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size); + d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1); } - // Record results to file if required - if (d_dump) - { - std::stringstream filename; - std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write - filename.str(""); + // In case that d_bit_transition_flag = true, we compare the potentially + // new maximum test statistics (d_mag/d_input_power) with the value in + // d_test_statistics. When the second dwell is being processed, the value + // of d_mag/d_input_power could be lower than d_test_statistics (i.e, + // the maximum test statistics in the previous dwell is greater than + // current d_mag/d_input_power). Note that d_test_statistics is not + // restarted between consecutive dwells in multidwell operation. - boost::filesystem::path p = d_dump_filename; - filename << p.parent_path().string() + if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) + { + d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); + d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); + d_gnss_synchro->Acq_samplestamp_samples = samp_count; + + // 5- Compute the test statistics and compare to the threshold + //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; + d_test_statistics = d_mag / d_input_power; + } + } + // Record results to file if required + if (d_dump) + { + std::stringstream filename; + std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write + filename.str(""); + boost::filesystem::path p = d_dump_filename; + filename << p.parent_path().string() << boost::filesystem::path::preferred_separator << p.stem().string() << "_" << d_gnss_synchro->System @@ -508,15 +454,32 @@ void pcps_acquisition_cc::acquisition_core( void ) << doppler << p.extension().string(); - DLOG(INFO) << "Writing ACQ out to " << filename.str(); + DLOG(INFO) << "Writing ACQ out to " << filename.str(); - d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); - d_dump_file.write(reinterpret_cast(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? - d_dump_file.close(); - } + d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); + d_dump_file.write(reinterpret_cast(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? + d_dump_file.close(); } - - if (!d_bit_transition_flag) + } + lk.lock(); + if (!d_bit_transition_flag) + { + if (d_test_statistics > d_threshold) + { + d_state = 0; // Positive acquisition + d_active = false; + send_positive_acquisition(); + } + else if (d_well_count == d_max_dwells) + { + d_state = 0; + d_active = false; + send_negative_acquisition(); + } + } + else + { + if (d_well_count == d_max_dwells) // d_max_dwells = 2 { if (d_test_statistics > d_threshold) { @@ -524,66 +487,13 @@ void pcps_acquisition_cc::acquisition_core( void ) d_active = false; send_positive_acquisition(); } - else if (d_well_count == d_max_dwells) + else { - d_state = 0; + d_state = 0; // Negative acquisition d_active = false; send_negative_acquisition(); } } - else - { - if (d_well_count == d_max_dwells) // d_max_dwells = 2 - { - if (d_test_statistics > d_threshold) - { - d_state = 0; // Positive acquisition - d_active = false; - send_positive_acquisition(); - } - else - { - d_state = 0; // Negative acquisition - d_active = false; - send_negative_acquisition(); - } - } - } - - lk.lock(); - d_worker_active = false; - d_new_data_available = false; - lk.unlock(); - d_cond.notify_one(); } -} - - -bool pcps_acquisition_cc::start( void ) -{ d_worker_active = false; - d_done = false; - - // Start the worker thread and wait for it to acknowledge: - d_worker_thread = std::move( std::thread( &pcps_acquisition_cc::acquisition_core, this ) ); - - return gr::block::start(); } - - -bool pcps_acquisition_cc::stop( void ) -{ - // Let the worker thread know that we are done and then wait to join - if( d_worker_thread.joinable() ) - { - { - std::lock_guard lk( d_mutex ); - d_done = true; - d_cond.notify_one(); - } - - d_worker_thread.join(); - } - return gr::block::stop(); -} - diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index e33746f2b..17fa90689 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -21,6 +21,7 @@ *
  • Luis Esteve, 2012. luis(at)epsilon-formacion.com *
  • Marc Molina, 2013. marc.molina.pena@gmail.com *
  • Cillian O'Driscoll, 2017. cillian(at)ieee.org + *
  • Antonio Ramos, 2017. antonio.ramos@cttc.es * * * ------------------------------------------------------------------------- @@ -53,9 +54,6 @@ #include #include -#include -#include -#include #include #include #include @@ -100,7 +98,7 @@ private: void update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq); - void acquisition_core( void ); + void acquisition_core( unsigned long int samp_count ); void send_negative_acquisition(); void send_positive_acquisition(); @@ -110,7 +108,6 @@ private: int d_samples_per_code; //unsigned int d_doppler_resolution; float d_threshold; - std::string d_satellite_str; unsigned int d_doppler_max; unsigned int d_doppler_step; unsigned int d_sampled_ms; @@ -138,16 +135,8 @@ private: bool d_dump; unsigned int d_channel; std::string d_dump_filename; - - std::thread d_worker_thread; - std::mutex d_mutex; - - std::condition_variable d_cond; - bool d_done; - bool d_new_data_available; bool d_worker_active; bool d_blocking; - gr_complex *d_data_buffer; public: @@ -252,15 +241,6 @@ public: gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - /*! - * Called by the flowgraph when processing is about to start. - */ - bool start( void ); - - /*! - * Called by the flowgraph when processing is done. - */ - bool stop( void ); }; #endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/ 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 c3a37b548..b47e26a68 100755 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -57,19 +57,31 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( std::string item_type; std::string default_item_type = "gr_complex"; float pll_bw_hz; + float pll_bw_narrow_hz; float dll_bw_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; 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", 50.0); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); + dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); + pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + int extend_correlation_symbols; + extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); + early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.6); + + bool track_pilot=configuration->property(role + ".track_pilot", false); std::string default_dump_filename = "./track_ch"; dump_filename = configuration->property(role + ".dump_filename", @@ -88,8 +100,24 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( 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); + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot); +// tracking_ = galileo_e1_dll_pll_veml_make_tracking_cc( +// f_if, +// fs_in, +// vector_length, +// dump, +// dump_filename, +// pll_bw_hz, +// dll_bw_hz, +// early_late_space_chips, +// very_early_late_space_chips); } else { diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 9250de398..a21865500 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -72,21 +72,40 @@ galileo_e1_dll_pll_veml_make_tracking_cc( 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 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) { return galileo_e1_dll_pll_veml_tracking_cc_sptr(new galileo_e1_dll_pll_veml_tracking_cc(if_freq, - fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips, very_early_late_space_chips)); + 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)); } void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items, - gr_vector_int &ninput_items_required) + gr_vector_int &ninput_items_required) { if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } } @@ -98,10 +117,16 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( 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): - gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + 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): + gr::block("galileo_e1_dll_pll_veml_tracking_cc", 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")); @@ -121,24 +146,31 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( // Initialize tracking ========================================== // Set bandwidth of code and carrier loop filters - d_code_loop_filter.set_DLL_BW(dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); + d_dll_bw_hz=dll_bw_hz; + d_pll_bw_hz=pll_bw_hz; + d_dll_bw_narrow_hz=dll_bw_narrow_hz; + d_pll_bw_narrow_hz=pll_bw_narrow_hz; + + d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); // Correlator spacing d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) d_very_early_late_spc_chips = very_early_late_space_chips; // Define very-early-late offset (in chips) + d_early_late_spc_narrow_chips = early_late_space_narrow_chips; // Define narrow early-late offset (in chips) + d_very_early_late_spc_narrow_chips = very_early_late_space_narrow_chips; // Define narrow very-early-late offset (in chips) // Initialization of local code replica // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip - d_ca_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); + d_tracking_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_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); - } + { + 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]; @@ -155,9 +187,32 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( 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 * Galileo_E1_B_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(Galileo_E1_CODE_CHIP_RATE_HZ); @@ -171,9 +226,6 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( //d_sample_counter_seconds = 0; d_acq_sample_stamp = 0; - d_enable_tracking = false; - d_pull_in = false; - d_current_prn_length_samples = static_cast(d_vector_length); // CN0 estimation and lock detector buffers @@ -185,11 +237,8 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; systemName["E"] = std::string("Galileo"); - *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); + + clear_tracking_vars(); d_acquisition_gnss_synchro = 0; d_channel = 0; @@ -197,106 +246,391 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_acq_carrier_doppler_hz = 0.0; d_carrier_doppler_hz = 0.0; d_acc_carrier_phase_rad = 0.0; - d_acc_code_phase_secs = 0.0; + + d_state=0;// intial state: stanby } void galileo_e1_dll_pll_veml_tracking_cc::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 - // generate local reference ALWAYS starting at chip 1 (2 samples per chip) - galileo_e1_code_gen_float_sampled(d_ca_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - 2 * Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - multicorrelator_cpu.set_local_code_and_taps(static_cast(2 * Galileo_E1_B_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); + 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_correlator_outs[n] = gr_complex(0,0); + } d_carrier_lock_fail_counter = 0; - d_rem_code_phase_samples = 0.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_acc_code_phase_secs = 0.0; - d_carrier_doppler_hz = d_acq_carrier_doppler_hz; - d_current_prn_length_samples = d_vector_length; + d_code_phase_samples = d_acq_code_phase_samples; std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0, 1); + sys = sys_.substr(0,1); // 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 - d_pull_in = true; - d_enable_tracking = true; + // enable tracking pull-in + d_state=1; LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz - << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; + + } galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() { if (d_dump_file.is_open()) + { + try { - try - { - d_dump_file.close(); - } - catch(const std::exception & ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } + d_dump_file.close(); } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } + } if(d_dump) + { + if(d_channel == 0) { - if(d_channel == 0) - { - std::cout << "Writing .mat files ..."; - } - galileo_e1_dll_pll_veml_tracking_cc::save_matfile(); - if(d_channel == 0) - { - std::cout << " done." << std::endl; - } + std::cout << "Writing .mat files ..."; } + galileo_e1_dll_pll_veml_tracking_cc::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_ca_code); - delete[] d_Prompt_buffer; - multicorrelator_cpu.free(); + 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(); + LOG(WARNING) << "Exception in destructor " << ex.what(); } } - -int galileo_e1_dll_pll_veml_tracking_cc::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) +bool galileo_e1_dll_pll_veml_tracking_cc::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 galileo_e1_dll_pll_veml_tracking_cc::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 galileo_e1_dll_pll_veml_tracking_cc::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 galileo_e1_dll_pll_veml_tracking_cc::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 galileo_e1_dll_pll_veml_tracking_cc::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 galileo_e1_dll_pll_veml_tracking_cc::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 galileo_e1_dll_pll_veml_tracking_cc::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) { - double carr_error_hz = 0.0; - double carr_error_filt_hz = 0.0; - double code_error_chips = 0.0; - double code_error_filt_chips = 0.0; // Block input data and block output stream pointers const gr_complex* in = reinterpret_cast(input_items[0]); @@ -304,123 +638,208 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro current_synchro_data = Gnss_Synchro(); - if (d_enable_tracking == true) + 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; - if (d_pull_in == true) + //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 samples + + // ########### Output the tracking results to Telemetry block ########## + if (d_track_pilot) { - /* - * Signal alignment (skip samples until the incoming signal is aligned with local replica) - */ - 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 + samples_offset; - 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 - d_pull_in = false; - consume_each(samples_offset); //shift input to perform alignment with local replica - return 1; + 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; - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in); + //enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); - double carr_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - double code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(d_fs_in)); - double rem_code_phase_half_chips = d_rem_code_phase_samples * (2.0*d_code_freq_chips / d_fs_in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, - carr_phase_step_rad, - rem_code_phase_half_chips, - code_phase_step_half_chips, - d_correlation_length_samples); + //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)<(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 ########################################################## - // PLL discriminator - carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GALILEO_TWO_PI; - // Carrier discriminator filter - carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + 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); //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 ########################################################## - // DLL discriminator - code_error_chips = dll_nc_vemlp_normalized(*d_Very_Early, *d_Early, *d_Late, *d_Very_Late); //[chips/Ti] - // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] - //Code phase accumulator - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - double T_chip_seconds; - double T_prn_seconds; - double T_prn_samples; - double K_blk_samples; // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1.0 / d_code_freq_chips; - T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + 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 - // ####### 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_Prompt; - d_cn0_estimation_counter++; - } - 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; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - // ########### Output the tracking results to Telemetry block ########## - - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) + 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 @@ -431,94 +850,113 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri 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 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 + } } - else - { - *d_Early = gr_complex(0,0); - *d_Prompt = gr_complex(0,0); - *d_Late = gr_complex(0,0); - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - current_synchro_data.Tracking_sample_counter = d_sample_counter; } + //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.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; - 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 = (*d_Prompt).real(); - prompt_Q = (*d_Prompt).imag(); - tmp_VE = std::abs(*d_Very_Early); - tmp_E = std::abs(*d_Early); - tmp_P = std::abs(*d_Prompt); - tmp_L = std::abs(*d_Late); - tmp_VL = std::abs(*d_Very_Late); - - 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 = carr_error_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = carr_error_filt_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - //DLL commands - tmp_float = code_error_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = 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(); - } - } 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 - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + if (current_synchro_data.Flag_valid_symbol_output) + { + return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + }else{ + return 0; + } } @@ -534,25 +972,25 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try { - dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + 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; + 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); - } + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } else - { - return 1; - } + { + return 1; + } float * abs_VE = new float [num_epoch]; float * abs_E = new float [num_epoch]; float * abs_P = new float [num_epoch]; @@ -576,58 +1014,58 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() 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(); + 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; + 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 @@ -638,88 +1076,88 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() 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); + { + 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_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_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_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("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_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("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("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("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("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("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_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("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_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("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("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("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("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("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); - } + 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; @@ -751,23 +1189,23 @@ void galileo_e1_dll_pll_veml_tracking_cc::set_channel(unsigned int channel) LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) + { + if (d_dump_file.is_open() == false) { - 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(); - } - } + 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(); + } } + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h index 2d1aef220..f02c51b8e 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h @@ -53,8 +53,14 @@ galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, 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 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); /*! * \brief This class implements a code DLL + carrier PLL VEML (Very Early @@ -88,8 +94,14 @@ private: 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 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); galileo_e1_dll_pll_veml_tracking_cc(long if_freq, long fs_in, unsigned @@ -98,12 +110,25 @@ private: 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 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); + 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; @@ -114,16 +139,29 @@ private: long d_if_freq; long d_fs_in; + //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; - float* d_ca_code; + 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; @@ -131,6 +169,22 @@ private: 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; @@ -143,11 +197,24 @@ private: 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_acc_code_phase_secs; + double d_rem_code_phase_chips; + double d_code_phase_samples; //PRN period in samples int d_current_prn_length_samples; @@ -158,16 +225,13 @@ private: // 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; - // control vars - bool d_enable_tracking; - bool d_pull_in; - // file dump std::string d_dump_filename; std::ofstream d_dump_file; diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index a95afd1ca..be5704a6e 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -57,7 +57,7 @@ const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-car const double Galileo_E1_SUB_CARRIER_B_RATE_HZ = 6.138e6; //!< Galileo E1 sub-carrier 'b' rate [Hz] const double Galileo_E1_B_CODE_LENGTH_CHIPS = 4092.0; //!< Galileo E1-B code length [chips] const double Galileo_E1_B_SYMBOL_RATE_BPS = 250.0; //!< Galileo E1-B symbol rate [bits/second] -const double Galileo_E1_C_SECONDARY_CODE_LENGTH = 25.0; //!< Galileo E1-C secondary code length [chips] +const int Galileo_E1_C_SECONDARY_CODE_LENGTH = 25; //!< Galileo E1-C secondary code length [chips] const int Galileo_E1_NUMBER_OF_CODES = 50; const double GALILEO_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)