From 6b67037fed1738674e67a09b05c9c93674bb6555 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 9 Jul 2018 17:56:47 +0200 Subject: [PATCH] Add first-to-second peak statistic --- .../galileo_e1_pcps_ambiguous_acquisition.cc | 1 + .../adapters/galileo_e5a_pcps_acquisition.cc | 1 + .../glonass_l1_ca_pcps_acquisition.cc | 1 + .../glonass_l2_ca_pcps_acquisition.cc | 1 + .../adapters/gps_l1_ca_pcps_acquisition.cc | 1 + ...gps_l1_ca_pcps_acquisition_fine_doppler.cc | 1 + .../gps_l1_ca_pcps_acquisition_fpga.cc | 1 + .../adapters/gps_l2_m_pcps_acquisition.cc | 1 + .../adapters/gps_l5i_pcps_acquisition.cc | 1 + .../gnuradio_blocks/pcps_acquisition.cc | 115 +++++++++++------- .../gnuradio_blocks/pcps_acquisition.h | 3 + src/algorithms/acquisition/libs/acq_conf.cc | 1 + src/algorithms/acquisition/libs/acq_conf.h | 1 + 13 files changed, 85 insertions(+), 44 deletions(-) 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 f377a72c8..a98207678 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -58,6 +58,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil((1.0 / Galileo_E1_CODE_CHIP_RATE_HZ) * static_cast(acq_parameters.fs_in))); dump_ = configuration_->property(role + ".dump", false); acq_parameters.dump = dump_; acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0); diff --git a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc index 85aaee1d3..9ff35a922 100644 --- a/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc @@ -57,6 +57,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 32000000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil((1.0 / Galileo_E5a_CODE_CHIP_RATE_HZ) * static_cast(acq_parameters.fs_in))); acq_pilot_ = configuration_->property(role + ".acquire_pilot", false); acq_iq_ = configuration_->property(role + ".acquire_iq", false); if (acq_iq_) diff --git a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc index b654881d7..43354e627 100644 --- a/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc @@ -59,6 +59,7 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition( long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil(GLONASS_L1_CA_CHIP_PERIOD * static_cast(acq_parameters.fs_in))); dump_ = configuration_->property(role + ".dump", false); acq_parameters.dump = dump_; acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0); diff --git a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc index 1f5d251fb..374b69e35 100644 --- a/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc @@ -58,6 +58,7 @@ GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition( long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil(GLONASS_L2_CA_CHIP_PERIOD * static_cast(acq_parameters.fs_in))); dump_ = configuration_->property(role + ".dump", false); acq_parameters.dump = dump_; acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0); diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc index f5602b1b6..18c728412 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc @@ -60,6 +60,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition( long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil(GPS_L1_CA_CHIP_PERIOD * static_cast(acq_parameters.fs_in))); dump_ = configuration_->property(role + ".dump", false); acq_parameters.dump = dump_; acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0); diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc index 44dcfc0ee..68d67dac8 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc @@ -56,6 +56,7 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler( long fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil(GPS_L1_CA_CHIP_PERIOD * static_cast(acq_parameters.fs_in))); dump_ = configuration->property(role + ".dump", false); acq_parameters.dump = dump_; dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc index 8480441f7..a8e2a3f17 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc @@ -60,6 +60,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga( long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); long fs_in = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in; + acq_parameters.samples_per_chip = static_cast(ceil(GPS_L1_CA_CHIP_PERIOD * static_cast(acq_parameters.fs_in))); doppler_max_ = configuration_->property(role + ".doppler_max", 5000); if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max; acq_parameters.doppler_max = doppler_max_; diff --git a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc index 2fbb72252..e68de5230 100644 --- a/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc @@ -60,6 +60,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition( long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil((1.0 / GPS_L2_M_CODE_RATE_HZ) * static_cast(acq_parameters.fs_in))); dump_ = configuration_->property(role + ".dump", false); acq_parameters.dump = dump_; acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0); diff --git a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc index da30699fa..82d9d0720 100644 --- a/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc +++ b/src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc @@ -59,6 +59,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition( long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); acq_parameters.fs_in = fs_in_; + acq_parameters.samples_per_chip = static_cast(ceil((1.0 / GPS_L5i_CODE_RATE_HZ) * static_cast(acq_parameters.fs_in))); dump_ = configuration_->property(role + ".dump", false); acq_parameters.dump = dump_; acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0); diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index c77b48ef1..276a72db7 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -127,6 +127,7 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu d_step_two = false; d_dump_number = 0; d_dump_channel = acq_parameters.dump_channel; + samplesPerChip = acq_parameters.samples_per_chip; } @@ -426,14 +427,72 @@ void pcps_acquisition::dump_results(int effective_fft_size) } } +float pcps_acquisition::first_vs_second_peak_statistics(uint32_t& indext, int& doppler) +{ + float firstPeak = 0.0; + int index_doppler = 0; + uint32_t tmp_intex_t = 0; + uint32_t index_time = 0; + + // Look for correlation peaks in the results ============================== + // Find the highest peak and compare it to the second highest peak + // The second peak is chosen not closer than 1 chip to the highest peak + //--- Find the correlation peak and the carrier frequency -------------- + for (int i = 0; i < d_num_doppler_bins; i++) + { + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i], d_fft_size); + if (d_magnitude_grid[i][tmp_intex_t] > firstPeak) + { + firstPeak = d_magnitude_grid[i][tmp_intex_t]; + index_doppler = i; + index_time = tmp_intex_t; + } + } + indext = index_time; + doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * index_doppler; + + // -- - Find 1 chip wide code phase exclude range around the peak + //uint32_t samplesPerChip = ceil(GPS_L1_CA_CHIP_PERIOD * static_cast(this->d_fs_in)); + int32_t excludeRangeIndex1 = index_time - samplesPerChip; + int32_t excludeRangeIndex2 = index_time + samplesPerChip; + + // -- - Correct code phase exclude range if the range includes array boundaries + if (excludeRangeIndex1 < 0) + { + excludeRangeIndex1 = d_fft_size + excludeRangeIndex1; + } + else if (excludeRangeIndex2 >= static_cast(d_fft_size)) + { + excludeRangeIndex2 = excludeRangeIndex2 - d_fft_size; + } + + int32_t idx = excludeRangeIndex1; + memcpy(d_tmp_buffer, d_magnitude_grid[index_doppler], d_fft_size); + do + { + d_tmp_buffer[idx] = 0.0; + idx++; + if (idx == static_cast(d_fft_size)) idx = 0; + } + while (idx != excludeRangeIndex2); + + //--- Find the second highest correlation peak in the same freq. bin --- + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_tmp_buffer, d_fft_size); + float secondPeak = d_tmp_buffer[tmp_intex_t]; + + // 5- Compute the test statistics and compare to the threshold + return firstPeak / secondPeak; +} + void pcps_acquisition::acquisition_core(unsigned long int samp_count) { gr::thread::scoped_lock lk(d_setlock); // initialize acquisition algorithm - uint32_t indext = 0; float magt = 0.0; + int doppler = 0; + uint32_t indext = 0; const gr_complex* in = d_data_buffer; // Get the input samples pointer int effective_fft_size = (acq_parameters.bit_transition_flag ? d_fft_size / 2 : d_fft_size); if (d_cshort) @@ -466,9 +525,7 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) { for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) { - // doppler search steps - int doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * doppler_index; - + // Remove doppler 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) @@ -482,7 +539,7 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) // compute the inverse FFT d_ifft->execute(); - // Search maximum + // compute squared magnitude (and accumulate in case of non-coherent integration) size_t offset = (acq_parameters.bit_transition_flag ? effective_fft_size : 0); if (d_num_noncoherent_integrations_counter == 1) { @@ -493,51 +550,21 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) volk_32fc_magnitude_squared_32f(d_tmp_buffer, d_ifft->get_outbuf() + offset, effective_fft_size); volk_32f_x2_add_32f(d_magnitude_grid[doppler_index], d_magnitude_grid[doppler_index], d_tmp_buffer, effective_fft_size); } - volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude_grid[doppler_index], effective_fft_size); - magt = d_magnitude_grid[doppler_index][indext]; - - if (acq_parameters.use_CFAR_algorithm_flag) - { - // Normalize the maximum value to correct the scale factor introduced by FFTW - magt = magt / (fft_normalization_factor * fft_normalization_factor); - } - // 4- record the maximum peak and the associated synchronization parameters - if (d_mag < magt) - { - d_mag = magt; - - if (!acq_parameters.use_CFAR_algorithm_flag) - { - // Search grid noise floor approximation for this doppler line - volk_32f_accumulator_s32f(&d_input_power, d_magnitude_grid[doppler_index], effective_fft_size); - d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1); - } - - // In case that acq_parameters.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) or !acq_parameters.bit_transition_flag) - { - d_gnss_synchro->Acq_delay_samples = static_cast(indext % acq_parameters.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 (acq_parameters.dump and d_channel == d_dump_channel) { memcpy(grid_.colptr(doppler_index), d_magnitude_grid[doppler_index], sizeof(float) * effective_fft_size); } } + // 5- Compute the test statistics and compare to the threshold + float computed_statistic = first_vs_second_peak_statistics(indext, doppler); + if (d_test_statistics < computed_statistic or !acq_parameters.bit_transition_flag) + { + d_gnss_synchro->Acq_delay_samples = static_cast(indext % acq_parameters.samples_per_code); + d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); + d_gnss_synchro->Acq_samplestamp_samples = samp_count; + d_test_statistics = computed_statistic; + } } else { diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h index d26c98d5e..5d648ebf3 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h @@ -95,6 +95,8 @@ private: void dump_results(int effective_fft_size); + float first_vs_second_peak_statistics(uint32_t& indext, int& doppler); + Acq_Conf acq_parameters; bool d_active; bool d_worker_active; @@ -108,6 +110,7 @@ private: float* d_magnitude; float** d_magnitude_grid; float* d_tmp_buffer; + uint32_t samplesPerChip; long d_old_freq; int d_state; unsigned int d_channel; diff --git a/src/algorithms/acquisition/libs/acq_conf.cc b/src/algorithms/acquisition/libs/acq_conf.cc index ed79db2fa..c3439b9c0 100644 --- a/src/algorithms/acquisition/libs/acq_conf.cc +++ b/src/algorithms/acquisition/libs/acq_conf.cc @@ -36,6 +36,7 @@ Acq_Conf::Acq_Conf() /* PCPS acquisition configuration */ sampled_ms = 0; max_dwells = 0; + samples_per_chip = 0; doppler_max = 0; num_doppler_bins_step2 = 0; doppler_step2 = 0.0; diff --git a/src/algorithms/acquisition/libs/acq_conf.h b/src/algorithms/acquisition/libs/acq_conf.h index 4707aeba7..afd43b1fb 100644 --- a/src/algorithms/acquisition/libs/acq_conf.h +++ b/src/algorithms/acquisition/libs/acq_conf.h @@ -40,6 +40,7 @@ class Acq_Conf public: /* PCPS Acquisition configuration */ unsigned int sampled_ms; + unsigned int samples_per_chip; unsigned int max_dwells; unsigned int doppler_max; unsigned int num_doppler_bins_step2;